main.c

Go to the documentation of this file.
00001 #include "V24Control.h"
00002 #include "SensorOnlyMode.h"
00003 
00004 
00005 //#include "test.h"
00006 
00007 volatile int softwareMode = ROBOT_STANDALONE; // for interrupt sharing
00008 
00010 int main( void )
00011 {    
00012 
00013 
00014 
00015      // initialize ports+LEDs
00016      BoardController::initPorts();
00017 
00018      // software mode
00019      if (0) {
00020           softwareMode = SENSOR_MODE;
00021           sensorMain();  
00022      } else {
00023           softwareMode = ROBOT_STANDALONE;
00024      }
00025 
00026 
00027      // initialize serial communication class with standard VI agent
00028      V24Control *v24 = new V24Control();
00029      ValueIteration *vi = v24->vi;
00030 
00031 #ifdef TESTMODE
00032 #warning compiled TESTMODE!
00033 
00034 
00035 
00036      //BoardController::blinkLED((1<<LED_POWER)|(1<<LED_WALK)|(1<<LED_LEARN)|(1<<LED_EXPLORATION), 20, 200);
00037 
00038      // initialize serial communication class with standard VI agent
00039      //V24Control *v24 = new V24Control();
00040      //ValueIteration *vi = v24->vi;
00041 
00042 
00043 
00044      /******************************************
00045      * Test Mode
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                //BoardController::msDelay(1000);  
00066 
00067                v24->receive(&sByte);
00068                //PORTC |= (1<<RTS);
00069 
00070                //sByte++;
00071                //PORTC |= (1<<RTS);
00072                //v24->transmit(sByte);
00073 
00074                // LEFT
00075                if (sByte == 'a') {
00076                     vi->env->doAction(LEFT, 1000, &aRet);
00077                     v24->transmit('b');
00078                // RIGHT
00079                } else if (sByte == 's') {
00080                     vi->env->doAction(RIGHT, 1000, &aRet);
00081                     v24->transmit('t');
00082                // UP
00083                } else if (sByte == 'd') {
00084                     vi->env->doAction(UP, 1000, &aRet);
00085                     v24->transmit('e');
00086                // DOWN
00087                } else if (sByte == 'f') {
00088                     vi->env->doAction(DOWN, 1000, &aRet);
00089                     v24->transmit('g');
00090                }
00091 
00092                // indicate that controller is again ready for reception
00093 
00094                BoardController::powerOnLED(LED_LEARN, false);
00095           }
00096 
00097           
00098           /*
00099           for (unsigned char i=33; i<125; i++) {
00100                v24->transmit(i);
00101           }
00102           
00103           v24->transmit(0x30);
00104           BoardController::msDelay(1000);
00105           */
00106 
00107           /*
00108           BoardController::blinkLED (
00109                (1<<LED_WALK)|(1<<LED_LEARN)|(1<<LED_EXPLORATION), 1, 500
00110           );
00111           */
00112 
00113           //PORTC |= (1<<RTS);
00114           //v24->transmit(0x30);
00115           //BoardController::msDelay(1000);
00116           PORTC &= ~(1<<RTS);
00117 
00118      }
00119 
00120 #else 
00121 
00122      /******************************************
00123      * NORMAL Mode
00124      ******************************************/
00125 
00126 
00127      // initialize auto start counter 
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           // set new mode in case of interrupt
00137           mainMode = newMainMode;
00138 
00139           // only walk when PAUSE 
00140           if (BoardController::getSwitchState(SWITCH_WALK)) {
00141 
00142                // Check for new serial data
00143                if (v24->newDataReceived()) {
00144 
00145                     // stop robot
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                // Autostart-Routine
00157                } else if (autoStart) {
00158 
00159                     // delay for at least 5 seconds
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                     // start after 5 seconds
00168                     } else {
00169 
00170                          // disable auto-start
00171                          autoStart = false;
00172                          
00173                          // explore environment if SWITCH_FULL_EXP_AT_START is enabled
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                // do next action
00183                } else if (mainMode == MAIN_MODE_WALK_POLICY) {
00184 
00185                     vi->doNextAction();
00186 
00187                // explore environment
00188                } else if (mainMode == MAIN_MODE_EXPLORE_FEEDBACK) {
00189      
00190                     vi->exploreAllFeedbacks(true);
00191      
00192                     // perform value-iteration to learn the modell
00193                     newMainMode = MAIN_MODE_LEARN;
00194      
00195                // learning-only mode
00196                } else if (mainMode == MAIN_MODE_LEARN) {
00197 
00198                     // value iterate all states
00199                     vi->valueIterateAllStates();  
00200                     
00201                // pause
00202                } else if (mainMode == MAIN_MODE_STOP) {
00203      
00204                     // dummy
00205                }
00206 
00207           // if robot is in PAUSE-mode, only check for new serial data
00208           } else {
00209                // check for new serial data
00210                if (v24->newDataReceived()) {
00211 
00212                     // stop robot
00213                     newMainMode = MAIN_MODE_STOP; 
00214      
00215                     // process serial command
00216                     v24->processCommand();
00217      
00218                     // update main mode
00219                     newMainMode = v24->getNewMainMode(mainMode);
00220 
00221                // otherwise perform value-iteration
00222                } else if (mainMode == MAIN_MODE_LEARN) {
00223                     vi->valueIterateAllStates();
00224                }
00225           }
00226      }
00227 #endif
00228 
00229 
00230 }
00231 
00232 

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