00001 #include "V24Control.h"
00002
00006 extern V24Control v24;
00007 V24COMMAND tx_command, rx_command;
00008 volatile bool newDataAvailable = false;
00009
00010
00014 bool V24Control::newDataReceived() {
00015
00016 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00017
00018
00019 this->vi.ctr.enableV24Interrupt();
00020
00021
00022
00023
00024
00025
00026
00027
00028 return (UCSRA & (1<<RXC));
00029 }
00030
00031 return false;
00032 }
00033
00037 void V24Control::init () {
00038
00039
00040
00041 vi.init();
00042
00043 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00044
00045
00046 PORTD |= (1<<PD0);
00047 PORTD &= ~(1<<PD1);
00048 PORTC |= (1<<PC2);
00049 DDRC &= ~(1<<PC2);
00050 DDRC |= (1<<PC3);
00051 PORTC &= ~(1<<PC3);
00052
00053 UBRRH = (unsigned char) (MYUBRR>>8);
00054 UBRRL = (unsigned char) MYUBRR;
00055
00056
00057 UCSRB = (1<<RXEN) | (1<<TXEN);
00058
00059
00060 UCSRC = (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0);
00061
00062
00063 while ((UCSRA & (1<<RXC)));
00064
00065
00066 vi.ctr.enableV24Interrupt();
00067 newDataAvailable = false;
00068
00069
00070 rxtx = true;
00071 }
00072 }
00073
00074
00078 void V24Control::transmit (unsigned char data) {
00079
00080 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00081
00082
00083 while (!(UCSRA & (1<<UDRE)));
00084
00085
00086 while (PINC & (1<<PC2));
00087
00088
00089 UDR = data;
00090
00091
00092
00093 }
00094 }
00095
00096
00100 bool V24Control::receive (unsigned char *data) {
00101
00102 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00103
00104 vi.ctr.powerOnLED(LED_POWER, false);
00105 unsigned char rx;
00106 int errorLoop = 0;
00107
00108 while (!(UCSRA & (1<<RXC)) && errorLoop <100) {
00109 errorLoop++;
00110 vi.ctr.msDelay(1);
00111 }
00112
00113
00114 if (errorLoop==100) {
00115 vi.ctr.powerOnLED(LED_POWER, true);
00116
00117 return false;
00118 } else {
00119
00120 *data = UDR;
00121
00122 vi.ctr.powerOnLED(LED_POWER, true);
00123 return true;
00124 }
00125 }
00126 }
00127
00131 bool V24Control::transmitCommand (V24COMMAND *c) {
00132
00133 unsigned char loopCount;
00134
00135 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00136
00137
00138 unsigned char tmp;
00139
00140
00141 transmit(c->command);
00142
00143
00144
00145
00146
00147
00148 transmit(c->dataLength);
00149
00150
00151
00152
00153
00154
00155 for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00156
00157 transmit(c->data[loopCount]);
00158
00159
00160
00161
00162 }
00163
00164 return true;
00165 }
00166 }
00167
00171 bool V24Control::receiveCommand (V24COMMAND *c) {
00172
00173 unsigned char loopCount, rData, data;
00174 bool sync = false;
00175 char i;
00176
00177 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00178
00179
00180 rData = !V24_COMMAND_SYNC_COMMAND;
00181 do {
00182 if (receive(&rData) && rData==V24_COMMAND_SYNC_COMMAND) {
00183 transmit(V24_COMMAND_SYNC_OK_COMMAND);
00184 sync = true;
00185 }
00186 } while (!sync);
00187
00188
00189 sync = false;
00190 do {
00191 if (receive(&c->command)) {
00192 transmit(c->command);
00193 sync = true;
00194 }
00195 } while (!sync);
00196
00197
00198 sync = false;
00199 do {
00200 if (receive(&c->dataLength)) {
00201 transmit(c->dataLength);
00202 sync = true;
00203 }
00204 } while (!sync);
00205
00206
00207
00208 for (loopCount=0; loopCount < c->dataLength; loopCount++) {
00209 sync = false;
00210 do {
00211 if (receive(&c->data[loopCount])) {
00212 transmit(c->data[loopCount]);
00213 sync = true;
00214 }
00215 } while (!sync);
00216 }
00217
00218 return true;
00219 }
00220 }
00221
00222
00226 void V24Control::processCommand() {
00227
00228 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00229
00230
00231 cli();
00232 this->vi.ctr.disableV24Interrupt();
00233
00234 char i=0;
00235 char cmd;
00236 unsigned char* bytes;
00237 int _row, _column;
00238 V24COMMAND tmp_command;
00239
00240
00241 vi.ctr.powerOnLED (LED_LEARN, false);
00242 vi.ctr.powerOnLED (LED_WALK, false);
00243 vi.ctr.powerOnLED (LED_EXPLORATION, false);
00244
00245
00246 vi.ctr.autoStart = false;
00247
00248
00249
00250 if (!this->receiveCommand ( &rx_command )) {
00251 vi.ctr.newMainMode = MAIN_MODE_STOP;
00252 vi.ctr.msDelay(1000);
00253 this->flushReceiveBuffer();
00254 this->vi.ctr.enableV24Interrupt();
00255 return;
00256 }
00257
00258
00259
00260
00261 cmd = rx_command.command;
00262
00263
00264
00265
00266 if ( cmd == V24_COMMAND_START_WALKING ) {
00267
00268
00269 vi.ctr.gotoStateWithoutSavingFeedback(0, 0);
00270 vi.ctr.newMainMode = MAIN_MODE_WALK_POLICY;
00271 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00272
00273
00274
00275
00276 } else if ( cmd == V24_COMMAND_STOP_WALKING ) {
00277
00278 vi.ctr.newMainMode = MAIN_MODE_STOP;
00279 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00280
00281
00282
00283
00284 } else if ( cmd == V24_COMMAND_GET_GAMMA ) {
00285
00286 processGetGamma();
00287 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00288
00289
00290
00291
00292 } else if ( cmd == V24_COMMAND_SET_GAMMA ) {
00293
00294 processSetGamma(&rx_command);
00295 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00296
00297
00298
00299
00300 } else if ( cmd == V24_COMMAND_SET_GRIDSIZE ) {
00301
00302 processSetGridsize(&rx_command);
00303
00304
00305
00306
00307 } else if ( cmd == V24_COMMAND_GET_GRIDSIZE ) {
00308
00309 processGetGridsize();
00310 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00311
00312
00313
00314
00315 } else if ( cmd == V24_COMMAND_SET_STATE ) {
00316
00317 processSetState(&rx_command);
00318
00319
00320
00321
00322 } else if ( cmd == V24_COMMAND_GET_STATE ) {
00323
00324 processGetState(&rx_command);
00325
00326
00327
00328
00329 } else if ( cmd == V24_COMMAND_GET_EXPLORATION_CONFIG ) {
00330
00331 processGetExplorationConfig();
00332
00333
00334
00335
00336 } else if ( cmd == V24_COMMAND_GET_DEBUG ) {
00337
00338 processGetDebug();
00339
00340
00341
00342
00343 } else if ( cmd == V24_COMMAND_WRITE_RAM_BYTES ) {
00344
00345 vi.ctr.mainMode = vi.ctr.newMainMode = MAIN_MODE_STOP;
00346
00347
00348 vi.ctr.states.ramtestSaveBytes(
00349 ((rx_command.data[0]*256) + rx_command.data[1]),
00350 &(rx_command.data[2]),
00351 rx_command.dataLength-2
00352
00353 );
00354 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00355
00356
00357
00358
00359 } else if ( cmd == V24_COMMAND_READ_RAM_BYTES ) {
00360
00361
00362 vi.ctr.mainMode = vi.ctr.newMainMode = MAIN_MODE_STOP;
00363
00364
00365 tmp_command.command = V24_COMMAND_WRITE_RAM_BYTES;
00366 tmp_command.dataLength = rx_command.data[2]+2;
00367 tmp_command.data[0] = rx_command.data[0];
00368 tmp_command.data[1] = rx_command.data[1];
00369
00370
00371 vi.ctr.states.ramtestReadBytes(
00372 ((rx_command.data[0]*256) + rx_command.data[1]),
00373 &(tmp_command.data[2]),
00374 rx_command.data[2]
00375 );
00376
00377
00378 if (!v24.transmitCommand (&tmp_command)) return;
00379 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00380
00381
00382
00383
00384 } else if ( cmd == V24_COMMAND_EXPLORE_FEEDBACK ) {
00385
00386 vi.ctr.newMainMode = MAIN_MODE_EXPLORE_FEEDBACK;
00387 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00388
00389
00390
00391
00392 } else if ( cmd == V24_COMMAND_START_LEARNING ) {
00393
00394 vi.ctr.newMainMode = MAIN_MODE_LEARN;
00395 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00396
00397
00398
00399
00400 } else if (cmd == V24_COMMAND_GET_DISTANCE ) {
00401
00402 signed long int _distance = vi.getDistanceCovered();
00403 unsigned char* bytes = (unsigned char*)&_distance;
00404
00405 tx_command.command = V24_COMMAND_SET_DISTANCE;
00406 tx_command.dataLength = 0x04;
00407 tx_command.data[0] = bytes[0];
00408 tx_command.data[1] = bytes[1];
00409 tx_command.data[2] = bytes[2];
00410 tx_command.data[3] = bytes[3];
00411
00412 if (!v24.transmitCommand (&tx_command)) return;
00413 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00414
00415
00416
00417
00418 } else if (cmd == V24_COMMAND_RESET_DISTANCE_COUNTER ) {
00419
00420 vi.resetDistanceCounter();
00421 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00422
00423
00424
00425
00426 } else if (cmd == V24_COMMAND_SET_EXPLORATION_CONFIG ) {
00427
00428 unsigned char* bytes;
00429 register char i;
00430
00431
00432 bytes = (unsigned char*)&vi.expParameter;
00433 for (i=0; i < sizeof(vi.expParameter); i++) {
00434 bytes[i] = rx_command.data[i];
00435 }
00436
00437
00438 bytes = (unsigned char*)&vi.expMode;
00439 bytes[0] = rx_command.data[sizeof(vi.expParameter)];
00440
00441 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00442
00443
00444
00445
00446 } else if (cmd == V24_COMMAND_GET_CURRENTPOSITION_MODE_ITERATIONS ) {
00447
00448 unsigned long int _itCount = vi.getIterationCount();
00449 unsigned char* bytes = (unsigned char*)&_itCount;
00450 signed char wert;
00451
00452 tx_command.command = V24_COMMAND_SET_CURRENTPOSITION_MODE_ITERATIONS;
00453 tx_command.dataLength = 0x09;
00454 tx_command.data[0] = vi.ctr.currentY;
00455 tx_command.data[1] = vi.ctr.currentX;
00456 tx_command.data[2] = (unsigned char)vi.ctr.mainMode;
00457 tx_command.data[3] = bytes[0];
00458 tx_command.data[4] = bytes[1];
00459 tx_command.data[5] = bytes[2];
00460 tx_command.data[6] = bytes[3];
00461 wert = 127;
00462 tx_command.data[7] = *(unsigned char*)&wert;
00463 wert = -127;
00464 tx_command.data[8] = *(unsigned char*)&wert;
00465
00466
00467 if (!v24.transmitCommand (&tx_command)) return;
00468 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00469
00470
00471
00472
00473 } else if (cmd == V24_COMMAND_SET_ALL_STATES_AND_GAMMA ) {
00474
00475
00476 if (!this->receiveCommand(&tmp_command)) return;
00477 processSetGamma(&tmp_command);
00478 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00479
00480
00481 if (!this->receiveCommand(&tmp_command)) return;
00482 processSetGridsize(&tmp_command);
00483
00484 for (_row=0; _row < vi.ctr.states.scalY; _row++) {
00485 for (_column=0; _column < vi.ctr.states.scalX; _column++) {
00486
00487
00488 if (!this->receiveCommand (&tmp_command)) return;
00489 processSetState(&tmp_command);
00490 }
00491 }
00492
00493
00494
00495
00496 } else if (cmd == V24_COMMAND_GET_ALL_STATES_AND_GAMMA ) {
00497
00498
00499 if (!this->receiveCommand(&tmp_command)) return;
00500 processGetGamma();
00501 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00502
00503
00504 if (!this->receiveCommand(&tmp_command)) return;
00505 processGetGridsize();
00506 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00507
00508 for (_row=0; _row < vi.ctr.states.scalY; _row++) {
00509 for (_column=0; _column < vi.ctr.states.scalX; _column++) {
00510
00511
00512 if (!this->receiveCommand (&tmp_command)) return;
00513
00514
00515 processGetState(&tmp_command);
00516 }
00517 }
00518
00519
00520 } else if (cmd == V24_COMMAND_WALK_AND_RETURN ) {
00521
00522
00523 tx_command.command = V24_COMMAND_WALK_AND_RETURN;
00524 tx_command.dataLength = 1;
00525 tx_command.data[0] = vi.moveAndSaveFeedback(rx_command.data[0], 0);
00526
00527 if (!v24.transmitCommand (&tx_command)) return;
00528 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return;
00529
00530
00531
00532
00533 } else {
00534 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_UNKNOWN_COMMAND)) return;
00535 }
00536
00537
00538
00539 this->vi.ctr.enableV24Interrupt();
00540 }
00541 }
00542
00546 bool V24Control::transmitReceiveCommandProcessed(unsigned char _code, unsigned char _error) {
00547
00548 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568 }
00569 return true;
00570 }
00571
00575 void V24Control::flushReceiveBuffer() {
00576 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00577 unsigned char dummy;
00578 while (UCSRA & (1<<RXC)) dummy = UDR;
00579 }
00580 }
00581
00582
00586 bool V24Control::processGetExplorationConfig() {
00587
00588 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00589
00590 V24COMMAND tx_command;
00591 unsigned char* bytes;
00592 register char i;
00593
00594 tx_command.command = V24_COMMAND_SET_EXPLORATION_CONFIG;
00595 tx_command.dataLength = 5;
00596
00597
00598 bytes = (unsigned char*)&(vi.expParameter);
00599 for (i=0; i < sizeof(vi.expParameter); i++) {
00600 tx_command.data[i] = bytes[i];
00601 }
00602
00603
00604 tx_command.data[4] = vi.expMode;
00605
00606
00607 if (!transmitCommand (&tx_command)) return false;
00608 return true;
00609 }
00610 }
00611
00612
00616 bool V24Control::processGetGamma() {
00617
00618 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00619
00620 V24COMMAND tx_command;
00621 unsigned char* bytes;
00622 register char i;
00623
00624 tx_command.command = V24_COMMAND_SET_GAMMA;
00625 tx_command.dataLength = 4;
00626 bytes = (unsigned char*)&(vi.gamma);
00627
00628 for (i=0; i < tx_command.dataLength; i++) {
00629 tx_command.data[i] = bytes[i];
00630 }
00631
00632 if (!transmitCommand (&tx_command)) return false;
00633 return true;
00634 }
00635 }
00636
00640 void V24Control::processSetGamma(V24COMMAND *cmd) {
00641
00642 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00643
00644 unsigned char* bytes;
00645 register char i;
00646
00647 bytes = (unsigned char*)&vi.gamma;
00648
00649 for (i=0; i < 4; i++) {
00650 bytes[i] = cmd->data[i];
00651 }
00652 }
00653 }
00654
00658 bool V24Control::processGetDebug() {
00659
00660 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00661
00662 V24COMMAND tx_command;
00663
00664 tx_command.command = V24_COMMAND_GET_DEBUG;
00665 tx_command.dataLength = 10;
00666
00667 register char i;
00668
00669 unsigned char* bytes = (unsigned char*)&(vi.currentExpProbability);
00670 for (i=0; i < tx_command.dataLength; i++) {
00671 tx_command.data[i] = bytes[i];
00672 }
00673
00674 for (i=4; i<tx_command.dataLength; i++) {
00675 tx_command.data[i] = rand()%(NUM_ACTIONS);
00676 srand (rand()+1);
00677 }
00678
00679 if (!transmitCommand (&tx_command)) return false;
00680 return true;
00681 }
00682 }
00683
00684
00688 bool V24Control::processGetGridsize() {
00689
00690 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00691
00692 V24COMMAND tx_command;
00693
00694 tx_command.command = V24_COMMAND_SET_GRIDSIZE;
00695 tx_command.dataLength = 2;
00696 tx_command.data[0] = vi.ctr.states.scalY;
00697 tx_command.data[1] = vi.ctr.states.scalX;
00698
00699 if (!transmitCommand (&tx_command)) return false;
00700 return true;
00701 }
00702 }
00703
00707 bool V24Control::processSetGridsize(V24COMMAND *cmd) {
00708
00709 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00710
00711 unsigned char _scalX, _scalY;
00712
00713 _scalY = cmd->data[0];
00714 _scalX = cmd->data[1];
00715
00716
00717 if ((_scalX*_scalY) > (MAX_STATES)) {
00718 if (!transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00719 return false;
00720
00721 } else {
00722 vi.ctr.states.scalY = _scalY;
00723 vi.ctr.states.scalX = _scalX;
00724 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00725 return true;
00726 }
00727 }
00728 }
00729
00733 bool V24Control::processGetState(V24COMMAND *cmd) {
00734
00735 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00736
00737 V24COMMAND tx_command;
00738 unsigned char _row = cmd->data[0];
00739 unsigned char _column = cmd->data[1];
00740
00741 if (_row >= vi.ctr.states.scalY || _column >= vi.ctr.states.scalX) {
00742 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00743 return false;
00744 } else {
00745
00746
00747 vi.ctr.states.getState(_row, _column, &tmpState);
00748 unsigned char* bytes = (unsigned char*)&tmpState;
00749
00750 tx_command.command = V24_COMMAND_SET_STATE;
00751 tx_command.dataLength = STATESIZE+2;
00752 tx_command.data[0] = _row;
00753 tx_command.data[1] = _column;
00754
00755 for (unsigned char i=0; i < STATESIZE; i++) {
00756 tx_command.data[2 + i] = bytes[i];
00757 }
00758
00759 if (!v24.transmitCommand (&tx_command)) return false;
00760 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00761 return true;
00762 }
00763 }
00764 }
00765
00769 bool V24Control::processSetState(V24COMMAND *cmd) {
00770
00771 if (vi.ctr.getSwitchState(SWITCH_V24)) {
00772
00773 unsigned char _row = cmd->data[0];
00774 unsigned char _column = cmd->data[1];
00775
00776 unsigned char* bytes = (unsigned char*)&tmpState;
00777
00778
00779 if (_row >= vi.ctr.states.scalY || _column >= vi.ctr.states.scalX) {
00780 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_ERROR, V24_ERRORCODE_GRIDSIZE_TOO_LARGE)) return false;
00781 return false;
00782 } else {
00783
00784 for (unsigned char i=0; i<STATESIZE; i++) {
00785 bytes[i] = cmd->data[i + 2];
00786 }
00787
00788
00789 vi.ctr.states.setState (_row, _column, &tmpState);
00790 if (!v24.transmitReceiveCommandProcessed (V24_COMMAND_COMMAND_OK, 0)) return false;
00791 return true;
00792 }
00793 }
00794 }
00795
00796
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842