00001 #include "test.h" 00002 #include "V24Control.h" 00003 00004 // the global object 00005 V24Control v24; 00006 00008 int main( void ) 00009 { 00010 v24.init(); 00011 00012 // initialize auto start counter 00013 unsigned char autoStartCounter = 0; 00014 00015 v24.vi.ctr.newMainMode = MAIN_MODE_WALK_POLICY; 00016 00017 while ( 1 ) { 00018 00019 // set new mode in case of interrupt 00020 v24.vi.ctr.mainMode = v24.vi.ctr.newMainMode; 00021 00022 // only walk when PAUSE 00023 if (v24.vi.ctr.getSwitchState(SWITCH_WALK)) { 00024 00025 // Check for new serial data 00026 if (v24.newDataReceived()) { 00027 00028 // stop robot 00029 v24.vi.ctr.disableV24Interrupt(); 00030 cli(); 00031 v24.vi.ctr.newMainMode = MAIN_MODE_STOP; 00032 00033 v24.processCommand(); 00034 v24.vi.ctr.mainMode = v24.vi.ctr.newMainMode; 00035 v24.vi.ctr.autoStart = 0; 00036 00037 00038 // Autostart-Routine 00039 } else if (v24.vi.ctr.autoStart) { 00040 00041 // delay for at least 5 seconds 00042 if (autoStartCounter < 5) { 00043 00044 autoStartCounter++; 00045 00046 v24.vi.ctr.blinkLED ( 00047 (1<<LED_WALK)|(1<<LED_LEARN)|(1<<LED_EXPLORATION), 1, 500); 00048 00049 // start after 5 seconds 00050 } else { 00051 00052 // disable auto-start 00053 v24.vi.ctr.autoStart = false; 00054 00055 // explore environment if SWITCH_FULL_EXP_AT_START is enabled 00056 if (v24.vi.ctr.getSwitchState(SWITCH_FULL_EXP_AT_START)) { 00057 v24.vi.exploreAllFeedbacks(false); 00058 } 00059 v24.vi.ctr.enableV24Interrupt(); 00060 00061 v24.vi.ctr.newMainMode = MAIN_MODE_WALK_POLICY; 00062 } 00063 00064 // do next action 00065 } else if (v24.vi.ctr.mainMode == MAIN_MODE_WALK_POLICY) { 00066 00067 // execute next action - with respect on SWTICH_ENABLE_EXPLORATION switch 00068 v24.vi.moveAndSaveFeedback( 00069 // choose best action 00070 v24.vi.getGreedyAction( 00071 v24.vi.ctr.currentY, v24.vi.ctr.currentX 00072 ), 00073 // pure greedy or exploitation/exploration 00074 v24.vi.ctr.getSwitchState(SWITCH_ENABLE_EXPLORATION) 00075 ); 00076 00077 // save old value(currentState) (for VDBE) 00078 v24.vi.oldValue = v24.vi.ctr.states.getValue( 00079 v24.vi.ctr.currentY, v24.vi.ctr.currentX 00080 ); 00081 00082 // value iterate all states 00083 v24.vi.valueIterateAllStates(); 00084 00085 // save new value(currentState) (for VDBE) 00086 v24.vi.newValue = v24.vi.ctr.states.getValue( 00087 v24.vi.ctr.currentY, v24.vi.ctr.currentX 00088 ); 00089 00090 v24.vi.ctr.newMainMode = MAIN_MODE_WALK_POLICY; 00091 00092 // explore environment 00093 } else if (v24.vi.ctr.mainMode == MAIN_MODE_EXPLORE_FEEDBACK) { 00094 00095 v24.vi.exploreAllFeedbacks(true); 00096 00097 // set learning mode 00098 v24.vi.ctr.newMainMode = MAIN_MODE_LEARN; 00099 00100 // learning-only mode 00101 } else if (v24.vi.ctr.mainMode == MAIN_MODE_LEARN) { 00102 00103 // value iterate all states 00104 v24.vi.valueIterateAllStates(); 00105 00106 // pause 00107 } else if (v24.vi.ctr.mainMode == MAIN_MODE_STOP) { 00108 00109 //vi.valueIterateAllStates(); 00110 } 00111 00112 // if robot is in PAUSE-mode, only check for new serial data 00113 } else { 00114 00115 // check for new serial data 00116 if (v24.newDataReceived()) { 00117 00118 // stop robot 00119 v24.vi.ctr.newMainMode = MAIN_MODE_STOP; 00120 00121 // process serial command 00122 v24.processCommand(); 00123 00124 // update main mode 00125 v24.vi.ctr.mainMode = v24.vi.ctr.newMainMode; 00126 00127 // otherwise perform value-iteration 00128 } else if (v24.vi.ctr.mainMode == MAIN_MODE_LEARN) { 00129 v24.vi.valueIterateAllStates(); 00130 } 00131 } 00132 } 00133 } 00134 00135