00001 #include "global.h"
00002 #include "Controller.h"
00003
00004
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
00015 servo_x.setTimer(TIMER1B);
00016 servo_y.setTimer(TIMER1A);
00017
00018
00019 DDRA = (1<<PA0) | (1<<PA1) | (1<<PA2) | (1<<PA3);
00020
00021 PORTA |= (1<<PA4) | (1<<PA5) | (1<<PA6) | (1<<PA7);
00022
00023
00024 DDRB = 0x00;
00025
00026 PORTB |= (1<<PB0) | (1<<PB1) | (1<<PB2) | (1<<PB4);
00027
00028
00029 initLEDs();
00030
00031
00032 autoStart = true;
00033
00034
00035 currentX = 0x00;
00036 currentY = 0x00;
00037
00038
00039 setDefaultPolicy();
00040 }
00041
00043 unsigned char Controller::getSwitchState (unsigned char _switch) {
00044
00045 bool status;
00046
00047 if (_switch == SWITCH_WALK) {
00048
00049 status = (PINB & (1<<PB4));
00050
00051 } else if (_switch == SWITCH_RANDOM_STATE) {
00052
00053 status = (PINB & (1<<PB2));
00054
00055 } else if (_switch == SWITCH_INV_FB_0) {
00056
00057 status = (PINB & (1<<PB1));
00058
00059 } else if (_switch == SWITCH_ENABLE_EXPLORATION) {
00060
00061 status = (PINB & (1<<PB0));
00062
00063 } else if (_switch == SWITCH_FULL_EXP_AT_START) {
00064
00065 status = (PINA & (1<<PA4));
00066
00067 } else if (_switch == SWITCH_V24) {
00068
00069 status = (PINA & (1<<PA5));
00070
00071 } else if (_switch == SWITCH_VDBE_EGREEDY) {
00072
00073 status = (PINA & (1<<PA6));
00074
00075 } else if (_switch == SWITCH_RAM) {
00076
00077 status = SWITCH_RAM_VALUE;
00078
00079 } else if (_switch == SWITCH_GAMMA) {
00080 status = (PINA & (1<<PA7));
00081
00082
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
00116
00117 }
00118
00119
00120 }
00121
00123 void Controller::disableV24Interrupt() {
00124 if (this->getSwitchState(SWITCH_V24)) {
00125
00126
00127 }
00128
00129
00130
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
00152 states.scalX = 5;
00153 states.scalY = 5;
00154
00155
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
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
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
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 }