FeedbackSensor.c

Go to the documentation of this file.
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 

Generated on Wed Mar 25 12:58:40 2009 for Crawling Robot Microcontroller Software by  doxygen 1.5.5