00001 #include "V24Control.h"
00002
00006
00007
00008
00009
00013 V24Control::V24Control() {
00014
00015
00016
00017
00018
00019
00020 this->vi = new ValueIteration(new ENV_POS(5,5));
00021
00022 }
00023
00024
00026 V24Control::~V24Control() {
00027 delete this->vi;
00028 }
00029
00033 bool V24Control::newDataReceived() {
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 return (UCSRA & (1<<RXC));
00048
00049
00050
00051 }
00052
00056 void V24Control::init () {
00057
00058
00059 rxtx = false;
00060
00061
00062 BoardController::enableSerialV24Interface();
00063 }
00064
00065
00069 void V24Control::transmit (unsigned char data) {
00070
00071
00072
00073
00074
00075
00076 while (!(UCSRA & (1<<UDRE)));
00077
00078
00079 while (PINC & (1<<CTS));
00080
00081
00082 UDR = data;
00083
00084
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
00098
00099 BoardController::powerOnLED(LED_POWER, false);
00100
00101 int errorLoop = 0;
00102
00103
00104
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
00116
00117
00118
00119
00120
00121
00122
00123 *data = UDR;
00124 BoardController::powerOnLED(LED_POWER, true);
00125
00126
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
00147
00148
00149
00150
00151
00152
00153
00154
00155 transmit(c->command);
00156
00157
00158
00159
00160
00161
00162 transmit(c->dataLength);
00163
00164
00165
00166
00167
00168
00169 for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00170
00171 transmit(c->data[loopCount]);
00172
00173
00174
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
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203 sync = false;
00204 do {
00205 if (receive(&c->command)) {
00206
00207 sync = true;
00208 }
00209 } while (!sync);
00210
00211
00212 sync = false;
00213 do {
00214 if (receive(&c->dataLength)) {
00215
00216 sync = true;
00217 }
00218 } while (!sync);
00219
00220
00221
00222 for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00223 sync = false;
00224 do {
00225 if (receive(&c->data[loopCount])) {
00226
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
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
00255 BoardController::powerOnLED (LED_LEARN, false);
00256 BoardController::powerOnLED (LED_WALK, false);
00257 BoardController::powerOnLED (LED_EXPLORATION, false);
00258
00259
00260
00261
00262
00263
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
00272
00273
00274
00275 cmd = rx_command.command;
00276
00277
00278
00279
00280 if ( cmd == V24_COMMAND_START_WALKING ) {
00281
00282
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
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
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
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
00314
00315 } else if ( cmd == V24_COMMAND_SET_GRIDSIZE ) {
00316
00317 processSetGridsize(&rx_command);
00318
00319
00320
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
00329
00330 } else if ( cmd == V24_COMMAND_SET_STATE ) {
00331
00332 processSetState(&rx_command);
00333
00334
00335
00336
00337 } else if ( cmd == V24_COMMAND_GET_STATE ) {
00338
00339 processGetState(&rx_command);
00340
00341
00342
00343
00344 } else if ( cmd == V24_COMMAND_GET_EXPLORATION_CONFIG ) {
00345
00346 processGetExplorationConfig();
00347
00348
00349
00350
00351 } else if ( cmd == V24_COMMAND_GET_DEBUG ) {
00352
00353 processGetDebug();
00354
00355
00356
00357
00358 } else if ( cmd == V24_COMMAND_WRITE_RAM_BYTES ) {
00359
00360 this->newMainMode = MAIN_MODE_STOP;
00361
00362
00363 vi->states->ramtestSaveBytes(
00364 ((rx_command.data[0]*256) + rx_command.data[1]),
00365 &(rx_command.data[2]),
00366 rx_command.dataLength-2
00367
00368 );
00369 if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00370
00371
00372
00373
00374 } else if ( cmd == V24_COMMAND_READ_RAM_BYTES ) {
00375
00376
00377 this->newMainMode = MAIN_MODE_STOP;
00378
00379
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
00386 vi->states->ramtestReadBytes(
00387 ((rx_command.data[0]*256) + rx_command.data[1]),
00388 &(tmp_command.data[2]),
00389 rx_command.data[2]
00390 );
00391
00392
00393 if (!this->transmitCommand (&tmp_command)) return;
00394 if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00395
00396
00397
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
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
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
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
00440
00441 } else if (cmd == V24_COMMAND_SET_EXPLORATION_CONFIG ) {
00442
00443 unsigned char* bytes;
00444 register char i;
00445
00446
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
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
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
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
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
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
00567
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
00574
00575
00576
00577
00578 if (!this->transmitCommand (&tx_command)) return;
00579
00580
00581 this->setMainMode(MAIN_MODE_STOP);
00582
00583
00584
00585
00586 } else {
00587 if (!this->transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_UNKNOWN_COMMAND)) return;
00588 }
00589
00590
00591
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
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
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
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
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
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
00780
00781
00782
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
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
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
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
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
00930