00001 #include "V24Control.h"
00002 #include "SensorOnlyMode.h"
00003
00004
00005
00006
00007 volatile int softwareMode = ROBOT_STANDALONE;
00008
00010 int main( void )
00011 {
00012
00013
00014
00015
00016 BoardController::initPorts();
00017
00018
00019 if (0) {
00020 softwareMode = SENSOR_MODE;
00021 sensorMain();
00022 } else {
00023 softwareMode = ROBOT_STANDALONE;
00024 }
00025
00026
00027
00028 V24Control *v24 = new V24Control();
00029 ValueIteration *vi = v24->vi;
00030
00031 #ifdef TESTMODE
00032 #warning compiled TESTMODE!
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 BoardController::enableSerialV24Interface();
00050
00051 ACTION_RETURN aRet;
00052
00053 while (1) {
00054 for (unsigned char i=48; i<55; i++) {
00055 v24->transmit(i);
00056 BoardController::msDelay(500);
00057 }
00058 }
00059
00060 unsigned char sByte=0;
00061 while (1) {
00062 if (v24->newDataReceived()) {
00063
00064 BoardController::powerOnLED(LED_LEARN, true);
00065
00066
00067 v24->receive(&sByte);
00068
00069
00070
00071
00072
00073
00074
00075 if (sByte == 'a') {
00076 vi->env->doAction(LEFT, 1000, &aRet);
00077 v24->transmit('b');
00078
00079 } else if (sByte == 's') {
00080 vi->env->doAction(RIGHT, 1000, &aRet);
00081 v24->transmit('t');
00082
00083 } else if (sByte == 'd') {
00084 vi->env->doAction(UP, 1000, &aRet);
00085 v24->transmit('e');
00086
00087 } else if (sByte == 'f') {
00088 vi->env->doAction(DOWN, 1000, &aRet);
00089 v24->transmit('g');
00090 }
00091
00092
00093
00094 BoardController::powerOnLED(LED_LEARN, false);
00095 }
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 PORTC &= ~(1<<RTS);
00117
00118 }
00119
00120 #else
00121
00122
00123
00124
00125
00126
00127
00128 unsigned char autoStartCounter = 0;
00129 bool autoStart = 1;
00130
00131 int mainMode = MAIN_MODE_WALK_POLICY;
00132 int newMainMode = MAIN_MODE_WALK_POLICY;
00133
00134 while ( 1 ) {
00135
00136
00137 mainMode = newMainMode;
00138
00139
00140 if (BoardController::getSwitchState(SWITCH_WALK)) {
00141
00142
00143 if (v24->newDataReceived()) {
00144
00145
00146 BoardController::disableV24Interrupt();
00147 cli();
00148 newMainMode = MAIN_MODE_STOP;
00149
00150 v24->processCommand();
00151 autoStart = 0;
00152
00153 newMainMode = v24->getNewMainMode(newMainMode);
00154
00155
00156
00157 } else if (autoStart) {
00158
00159
00160 if (autoStartCounter < 5) {
00161
00162 autoStartCounter++;
00163
00164 BoardController::blinkLED (
00165 (1<<LED_WALK)|(1<<LED_LEARN)|(1<<LED_EXPLORATION), 1, 500);
00166
00167
00168 } else {
00169
00170
00171 autoStart = false;
00172
00173
00174 if (BoardController::getSwitchState(SWITCH_FULL_EXP_AT_START)) {
00175 vi->exploreAllFeedbacks(false);
00176 }
00177 BoardController::enableV24Interrupt();
00178
00179 newMainMode = MAIN_MODE_WALK_POLICY;
00180 }
00181
00182
00183 } else if (mainMode == MAIN_MODE_WALK_POLICY) {
00184
00185 vi->doNextAction();
00186
00187
00188 } else if (mainMode == MAIN_MODE_EXPLORE_FEEDBACK) {
00189
00190 vi->exploreAllFeedbacks(true);
00191
00192
00193 newMainMode = MAIN_MODE_LEARN;
00194
00195
00196 } else if (mainMode == MAIN_MODE_LEARN) {
00197
00198
00199 vi->valueIterateAllStates();
00200
00201
00202 } else if (mainMode == MAIN_MODE_STOP) {
00203
00204
00205 }
00206
00207
00208 } else {
00209
00210 if (v24->newDataReceived()) {
00211
00212
00213 newMainMode = MAIN_MODE_STOP;
00214
00215
00216 v24->processCommand();
00217
00218
00219 newMainMode = v24->getNewMainMode(mainMode);
00220
00221
00222 } else if (mainMode == MAIN_MODE_LEARN) {
00223 vi->valueIterateAllStates();
00224 }
00225 }
00226 }
00227 #endif
00228
00229
00230 }
00231
00232