00001 #include "FeedbackSensor.h" 00002 #include <avr/interrupt.h> 00003 #include <avr/signal.h> 00004 00005 00007 volatile int feedback_value0, feedback_value1; 00008 00009 00011 void FeedbackSensor::init() { 00012 00013 00014 // set data direction to input 00015 DDRD &= ~((1<<PD2) | (1<<PD6)); // for second sensor | | (1<<PD3) | (1<<PD7)); 00016 00017 // enable pullups 00018 PORTD |= (1<<PD2) | (1<<PD6); // for second sensor | (1<<PD3) | (1<<PD7); 00019 00020 // consider falling and rising edges 00021 MCUCR |= (1<<ISC10) | (1<<ISC00); 00022 00023 // consider only falling edges 00024 //MCUCR |= (1<<ISC01) | (1<<ISC11);// | (1<<ISC01) | (1<<ISC11); 00025 00026 // disable sensor 00027 this->disableSensor(); 00028 00029 // initialize feedback values 00030 feedback_inv0 = feedback_inv1 = false; 00031 feedback_value0 = feedback_value1 = 0; 00032 } 00033 00035 void FeedbackSensor::enableSensor() { 00036 00037 // reset counters 00038 feedback_value0 = feedback_value1 = 0; 00039 00040 // set interrupt register flags 00041 GICR |= (1<<INT0); // for second sensor | (1<<INT1); 00042 GIFR |= (1<<INTF0); // for second sensor | (1<<INTF1); 00043 sei(); 00044 00045 } 00046 00048 void FeedbackSensor::disableSensor() { 00049 00050 00051 cli(); 00052 00053 // set interrupt register flags 00054 GICR &= ~((1<<INT0)); // for second sensor | (1<<INT1)); 00055 GIFR &= ~((1<<INTF0)); // for second sensor | (1<<INTF1)); 00056 00057 } 00058 00060 char FeedbackSensor::getFeedback() { 00061 00062 int _fb = 0; 00063 00064 00065 // add sensor0 value 00066 if (!feedback_inv0) { 00067 _fb += feedback_value0; 00068 } else { 00069 _fb -= feedback_value0; 00070 } 00071 00072 // add sensor1 value 00073 if (!feedback_inv1) { 00074 00075 if (((int)_fb + (int)feedback_value1) <= 127) { 00076 _fb += feedback_value1; 00077 } else { 00078 _fb = 127; 00079 } 00080 00081 } else { 00082 if (((int)_fb + (int)feedback_value1) > -127) { 00083 _fb -= feedback_value1; 00084 } else { 00085 _fb = -127; 00086 } 00087 } 00088 00089 // reset counters 00090 feedback_value0 = feedback_value1 = 0; 00091 00092 // suppress noise 00093 if (abs(_fb) >= FEEDBACK_THRESHOLD) return _fb; 00094 else return(0); 00095 } 00096 00098 void FeedbackSensor::invertSensor(unsigned char _sensorId, bool _state) { 00099 00100 if (_sensorId == FEEDBACK_SENSOR_0) { 00101 feedback_inv0 = _state; 00102 } else if (_sensorId == FEEDBACK_SENSOR_1) { 00103 feedback_inv1 = _state; 00104 } 00105 } 00106 00108 SIGNAL(SIG_INTERRUPT0) { 00109 00110 cli(); 00111 00112 register unsigned char pd = PIND; 00113 00114 if (( (pd & (1<<PIND6))>=1 && (pd & (1<<PIND2))==0 ) || 00115 ( (pd & (1<<PIND6))==0 && (pd & (1<<PIND2))>=1 ) 00116 ) { 00117 if (feedback_value0 < 127) { 00118 feedback_value0++; 00119 } 00120 00121 } else { 00122 if (feedback_value0 > -127) { 00123 feedback_value0--; 00124 } 00125 } 00126 00127 sei(); 00128 } 00129 00130 00131 00132 // Disabled on PCBv2 00134 // SIGNAL(SIG_INTERRUPT1) { 00135 // 00136 // cli(); 00137 // 00138 // register unsigned char pd = PIND; 00139 // 00140 // if (( (pd & (1<<PIND7))>=1 && (pd & (1<<PIND3))==0 ) || 00141 // ( (pd & (1<<PIND7))==0 && (pd & (1<<PIND3))>=1 ) 00142 // ) { 00143 // if (feedback_value1 < 127) { 00144 // feedback_value1++; 00145 // } 00146 // 00147 // } else { 00148 // if (feedback_value1 > -127) { 00149 // feedback_value1--; 00150 // } 00151 // } 00152 // 00153 // sei(); 00154 // } 00155 00156 00157 00158 00159 00160 00161