00001 00002 #include "FeedbackSensor.h" 00003 #include <avr/interrupt.h> 00004 #include <avr/signal.h> 00005 00006 00008 //volatile int feedback_value0, feedback_value1; 00009 extern int wheel1, wheel2; 00010 00011 00013 FeedbackSensor::FeedbackSensor() { 00014 00015 00016 // set data direction to input 00017 DDRD &= ~((1<<PD2) | (1<<PD6)); // for second sensor | | (1<<PD3) | (1<<PD7)); 00018 00019 // enable pullups 00020 PORTD |= (1<<PD2) | (1<<PD6); // for second sensor | (1<<PD3) | (1<<PD7); 00021 00022 // consider falling and rising edges 00023 MCUCR |= (1<<ISC10) | (1<<ISC00); 00024 00025 // consider only falling edges 00026 //MCUCR |= (1<<ISC01) | (1<<ISC11);// | (1<<ISC01) | (1<<ISC11); 00027 00028 // disable sensor 00029 this->disableSensor(); 00030 00031 // initialize feedback values 00032 feedback_inv1 = feedback_inv2 = false; 00033 wheel1 = wheel2 = 0; 00034 } 00035 00037 void FeedbackSensor::enableSensor() { 00038 00039 // reset counters 00040 wheel1 = wheel2 = 0; 00041 00042 // set interrupt register flags 00043 GICR |= (1<<INT0); // for second sensor | (1<<INT1); 00044 GIFR |= (1<<INTF0); // for second sensor | (1<<INTF1); 00045 sei(); 00046 } 00047 00049 void FeedbackSensor::disableSensor() { 00050 00051 00052 cli(); 00053 00054 // set interrupt register flags 00055 GICR &= ~((1<<INT0)); // for second sensor | (1<<INT1)); 00056 GIFR &= ~((1<<INTF0)); // for second sensor | (1<<INTF1)); 00057 00058 } 00059 00061 int FeedbackSensor::getFeedback() { 00062 00063 int _fb = 0; 00064 00065 00066 // add sensor0 value 00067 if (!feedback_inv1) { 00068 _fb += wheel1; 00069 } else { 00070 _fb -= wheel1; 00071 } 00072 00073 // add sensor1 value 00074 if (!feedback_inv2) { 00075 00076 if (((int)_fb + (int)wheel2) <= 127) { 00077 _fb += wheel2; 00078 } else { 00079 _fb = 127; 00080 } 00081 00082 } else { 00083 if (((int)_fb + (int)wheel2) > -127) { 00084 _fb -= wheel2; 00085 } else { 00086 _fb = -127; 00087 } 00088 } 00089 00090 // reset counters 00091 wheel1 = wheel2 = 0; 00092 00093 00094 // check for reward-signal invertion 00095 if (BoardController::getSwitchState(SWITCH_INV_FB_0)==0) { 00096 _fb *= -1; 00097 } 00098 00099 // suppress noise 00100 if (abs(_fb) >= FEEDBACK_THRESHOLD) return _fb; 00101 else return(0); 00102 } 00103 00104 00105 00106 // Disabled on PCBv2 00108 // SIGNAL(SIG_INTERRUPT1) { 00109 // 00110 // cli(); 00111 // 00112 // register unsigned char pd = PIND; 00113 // 00114 // if (( (pd & (1<<PIND7))>=1 && (pd & (1<<PIND3))==0 ) || 00115 // ( (pd & (1<<PIND7))==0 && (pd & (1<<PIND3))>=1 ) 00116 // ) { 00117 // if (feedback_value1 < 127) { 00118 // feedback_value1++; 00119 // } 00120 // 00121 // } else { 00122 // if (feedback_value1 > -127) { 00123 // feedback_value1--; 00124 // } 00125 // } 00126 // 00127 // sei(); 00128 // } 00129 00130 00131 00132 00133 00134 00135