FeedbackSensor.c

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

Generated on Wed Nov 25 11:27:41 2009 for Crawling Robot Microcontroller Software by  doxygen 1.5.5