Controller.c

Go to the documentation of this file.
00001 #include "global.h"
00002 #include "Controller.h"
00003 
00004 // clock configuration
00005 #define F_CPU  11059200UL
00006 
00008 void Controller::init() {
00009 
00010      mainMode = MAIN_MODE_STOP;
00011      newMainMode = MAIN_MODE_WALK_POLICY;
00012      states.init();
00013 
00014      // assign PWM-Timer to servos
00015      servo_x.setTimer(TIMER1B);
00016      servo_y.setTimer(TIMER1A);
00017      
00018      // initialize LED pins as outputs (PA0-3) and switch-pins (PA4-7) as inputs
00019      DDRA = (1<<PA0) | (1<<PA1) | (1<<PA2) | (1<<PA3); // LED1..4
00020      // enable internal pull-ups for switch-pins (PA4-7)
00021      PORTA |= (1<<PA4) | (1<<PA5) | (1<<PA6) | (1<<PA7);
00022      
00023      // initialize switch-pins (PB0, PB1, PB2, PB4) as inputs
00024      DDRB = 0x00;
00025      // enable internal pull-ups for switch-pins
00026      PORTB |= (1<<PB0) | (1<<PB1) | (1<<PB2) | (1<<PB4);
00027      
00028      // turn on all leds
00029      initLEDs();
00030      
00031      // enable auto-start
00032      autoStart = true;
00033 
00034      // set initial-state 
00035      currentX = 0x00;
00036      currentY = 0x00;
00037 
00038      // set default policy 
00039      setDefaultPolicy();
00040 }
00041 
00043 unsigned char Controller::getSwitchState (unsigned char _switch) {
00044 
00045      bool status;
00046 
00047      if (_switch == SWITCH_WALK) {
00048           //status = (PINB & (1<<PB0)); // PCBv1
00049           status = (PINB & (1<<PB4)); // PCBv2
00050           
00051      } else if (_switch == SWITCH_RANDOM_STATE) {
00052           //status = (PINB & (1<<PB1)); // PCBv1
00053           status = (PINB & (1<<PB2)); // PCBv2
00054           
00055      } else if (_switch == SWITCH_INV_FB_0) {
00056           //status = (PINB & (1<<PB2)); // PCBv1
00057           status = (PINB & (1<<PB1)); // PCBv2
00058      
00059      } else if (_switch == SWITCH_ENABLE_EXPLORATION) {
00060           //status = (PINA & (1<<PA7)); // PCBv1
00061           status = (PINB & (1<<PB0)); // PCBv2
00062           
00063      } else if (_switch == SWITCH_FULL_EXP_AT_START) {
00064           //status = (PINA & (1<<PA6)); // PCBv1
00065           status = (PINA & (1<<PA4)); // PCBv2
00066           
00067      } else if (_switch == SWITCH_V24) {
00068           // status = (PINA & (1<<PA5)); // PCBv1
00069           status = (PINA & (1<<PA5)); // PCBv2
00070                               
00071      } else if (_switch == SWITCH_VDBE_EGREEDY) {
00072           //status = (PINA & (1<<PA4)); // PCBv1
00073           status = (PINA & (1<<PA6)); // PCBv2
00074 
00075      } else if (_switch == SWITCH_RAM) {
00076           //status = SWITCH_RAM_VALUE; // PCBv1
00077           status = SWITCH_RAM_VALUE; // PCBv2
00078 
00079      } else if (_switch == SWITCH_GAMMA) {
00080           status = (PINA & (1<<PA7)); // PCBv2
00081 
00082      // Bei unbekanntem Kommando - wird geblinkt
00083      } else {
00084           
00085           register char i;
00086           
00087           for (i=0; i<100; i++) {
00088           
00089                this->powerOnLED(LED_POWER, 1);
00090                this->powerOnLED(LED_WALK, 0);
00091                this->powerOnLED(LED_LEARN, 0);
00092                this->powerOnLED(LED_EXPLORATION, 1);
00093                
00094                this->msDelay(250);
00095                
00096                this->powerOnLED(LED_POWER, 0);
00097                this->powerOnLED(LED_WALK, 1);
00098                this->powerOnLED(LED_LEARN, 1);
00099                this->powerOnLED(LED_EXPLORATION, 0);
00100                
00101                this->msDelay(250);
00102           }
00103           
00104           return 0;
00105      }
00106      
00107      return !status;
00108 }
00109 
00111 void Controller::enableV24Interrupt()  { 
00112 
00113      this->disableV24Interrupt();
00114      if (this->getSwitchState(SWITCH_V24)) {
00115           //UCSRB |=  (1 << RXCIE); 
00116           //sei();
00117      }
00118      //PORTC &= ~(1<<PC3);
00119      //UCSRB |= (1<<RXEN);
00120 }
00121 
00123 void Controller::disableV24Interrupt() { 
00124      if (this->getSwitchState(SWITCH_V24)) {
00125           //UCSRB &= ~(1 << RXCIE); 
00126           //cli();
00127      }
00128      //PINC |= (1<<PC3);
00129      //PORTC |= (1<<PC3);
00130      //UCSRB &= ~(1<<RXEN);
00131 }
00132 
00133 
00135 void Controller::msDelay (int n) {
00136 
00137      double maxDelay = (262.14 / (double)((11059200UL / 1000000)));
00138 
00139      for (int i=0; i< (n/maxDelay); i++) {
00140           _delay_ms (maxDelay);
00141      }
00142 }
00143 
00145 void Controller::setDefaultPolicy() {
00146 
00147      int _val=0;
00148      int _row, _column;
00149      STATE tmpState;
00150 
00151      // default grid size 5x5
00152      states.scalX = 5;
00153      states.scalY = 5;
00154      
00155      // initialize states 
00156      for (_row=0; _row < states.scalY; _row++) {
00157           for (_column=0; _column<states.scalX; _column++) {
00158 
00159                     tmpState.value = 0.0; 
00160 
00161                     tmpState.feedback_up = 0;
00162                     tmpState.feedback_down = 0;
00163                     tmpState.feedback_left = 0;
00164                     tmpState.feedback_right = 0;
00165                
00166                     states.setState (_row, _column, &tmpState);
00167           }
00168      }
00169 
00170      // position arm into air
00171      this->gotoStateWithoutSavingFeedback(0,0);
00172 }
00173 
00174 
00176 void Controller::gotoStateWithoutSavingFeedback(unsigned char _row, unsigned char _column) {
00177 
00178      servo_y.setPosition(_row, states.scalY);
00179      servo_x.setPosition(_column, states.scalX);
00180 
00181      currentX = _column;
00182      currentY = _row;
00183 }
00184 
00185 
00187 void Controller::powerOnLED(unsigned char _ledID, bool _status) {
00188 
00189      if (_status == true) {
00190           PORTA &= ~(1<<_ledID);
00191      } else {
00192           PORTA |= (1<<_ledID);
00193      }
00194 }
00195 
00197 void Controller::initLEDs() {
00198 
00199      // LED test (turn all LEDs on)
00200      this->powerOnLED(LED_POWER, true);
00201      this->powerOnLED(LED_WALK, true);
00202      this->powerOnLED(LED_LEARN, true);
00203      this->powerOnLED(LED_EXPLORATION, true);
00204 
00205      msDelay(800);
00206 
00207      // LED test (turn all LEDs off)
00208      this->powerOnLED(LED_POWER, true);
00209      this->powerOnLED(LED_WALK, false);
00210      this->powerOnLED(LED_LEARN, false);
00211      this->powerOnLED(LED_EXPLORATION, false);
00212 }
00213 
00214 
00215 void Controller::blinkLED (unsigned char led_id, unsigned int count, unsigned int delay) {
00216 
00217      unsigned char i;
00218 
00219      DDRA = 0x0F;
00220      PORTA |= (1<<PA4) | (1<<PA5) | (1<<PA6) | (1<<PA7);
00221 
00222      for (i=0; i<count; i++) {
00223 
00224           if (led_id & (1<<LED_LEARN)) this->powerOnLED ( LED_LEARN, false);
00225           if (led_id & (1<<LED_WALK)) this->powerOnLED ( LED_WALK, false);
00226           if (led_id & (1<<LED_EXPLORATION)) this->powerOnLED ( LED_EXPLORATION, false);
00227           if (led_id & (1<<LED_POWER)) this->powerOnLED ( LED_POWER, true);
00228 
00229           this->msDelay (delay);
00230 
00231 
00232           if (led_id & (1<<LED_WALK)) this->powerOnLED ( LED_WALK, true);
00233           if (led_id & (1<<LED_LEARN)) this->powerOnLED ( LED_LEARN, true);
00234           if (led_id & (1<<LED_EXPLORATION)) this->powerOnLED ( LED_EXPLORATION, true);
00235           if (led_id & (1<<LED_POWER)) this->powerOnLED ( LED_POWER, true);
00236 
00237           this->msDelay (delay);
00238      }
00239 }

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