V24Control.c

Go to the documentation of this file.
00001 #include "V24Control.h"
00002 
00006 //extern V24Control v24;
00007 //V24COMMAND tx_command, rx_command;
00008 //volatile bool newDataAvailable = false;
00009 
00013 V24Control::V24Control() {
00014 
00015 
00016 //#ifdef TESTMODE
00017 //#warning VI disabled
00018 //#else   
00019      // create new valueIteration object with a 5x5 StateSpace
00020      this->vi = new ValueIteration(new ENV_POS(5,5));
00021 //#endif
00022 }
00023 
00024 
00026 V24Control::~V24Control() {
00027      delete this->vi;
00028 }
00029 
00033 bool V24Control::newDataReceived() {
00034 
00035      //if (BoardController::getSwitchState(SWITCH_V24)) {
00036 
00037           // enable interrupt
00038           //BoardController::enableV24Interrupt();
00039           
00040 //        if (newDataAvailable == true || (UCSRA & (1<<RXC))) {
00041 //             newDataAvailable = false;
00042 //             return true;
00043 //        } else {
00044 //             return false;
00045 //        }
00046           
00047           return (UCSRA & (1<<RXC));
00048      //}
00049 
00050      //return false;
00051 }
00052 
00056 void V24Control::init () {
00057 
00058      // should each received byte be send back to the sender? (for debug)
00059      rxtx = false;
00060 
00061      // enable serial interface
00062      BoardController::enableSerialV24Interface();
00063 }
00064 
00065 
00069 void V24Control::transmit (unsigned char data) {
00070 
00071      //if (BoardController::getSwitchState(SWITCH_V24)) {
00072 
00073           //BoardController::enableSerialV24Interface();
00074 
00075           /* wait till transmit buffer is ready */
00076           while (!(UCSRA & (1<<UDRE)));
00077 
00078           /* wait until busy-pin (from RF04) is ready */
00079           while (PINC & (1<<CTS)); // PC2
00080      
00081           /* send data */
00082           UDR = data;
00083      
00084           // wait until transmission is complete
00085           while (!(UCSRA & (1<<TXC)));
00086      //}
00087 }
00088 
00089 
00093 bool V24Control::receive (unsigned char *data) {
00094 
00095      if (BoardController::getSwitchState(SWITCH_V24)) {
00096 
00097           // CTS setzen, damit GUI schicken kann
00098           //PINC |= (1<<CTS); // PC2
00099           BoardController::powerOnLED(LED_POWER, false);
00100           //unsigned char rx;
00101           int errorLoop = 0;
00102      
00103           
00104           // wait a maximum of 1 second until
00105           while (!(UCSRA & (1<<RXC)) && errorLoop <100) {
00106                errorLoop++;
00107                BoardController::msDelay(10);
00108           }
00109 
00110           if (errorLoop == 100)
00111                return false;
00112           
00113           
00114      
00115           // error check
00116           /*if (errorLoop>=100) {
00117                BoardController::powerOnLED(LED_POWER, true);
00118                //BoardController::newMainMode = MAIN_MODE_STOP;
00119                return false;
00120           } else {
00121           */
00122                // return received data
00123                *data = UDR;
00124                BoardController::powerOnLED(LED_POWER, true);
00125           /*
00126                return true;
00127           }
00128           */
00129      }
00130 
00131      return true;
00132 }
00133 
00137 bool V24Control::transmitCommand (V24COMMAND *c) {
00138 
00139      unsigned char loopCount;
00140 
00141      if (BoardController::getSwitchState(SWITCH_V24)) {
00142 
00143      
00144           unsigned char tmp;
00145 
00146 //        // transmit command-byte
00147 //        transmit(V24_COMMAND_SYNC_COMMAND);
00148 //        if (rxtx) {
00149 //             receive(&tmp);
00150 //             if (tmp != V24_COMMAND_SYNC_OK_COMMAND) return false;
00151 //        }
00152 // 
00153 //   
00154           // transmit command-byte
00155           transmit(c->command);
00156           /*if (rxtx) {
00157                receive(&tmp);
00158                if (tmp != c->command) return false;
00159           }*/
00160 
00161           // transmit command-length    
00162           transmit(c->dataLength);
00163           /*if (rxtx) {
00164                receive(&tmp);
00165                if (tmp != c->dataLength) return false;
00166           }*/
00167      
00168           // transmit data
00169           for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00170      
00171                transmit(c->data[loopCount]);
00172                /*if (rxtx) {
00173                     receive(&tmp);
00174                     if (tmp != c->data[loopCount]) return false;
00175                }*/
00176           }
00177      
00178           return true;
00179      }
00180 }
00181 
00185 bool V24Control::receiveCommand (V24COMMAND *c) {
00186 
00187      unsigned char loopCount, rData, data;
00188      bool sync = false;
00189      char i;
00190 
00191      if (BoardController::getSwitchState(SWITCH_V24)) {
00192 
00193 //        // synchronisation
00194 //        rData = !V24_COMMAND_SYNC_COMMAND;
00195 //        do {
00196 //             if (receive(&rData) && rData==V24_COMMAND_SYNC_COMMAND) {
00197 //                  transmit(V24_COMMAND_SYNC_OK_COMMAND);
00198 //                  sync = true;
00199 //             }
00200 //        } while (!sync);
00201 
00202           // receive command byte
00203           sync = false;
00204           do {
00205                if (receive(&c->command)) {
00206                     //transmit(c->command);
00207                     sync = true;
00208                }
00209           } while (!sync);
00210           
00211           // receive command-length
00212           sync = false;
00213           do {
00214                if (receive(&c->dataLength)) {
00215                     //transmit(c->dataLength);
00216                     sync = true;
00217                }
00218           } while (!sync);
00219 
00220           
00221           // receive command data
00222           for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00223                sync = false;
00224                do {
00225                     if (receive(&c->data[loopCount])) {
00226                          //transmit(c->data[loopCount]);
00227                          sync = true;
00228                     }
00229                } while (!sync);         
00230           }
00231      
00232           return true;
00233      }
00234 }
00235 
00236 
00240 void V24Control::processCommand() {
00241 
00242      if (BoardController::getSwitchState(SWITCH_V24)) {
00243      
00244           // disable all interrupts
00245           cli();
00246           BoardController::disableV24Interrupt();
00247      
00248           char i=0;
00249           char cmd;
00250           unsigned char* bytes;
00251           int _row, _column;
00252           V24COMMAND tmp_command;
00253                
00254           // turn off LEDs
00255           BoardController::powerOnLED (LED_LEARN, false);
00256           BoardController::powerOnLED (LED_WALK, false);
00257           BoardController::powerOnLED (LED_EXPLORATION, false);
00258      
00259           // disable auto start
00260           //BoardController::autoStart = false;
00261      
00262           // receive command
00263           //if (!commandParsedByIRQ) {
00264                if (!this->receiveCommand ( &rx_command )) {
00265                     this->newMainMode = MAIN_MODE_STOP;
00266                     BoardController::msDelay(1000);
00267                     this->flushReceiveBuffer();
00268                     BoardController::enableV24Interrupt();
00269                     return;
00270                }
00271           //} else {
00272           //   commandParsedByIRQ = false;
00273           //}
00274      
00275           cmd = rx_command.command;
00276      
00277           /***************
00278           * walking mode *
00279           ****************/
00280           if ( cmd == V24_COMMAND_START_WALKING ) {
00281      
00282                // go to start position (0,0)
00283                ENV_POS newPos(0, 0);
00284                vi->env->beamRobotToState(&newPos);
00285                this->newMainMode = MAIN_MODE_WALK_POLICY;
00286                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00287           
00288           /*************
00289           * stop robot *
00290           **************/
00291           } else if ( cmd == V24_COMMAND_STOP_WALKING ) {
00292      
00293                this->newMainMode = MAIN_MODE_STOP;
00294                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00295           
00296           /********************
00297           * send gamma to GUI *
00298           *********************/
00299           } else if ( cmd == V24_COMMAND_GET_GAMMA ) {
00300      
00301                processGetGamma();
00302                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00303      
00304           /************
00305           * set gamma *
00306           *************/
00307           } else if ( cmd == V24_COMMAND_SET_GAMMA ) {
00308      
00309                processSetGamma(&rx_command);
00310                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00311      
00312           /**********************
00313           * set grid dimensions *
00314           ***********************/
00315           } else if ( cmd == V24_COMMAND_SET_GRIDSIZE ) {
00316      
00317                processSetGridsize(&rx_command);
00318                
00319           /*******************************
00320           * send current grid dimensions *
00321           ********************************/
00322           } else if ( cmd == V24_COMMAND_GET_GRIDSIZE ) {
00323      
00324                processGetGridsize();
00325                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00326      
00327           /***************************************
00328           * set a state (Value+rewards)          *
00329           ****************************************/
00330           } else if ( cmd == V24_COMMAND_SET_STATE ) {
00331      
00332                processSetState(&rx_command);
00333      
00334           /**************************
00335           * send a state to the GUI *
00336           ***************************/
00337           } else if ( cmd == V24_COMMAND_GET_STATE ) {
00338           
00339                processGetState(&rx_command);
00340      
00341           /*****************************************************
00342           * returns the exploration/exploitation configuration *
00343           ******************************************************/
00344           } else if ( cmd == V24_COMMAND_GET_EXPLORATION_CONFIG ) {
00345           
00346                processGetExplorationConfig();
00347      
00348           /**********************************************
00349           * returns user defined debug information *
00350           ***********************************************/
00351           } else if ( cmd == V24_COMMAND_GET_DEBUG ) {
00352           
00353                processGetDebug();
00354 
00355           /*************************
00356           * RAM-Test - write bytes *
00357           **************************/
00358           } else if ( cmd == V24_COMMAND_WRITE_RAM_BYTES ) {
00359      
00360                this->newMainMode = MAIN_MODE_STOP;
00361      
00362                // write random numbers into memory
00363                vi->states->ramtestSaveBytes(  
00364                     ((rx_command.data[0]*256) + rx_command.data[1]), // Address
00365                     &(rx_command.data[2]),  // Pointer to data
00366                     rx_command.dataLength-2 // size
00367      
00368                );
00369                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00370      
00371           /************************
00372           * RAM-Test - read bytes *
00373           *************************/
00374           } else if ( cmd == V24_COMMAND_READ_RAM_BYTES ) {
00375      
00376                // stop main program
00377                this->newMainMode = MAIN_MODE_STOP;
00378      
00379                // create commando structure
00380                tmp_command.command = V24_COMMAND_WRITE_RAM_BYTES;
00381                tmp_command.dataLength = rx_command.data[2]+2; //
00382                tmp_command.data[0] = rx_command.data[0];
00383                tmp_command.data[1] = rx_command.data[1];
00384      
00385                // read data from RAM
00386                vi->states->ramtestReadBytes(  
00387                     ((rx_command.data[0]*256) + rx_command.data[1]), // address
00388                     &(tmp_command.data[2]),   // data pointer
00389                     rx_command.data[2]   // size
00390                );
00391      
00392                // send back to GUI
00393                if (!this->transmitCommand (&tmp_command)) return;
00394                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00395      
00396           /***********************************
00397           * explore all rewards            *
00398           ************************************/
00399           } else if ( cmd == V24_COMMAND_EXPLORE_FEEDBACK ) {
00400      
00401                this->newMainMode = MAIN_MODE_EXPLORE_FEEDBACK;
00402                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00403      
00404           /***********************************
00405           * learning only mode             *
00406           ************************************/
00407           } else if ( cmd == V24_COMMAND_START_LEARNING ) {
00408      
00409                this->newMainMode = MAIN_MODE_LEARN;
00410                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00411 
00412           /**********************************
00413           * returns the distance traveled   *
00414           ***********************************/
00415           } else if (cmd == V24_COMMAND_GET_DISTANCE ) {
00416 
00417                signed long int _distance = vi->getDistanceCovered();
00418                unsigned char* bytes = (unsigned char*)&_distance;
00419 
00420                tx_command.command = V24_COMMAND_SET_DISTANCE;
00421                tx_command.dataLength = 0x04;
00422                tx_command.data[0] = bytes[0];
00423                tx_command.data[1] = bytes[1];
00424                tx_command.data[2] = bytes[2];
00425                tx_command.data[3] = bytes[3];
00426 
00427                if (!this->transmitCommand (&tx_command)) return; 
00428                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00429 
00430           /**************************
00431            * reset distance counter *
00432            **************************/
00433           } else if (cmd == V24_COMMAND_RESET_DISTANCE_COUNTER ) {
00434 
00435                vi->resetDistanceCounter();
00436                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00437 
00438           /***************************
00439            * set exploration config  *
00440            ***************************/
00441           } else if (cmd == V24_COMMAND_SET_EXPLORATION_CONFIG ) {
00442 
00443                unsigned char* bytes;
00444                register char i;
00445           
00446                // exploration parameter
00447                bytes = (unsigned char*)&vi->expParameter;
00448                for (i=0; i < sizeof(vi->expParameter); i++) {
00449                     bytes[i] = rx_command.data[i];
00450                }
00451 
00452                // exploration mode           
00453                bytes = (unsigned char*)&vi->expMode;
00454                bytes[0] = rx_command.data[sizeof(vi->expParameter)];
00455 
00456                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00457 
00458           /***********************************
00459           * send current position, main-mode and iteration-count to GUI    *
00460           ***********************************/
00461           } else if (cmd == V24_COMMAND_GET_CURRENT_STATE_MODE_ITERATIONS ) {
00462      
00463                unsigned long int _itCount = vi->getIterationCount();
00464                unsigned char* bytes = (unsigned char*)&_itCount;
00465                signed char wert;
00466                
00467                tx_command.command = V24_COMMAND_SET_CURRENT_STATE_MODE_ITERATIONS;
00468                tx_command.dataLength = 0x09;
00469                
00470                ENV_POS pos;
00471                vi->env->getCurrentState(&pos);
00472                tx_command.data[0] = pos.y;
00473                tx_command.data[1] = pos.x;
00474                tx_command.data[2] = (unsigned char)this->mainMode;
00475                tx_command.data[3] = bytes[0];
00476                tx_command.data[4] = bytes[1];
00477                tx_command.data[5] = bytes[2];
00478                tx_command.data[6] = bytes[3];
00479                wert = 127;
00480                tx_command.data[7] = *(unsigned char*)&wert;
00481                wert = -127;
00482                tx_command.data[8] = *(unsigned char*)&wert;
00483                
00484      
00485                if (!this->transmitCommand (&tx_command)) return; 
00486                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00487 
00488           /***********************************
00489           * send current position, main-mode and iteration-count to GUI    *
00490           ***********************************/
00491           } else if (cmd == V24_COMMAND_GET_CURRENT_STATE ) {
00492                     
00493                tx_command.command = V24_COMMAND_SET_CURRENT_STATE;
00494                tx_command.dataLength = 0x02;
00495                
00496                ENV_POS pos;
00497                vi->env->getCurrentState(&pos);
00498                tx_command.data[0] = pos.x;
00499                tx_command.data[1] = pos.y;
00500                
00501                if (!this->transmitCommand (&tx_command)) return; 
00502                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00503 
00504      
00505 //        /*********************************************************
00506 //        * receive states (values+rewards) and gamma from the GUI *
00507 //        **********************************************************/
00508 //        } else if (cmd == V24_COMMAND_SET_ALL_STATES_AND_GAMMA ) {
00509 //        
00510 //             // set gamma
00511 //             if (!this->receiveCommand(&tmp_command)) return;
00512 //             processSetGamma(&tmp_command);
00513 //             if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00514 //   
00515 //             // set grid size
00516 //             if (!this->receiveCommand(&tmp_command)) return;
00517 //             processSetGridsize(&tmp_command);
00518 //   
00519 //             for (_row=0; _row < BoardController::states.scalY; _row++) {
00520 //                  for (_column=0; _column < BoardController::states.scalX; _column++) {
00521 //   
00522 //                       // receive state
00523 //                       if (!this->receiveCommand (&tmp_command)) return;
00524 //                       processSetState(&tmp_command);
00525 //                  }
00526 //             }         
00527 //        
00528 //        /********************************************************
00529 //        * send all states (Values+Rewards) and gamma to the GUI *
00530 //        *********************************************************/
00531 //        } else if (cmd == V24_COMMAND_GET_ALL_STATES_AND_GAMMA ) {
00532 //   
00533 //             // send gamma
00534 //             if (!this->receiveCommand(&tmp_command)) return;
00535 //             processGetGamma();
00536 //             if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00537 //   
00538 //             // send grid size
00539 //             if (!this->receiveCommand(&tmp_command)) return;
00540 //             processGetGridsize();
00541 //             if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00542 //   
00543 //             for (_row=0; _row < BoardController::states.scalY; _row++) {
00544 //                  for (_column=0; _column < BoardController::states.scalX; _column++) {
00545 //   
00546 //                       // receive getState Command
00547 //                       if (!this->receiveCommand (&tmp_command)) return;
00548 // 
00549 //                       // send state
00550 //                       processGetState(&tmp_command);
00551 //                  }
00552 //             }         
00553 
00554           // perform next action and return reward to the GUI
00555           } else if (cmd == V24_COMMAND_WALK_AND_RETURN ) {
00556 
00557                ENV_POS tmpPos;
00558 
00559                tx_command.command = V24_COMMAND_WALK_AND_RETURN_RESPONSE;
00560                tx_command.dataLength = 3;
00561 
00562                unsigned char action = rx_command.data[0];
00563                ACTION_RETURN aRet;
00564                vi->env->doAction(action, 100, &aRet);
00565 
00566                /* char reward = vi->moveAndSaveFeedback(
00567                          action, 0, &tmpPos);
00568                */
00569 
00570                tx_command.data[0] = aRet.state.x;
00571                tx_command.data[1] = aRet.state.y;
00572                tx_command.data[2] = aRet.reward;
00573                //tx_command.data[3] = rx_command.command;
00574                //tx_command.data[4] = rx_command.dataLength;
00575                //tx_command.data[5] = action;
00576 
00577 
00578                if (!this->transmitCommand (&tx_command)) return; 
00579                //if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00580 
00581                this->setMainMode(MAIN_MODE_STOP);
00582 
00583           /*******************************************************************************
00584           * return "UNKNOWN COMMAND" in case of an unknown command
00585           *******************************************************************************/
00586           } else {
00587                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_UNKNOWN_COMMAND)) return;
00588           }
00589      
00590      
00591           // enable serial interrupt
00592           BoardController::enableV24Interrupt();
00593      }
00594 }
00595 
00599 bool V24Control::transmitReceiveCommandProcessed(unsigned char _code, unsigned char _error) {
00600 
00601      if (BoardController::getSwitchState(SWITCH_V24)) {
00602      
00603 //        V24COMMAND ok_command;
00604 //        ok_command.command = _code;
00605 //   
00606 //        // Im Fehlerfall, den Fehlercode anhängen
00607 //        if (_error > 0) {
00608 //             ok_command.dataLength = 0x01;
00609 //             ok_command.data[0] = _error;
00610 //   
00611 //        // Ansonsten gibt es keinen Datenteil
00612 //        } else {
00613 //             ok_command.dataLength = 0x00;
00614 //        }
00615 //   
00616 //        // Bestätigung senden
00617 //        if (!v24.transmitCommand(&ok_command)) return false;
00618 //   
00619      //   return true;
00620      
00621      }
00622      return true;
00623 }
00624 
00628 void V24Control::flushReceiveBuffer() {
00629      if (BoardController::getSwitchState(SWITCH_V24)) {
00630           unsigned char dummy;
00631           while (UCSRA & (1<<RXC)) dummy = UDR;
00632      }
00633 }
00634 
00635 
00639 bool V24Control::processGetExplorationConfig() {
00640 
00641      if (BoardController::getSwitchState(SWITCH_V24)) {
00642      
00643           V24COMMAND tx_command;
00644           unsigned char* bytes;
00645           register char i;
00646      
00647           tx_command.command = V24_COMMAND_SET_EXPLORATION_CONFIG;
00648           tx_command.dataLength = 5;
00649 
00650           // Exploration Parameter
00651           bytes = (unsigned char*)&(vi->expParameter);
00652           for (i=0; i < sizeof(vi->expParameter); i++) {
00653                tx_command.data[i] = bytes[i];
00654           }
00655 
00656           // Exploration Mode
00657           tx_command.data[4] = vi->expMode;
00658           
00659      
00660           if (!transmitCommand (&tx_command)) return false;
00661           return true;
00662      }
00663 }
00664 
00665 
00669 bool V24Control::processGetGamma() {
00670 
00671      if (BoardController::getSwitchState(SWITCH_V24)) {
00672      
00673           V24COMMAND tx_command;
00674           unsigned char* bytes;
00675           register char i;
00676      
00677           tx_command.command = V24_COMMAND_SET_GAMMA;
00678           tx_command.dataLength = 4;
00679           bytes = (unsigned char*)&(vi->gamma);
00680      
00681           for (i=0; i < tx_command.dataLength; i++) {
00682                tx_command.data[i] = bytes[i];
00683           }
00684      
00685           if (!transmitCommand (&tx_command)) return false;
00686           return true;
00687      }
00688 }
00689 
00693 void V24Control::processSetGamma(V24COMMAND *cmd) {
00694 
00695      if (BoardController::getSwitchState(SWITCH_V24)) {
00696      
00697           unsigned char* bytes;
00698           register char i;
00699      
00700           bytes = (unsigned char*)&vi->gamma;
00701      
00702           for (i=0; i < 4; i++) {
00703                bytes[i] = cmd->data[i];
00704           }
00705      }
00706 }
00707 
00711 bool V24Control::processGetDebug() {
00712 
00713      if (BoardController::getSwitchState(SWITCH_V24)) {
00714      
00715           V24COMMAND tx_command;
00716      
00717           tx_command.command = V24_COMMAND_GET_DEBUG;
00718           tx_command.dataLength = 10;
00719 
00720           register char i;
00721 
00722           unsigned char* bytes = (unsigned char*)&(vi->currentExpProbability);
00723           for (i=0; i < tx_command.dataLength; i++) {
00724                tx_command.data[i] = bytes[i];
00725           }
00726 
00727           for (i=4; i<tx_command.dataLength; i++) {
00728                tx_command.data[i] = rand()%(NUM_ACTIONS);
00729                srand (rand()+1);
00730           }
00731      
00732           if (!transmitCommand (&tx_command)) return false;
00733           return true;
00734      }
00735 }
00736 
00737 
00741 bool V24Control::processGetGridsize() {
00742 
00743      if (BoardController::getSwitchState(SWITCH_V24)) {
00744      
00745           V24COMMAND tx_command;
00746      
00747           tx_command.command = V24_COMMAND_SET_GRIDSIZE;
00748           tx_command.dataLength = 2;
00749 
00750           ENV_POS pos;
00751           vi->env->getCurrentState(&pos);
00752 
00753           tx_command.data[0] = pos.y;
00754           tx_command.data[1] = pos.x;
00755      
00756           if (!transmitCommand (&tx_command)) return false;
00757           return true;
00758      }
00759 }
00760 
00764 bool V24Control::processSetGridsize(V24COMMAND *cmd) { 
00765 
00766      if (BoardController::getSwitchState(SWITCH_V24)) {
00767      
00768           unsigned char _scalX, _scalY;
00769      
00770           _scalY = cmd->data[0];
00771           _scalX = cmd->data[1];
00772      
00773           // check for maximum statespace size
00774           if ((_scalX*_scalY) > (MAX_STATES)) {
00775                if (!transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00776                return false;
00777 
00778           } else {
00779                //ENV_POS newDiscretization(_scalX, _scalY);
00780                //vi->env->setDiscretization(&newDiscretization);
00781 
00782                // create new VI object with statespace (scalx, scaly)
00783                delete this->vi;
00784                this->vi = new ValueIteration(new ENV_POS(_scalX, _scalY));
00785 
00786                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00787                return true;
00788           }
00789      }
00790 }
00791 
00795 bool V24Control::processGetState(V24COMMAND *cmd) {
00796 
00797      if (BoardController::getSwitchState(SWITCH_V24)) {
00798      
00799           ENV_POS pos, d;
00800           vi->env->getDiscretization(&d);
00801 
00802           V24COMMAND tx_command;
00803           pos.x = cmd->data[1];
00804           pos.y = cmd->data[0];
00805 
00806           
00807           if (pos.y >= d.y || pos.x >= d.x) {
00808                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00809                return false;
00810           } else {
00811      
00812                // get state
00813                vi->states->getState(&pos, &tmpState);
00814                unsigned char* bytes = (unsigned char*)&tmpState;
00815           
00816                tx_command.command = V24_COMMAND_SET_STATE;
00817                tx_command.dataLength = STATESIZE+2;
00818                tx_command.data[0] = pos.y;
00819                tx_command.data[1] = pos.x;
00820           
00821                for (unsigned char i=0; i < STATESIZE; i++) {
00822                     tx_command.data[2 + i] = bytes[i];
00823                }
00824      
00825                if (!this->transmitCommand (&tx_command)) return false;
00826                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00827                return true;
00828           }
00829      }
00830 }
00831 
00835 bool V24Control::processSetState(V24COMMAND *cmd) {
00836 
00837      if (BoardController::getSwitchState(SWITCH_V24)) {
00838 
00839           ENV_POS pos, d;
00840           pos.x = cmd->data[1];
00841           pos.y = cmd->data[0];
00842           vi->env->getDiscretization(&d);
00843                
00844           unsigned char* bytes = (unsigned char*)&tmpState;
00845      
00846           // check, if state exists in grid
00847           if (pos.y>= d.y || pos.x >= d.x) {
00848                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00849                return false;
00850           } else {
00851           
00852                for (unsigned char i=0; i<STATESIZE; i++) {
00853                     bytes[i] = cmd->data[i + 2];
00854                }
00855      
00856                // save state to memory
00857                vi->states->setState (&pos, &tmpState);
00858                if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00859                return true;
00860           }
00861      }
00862 }
00863 
00867 int V24Control::getNewMainMode(int oldMainMode) {
00868      if (this->newMainMode != MAIN_MODE_UNCHANGED) {
00869           int retMode = this->newMainMode;
00870           this->newMainMode = MAIN_MODE_UNCHANGED;
00871           return retMode;
00872      } else {
00873           return oldMainMode;
00874      }
00875 }
00876 
00880 void V24Control::setMainMode (int mainMode) {
00881      this->mainMode = mainMode;
00882 }
00883 
00884 
00886 //SIGNAL (SIG_UART_RECV) {
00887 
00888 // ISR (USART_RXC_vect) {
00889 //   newDataAvailable = true;
00890 // }
00891 //void SIG_UART_RECV (void) __attribute__ ((signal));
00892 //void SIG_UART_RECV (void) {
00893 
00894 //   if (BoardController::getSwitchState(SWITCH_V24)) {
00895 //   
00896 //        // disable interrups
00897 //        cli();
00898 //        BoardController::disableV24Interrupt();
00899 //   
00900 //        // signal that a new command arrived
00901 //        newDataAvailable = true;
00902 //        commandParsedByIRQ = true;    
00903 //   
00904 //        // switch off LEDs
00905 //        BoardController::powerOnLED (LED_LEARN, false);
00906 //        BoardController::powerOnLED (LED_WALK, false);
00907 //        BoardController::powerOnLED (LED_EXPLORATION, false);
00908 //   
00909 //        // receive command
00910 //        if(!v24.receiveCommand ( &rx_command )) return;
00911 //   
00912 //        // send immediate answer for some commands
00913 //        if (rx_command.command == V24_COMMAND_GET_CURRENTPOSITION_MODE_ITERATIONS ||
00914 //        rx_command.command == V24_COMMAND_GET_GRIDSIZE ||
00915 //        rx_command.command == V24_COMMAND_GET_GAMMA ) {
00916 //   
00917 //             // signal that command has already been processed
00918 //             newDataAvailable = false;
00919 //             
00920 //             // process command
00921 //             v24.processCommand();
00922 //   
00923 //             // enable interrupts
00924 //             BoardController::enableV24Interrupt();
00925 //             sei();
00926 //        }
00927 //   }
00928 //}
00929 
00930 
Generated on Fri Oct 8 17:10:07 2010 for Crawling Robot Microcontroller Software by  doxygen 1.6.3