Randal Riedinger / ROBOT

RETRO ROBOT E

ROBOT.cpp@0:4837c9695a02, 2015-03-02 (annotated)

Committer:
RLRiedinger
Date:
Mon Mar 02 04:20:45 2015 +0000
Revision:
0:4837c9695a02
RETRO ROBOT E

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RLRiedinger 0:4837c9695a02 1 #include "ROBOT.h"
RLRiedinger 0:4837c9695a02 2 #define AUTO_START
RLRiedinger 0:4837c9695a02 3
RLRiedinger 0:4837c9695a02 4 const char* ROBOT::LOSE_1 = "";
RLRiedinger 0:4837c9695a02 5 const char* ROBOT::LOSE_2 = "";
RLRiedinger 0:4837c9695a02 6 const char* ROBOT::SPLASH_1 = "";
RLRiedinger 0:4837c9695a02 7 const char* ROBOT::SPLASH_2 = "";
RLRiedinger 0:4837c9695a02 8 const char* ROBOT::SSID = "XXXXXXXX";
RLRiedinger 0:4837c9695a02 9 const char* ROBOT::PASSWORD = "XXXXXXXX";
RLRiedinger 0:4837c9695a02 10
RLRiedinger 0:4837c9695a02 11 struct match {
RLRiedinger 0:4837c9695a02 12
RLRiedinger 0:4837c9695a02 13 char *RESPONSE;
RLRiedinger 0:4837c9695a02 14 char *pRESPONSE;
RLRiedinger 0:4837c9695a02 15
RLRiedinger 0:4837c9695a02 16 } matchtab[] = {
RLRiedinger 0:4837c9695a02 17
RLRiedinger 0:4837c9695a02 18 { "+IPD," },
RLRiedinger 0:4837c9695a02 19 { "\r\n> " },
RLRiedinger 0:4837c9695a02 20 { "busy s..." },
RLRiedinger 0:4837c9695a02 21 { "OK\r\n" },
RLRiedinger 0:4837c9695a02 22 { "+ERROR\r\n" },
RLRiedinger 0:4837c9695a02 23
RLRiedinger 0:4837c9695a02 24 };
RLRiedinger 0:4837c9695a02 25
RLRiedinger 0:4837c9695a02 26 ROBOT::ROBOT() : left(P0_14, PullUp), right(P0_11, PullUp),
RLRiedinger 0:4837c9695a02 27 down(P0_12, PullUp), up(P0_13, PullUp),
RLRiedinger 0:4837c9695a02 28 square(P0_16, PullUp), circle(P0_1, PullUp),
RLRiedinger 0:4837c9695a02 29 led1(P0_9), led2(P0_8),
RLRiedinger 0:4837c9695a02 30 pwm(P0_18),
RLRiedinger 0:4837c9695a02 31 i2c(D_SDA, D_SCL),
RLRiedinger 0:4837c9695a02 32 serial_bridge1(&i2c, SC16IS750_SA0),
RLRiedinger 0:4837c9695a02 33 serial_bridge2(&i2c, SC16IS750_SA1),
RLRiedinger 0:4837c9695a02 34 esp8266_rst(ESP8266_RST) {
RLRiedinger 0:4837c9695a02 35
RLRiedinger 0:4837c9695a02 36 pIPD_BUFFER = IPD_BUFFER;
RLRiedinger 0:4837c9695a02 37
RLRiedinger 0:4837c9695a02 38 DEBUG = 0;
RLRiedinger 0:4837c9695a02 39
RLRiedinger 0:4837c9695a02 40 heartbeatflag = false;
RLRiedinger 0:4837c9695a02 41 Address_T = "192.168.0.32";
RLRiedinger 0:4837c9695a02 42 Address = Address_T;
RLRiedinger 0:4837c9695a02 43
RLRiedinger 0:4837c9695a02 44 #ifdef AUTO_START
RLRiedinger 0:4837c9695a02 45 echo_comands = 0;
RLRiedinger 0:4837c9695a02 46 #else
RLRiedinger 0:4837c9695a02 47 echo_comands = 0;
RLRiedinger 0:4837c9695a02 48 #endif
RLRiedinger 0:4837c9695a02 49
RLRiedinger 0:4837c9695a02 50 mPROMPT = 0;
RLRiedinger 0:4837c9695a02 51 mIPD = 0;
RLRiedinger 0:4837c9695a02 52 mCOUNT = 0;
RLRiedinger 0:4837c9695a02 53 COUNT = 0;
RLRiedinger 0:4837c9695a02 54 uCOUNT = 0;
RLRiedinger 0:4837c9695a02 55 mCOLON = 0;
RLRiedinger 0:4837c9695a02 56
RLRiedinger 0:4837c9695a02 57 this->serial_bridge1.baud(14745600UL, 9600);
RLRiedinger 0:4837c9695a02 58 this->serial_bridge2.baud(14745600UL, 19200);
RLRiedinger 0:4837c9695a02 59
RLRiedinger 0:4837c9695a02 60 this->serial_bridge1.format(8, Serial::None, 1);
RLRiedinger 0:4837c9695a02 61 this->serial_bridge2.format(8, Serial::None, 1);
RLRiedinger 0:4837c9695a02 62
RLRiedinger 0:4837c9695a02 63 this->serial_bridge1.set_fifo_control();
RLRiedinger 0:4837c9695a02 64 this->serial_bridge2.set_fifo_control();
RLRiedinger 0:4837c9695a02 65
RLRiedinger 0:4837c9695a02 66 this->serial_bridge1.flush();
RLRiedinger 0:4837c9695a02 67 this->serial_bridge2.flush();
RLRiedinger 0:4837c9695a02 68
RLRiedinger 0:4837c9695a02 69 this->serial_bridge1.set_modem_control();
RLRiedinger 0:4837c9695a02 70 this->serial_bridge2.set_modem_control();
RLRiedinger 0:4837c9695a02 71
RLRiedinger 0:4837c9695a02 72 this->serial_bridge1.set_flow_triggers();
RLRiedinger 0:4837c9695a02 73 this->serial_bridge2.set_flow_triggers();
RLRiedinger 0:4837c9695a02 74
RLRiedinger 0:4837c9695a02 75 this->ESP8266_reset();
RLRiedinger 0:4837c9695a02 76
RLRiedinger 0:4837c9695a02 77 srand(0);
RLRiedinger 0:4837c9695a02 78 this->lastUp = false;
RLRiedinger 0:4837c9695a02 79 this->lastDown = false;
RLRiedinger 0:4837c9695a02 80 this->mode = true;
RLRiedinger 0:4837c9695a02 81
RLRiedinger 0:4837c9695a02 82 this->colors[0] = DisplayN18::RED;
RLRiedinger 0:4837c9695a02 83 this->colors[1] = DisplayN18::GREEN;
RLRiedinger 0:4837c9695a02 84 this->colors[2] = DisplayN18::BLUE;
RLRiedinger 0:4837c9695a02 85
RLRiedinger 0:4837c9695a02 86 this->initialize();
RLRiedinger 0:4837c9695a02 87
RLRiedinger 0:4837c9695a02 88 }
RLRiedinger 0:4837c9695a02 89
RLRiedinger 0:4837c9695a02 90 void ROBOT::serial_print1(char * BUFFER) {
RLRiedinger 0:4837c9695a02 91
RLRiedinger 0:4837c9695a02 92 while (*BUFFER != '\0') {
RLRiedinger 0:4837c9695a02 93
RLRiedinger 0:4837c9695a02 94 serial_bridge1.putc((int) *BUFFER++);
RLRiedinger 0:4837c9695a02 95
RLRiedinger 0:4837c9695a02 96 }
RLRiedinger 0:4837c9695a02 97
RLRiedinger 0:4837c9695a02 98 }
RLRiedinger 0:4837c9695a02 99
RLRiedinger 0:4837c9695a02 100 void ROBOT::serial_print2(char * BUFFER) {
RLRiedinger 0:4837c9695a02 101
RLRiedinger 0:4837c9695a02 102 while (*BUFFER != '\0') {
RLRiedinger 0:4837c9695a02 103
RLRiedinger 0:4837c9695a02 104 serial_bridge2.putc((int) *BUFFER++);
RLRiedinger 0:4837c9695a02 105
RLRiedinger 0:4837c9695a02 106 }
RLRiedinger 0:4837c9695a02 107
RLRiedinger 0:4837c9695a02 108 }
RLRiedinger 0:4837c9695a02 109
RLRiedinger 0:4837c9695a02 110 void ROBOT::debug_print(unsigned int line, const char* function) {
RLRiedinger 0:4837c9695a02 111
RLRiedinger 0:4837c9695a02 112 if (DEBUG) {
RLRiedinger 0:4837c9695a02 113
RLRiedinger 0:4837c9695a02 114 sprintf(UART_BUFFER, "%6d %s\r\n", line, function);
RLRiedinger 0:4837c9695a02 115 this->serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 116
RLRiedinger 0:4837c9695a02 117 }
RLRiedinger 0:4837c9695a02 118
RLRiedinger 0:4837c9695a02 119 }
RLRiedinger 0:4837c9695a02 120
RLRiedinger 0:4837c9695a02 121 void ROBOT::debug_print_string(unsigned int line, const char* function, char * pBUFFER) {
RLRiedinger 0:4837c9695a02 122
RLRiedinger 0:4837c9695a02 123 if (DEBUG) {
RLRiedinger 0:4837c9695a02 124
RLRiedinger 0:4837c9695a02 125 sprintf(UART_BUFFER, "%6d %s [%s]\r\n", line, function, pBUFFER);
RLRiedinger 0:4837c9695a02 126 this->serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 127
RLRiedinger 0:4837c9695a02 128 }
RLRiedinger 0:4837c9695a02 129
RLRiedinger 0:4837c9695a02 130 }
RLRiedinger 0:4837c9695a02 131
RLRiedinger 0:4837c9695a02 132 void ROBOT::debug_print2(unsigned int line, const char* function) {
RLRiedinger 0:4837c9695a02 133
RLRiedinger 0:4837c9695a02 134 if (DEBUG) {
RLRiedinger 0:4837c9695a02 135
RLRiedinger 0:4837c9695a02 136 sprintf(UART_BUFFER, "%6d %s\r\n", line, function);
RLRiedinger 0:4837c9695a02 137 this->serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 138
RLRiedinger 0:4837c9695a02 139 }
RLRiedinger 0:4837c9695a02 140
RLRiedinger 0:4837c9695a02 141 }
RLRiedinger 0:4837c9695a02 142
RLRiedinger 0:4837c9695a02 143 void ROBOT::debug_print_string2(unsigned int line, const char* function, char * pBUFFER) {
RLRiedinger 0:4837c9695a02 144
RLRiedinger 0:4837c9695a02 145 if (DEBUG) {
RLRiedinger 0:4837c9695a02 146
RLRiedinger 0:4837c9695a02 147 sprintf(UART_BUFFER, "%6d %s [%s]\r\n", line, function, pBUFFER);
RLRiedinger 0:4837c9695a02 148 this->serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 149
RLRiedinger 0:4837c9695a02 150 }
RLRiedinger 0:4837c9695a02 151
RLRiedinger 0:4837c9695a02 152 }
RLRiedinger 0:4837c9695a02 153
RLRiedinger 0:4837c9695a02 154 void ROBOT::show_menu(void) {
RLRiedinger 0:4837c9695a02 155
RLRiedinger 0:4837c9695a02 156 serial_print2("0: Exit\n\r");
RLRiedinger 0:4837c9695a02 157 serial_print2("1: Show Menu\n\r");
RLRiedinger 0:4837c9695a02 158 serial_print2("2: Init\n\r");
RLRiedinger 0:4837c9695a02 159 serial_print2("3: IO Port Out\n\r");
RLRiedinger 0:4837c9695a02 160 serial_print2("4: Transparant mode\n\r");
RLRiedinger 0:4837c9695a02 161 serial_print2("5: Free bufferspace\n\r");
RLRiedinger 0:4837c9695a02 162
RLRiedinger 0:4837c9695a02 163 #if (0)
RLRiedinger 0:4837c9695a02 164 serial_print2("6: Enable RTS/CTS\n\r");
RLRiedinger 0:4837c9695a02 165 serial_print2("7: Disable RTS/CTS\n\r");
RLRiedinger 0:4837c9695a02 166 serial_print2("8: Write block\n\r");
RLRiedinger 0:4837c9695a02 167 serial_print2("9: Baudrate 9600\n\r");
RLRiedinger 0:4837c9695a02 168 serial_print2("A: Baudrate 115200\n\r");
RLRiedinger 0:4837c9695a02 169 serial_print2("B: Transparant mode with buffer display\n\r");
RLRiedinger 0:4837c9695a02 170 #endif
RLRiedinger 0:4837c9695a02 171
RLRiedinger 0:4837c9695a02 172 #if (0)
RLRiedinger 0:4837c9695a02 173 serial_print2("C: Test printf\n\r");
RLRiedinger 0:4837c9695a02 174 #endif
RLRiedinger 0:4837c9695a02 175
RLRiedinger 0:4837c9695a02 176 serial_print2("D: Test connected\n\r");
RLRiedinger 0:4837c9695a02 177
RLRiedinger 0:4837c9695a02 178 #if(0)
RLRiedinger 0:4837c9695a02 179 serial_print2("D: \n\r");
RLRiedinger 0:4837c9695a02 180 serial_print2("P: \n\r");
RLRiedinger 0:4837c9695a02 181 #endif
RLRiedinger 0:4837c9695a02 182
RLRiedinger 0:4837c9695a02 183 serial_print2("T: Test ESP8266 192.168.0.6\n\r");
RLRiedinger 0:4837c9695a02 184
RLRiedinger 0:4837c9695a02 185 #if (0)
RLRiedinger 0:4837c9695a02 186
RLRiedinger 0:4837c9695a02 187 serial_print2("U: Test ESP8266 192.168.0.14\n\r");
RLRiedinger 0:4837c9695a02 188 serial_print2("V: Test ESP8266 192.168.0.22\n\r");
RLRiedinger 0:4837c9695a02 189
RLRiedinger 0:4837c9695a02 190 #endif
RLRiedinger 0:4837c9695a02 191
RLRiedinger 0:4837c9695a02 192 serial_print2("W: Toggle ECHO mode\n\r");
RLRiedinger 0:4837c9695a02 193 serial_print2("Z: Toggle DEBUG mode\n\r");
RLRiedinger 0:4837c9695a02 194
RLRiedinger 0:4837c9695a02 195 serial_print2("\n\r");
RLRiedinger 0:4837c9695a02 196
RLRiedinger 0:4837c9695a02 197 while (serial_bridge2.writableCount() < 64);
RLRiedinger 0:4837c9695a02 198
RLRiedinger 0:4837c9695a02 199 First = 1;
RLRiedinger 0:4837c9695a02 200
RLRiedinger 0:4837c9695a02 201 }
RLRiedinger 0:4837c9695a02 202
RLRiedinger 0:4837c9695a02 203 void ROBOT::blink (DigitalOut LED) {
RLRiedinger 0:4837c9695a02 204
RLRiedinger 0:4837c9695a02 205 LED = 1;
RLRiedinger 0:4837c9695a02 206 wait(0.05);
RLRiedinger 0:4837c9695a02 207 LED = 0;
RLRiedinger 0:4837c9695a02 208
RLRiedinger 0:4837c9695a02 209 }
RLRiedinger 0:4837c9695a02 210
RLRiedinger 0:4837c9695a02 211 // Heartbeat monitor
RLRiedinger 0:4837c9695a02 212
RLRiedinger 0:4837c9695a02 213 void ROBOT::pulse(void) {
RLRiedinger 0:4837c9695a02 214
RLRiedinger 0:4837c9695a02 215 led2 = !led2;
RLRiedinger 0:4837c9695a02 216
RLRiedinger 0:4837c9695a02 217 }
RLRiedinger 0:4837c9695a02 218
RLRiedinger 0:4837c9695a02 219 void ROBOT::heartbeat_start(void) {
RLRiedinger 0:4837c9695a02 220
RLRiedinger 0:4837c9695a02 221 heartbeat.attach(this, &ROBOT::pulse, 0.5);
RLRiedinger 0:4837c9695a02 222
RLRiedinger 0:4837c9695a02 223 }
RLRiedinger 0:4837c9695a02 224
RLRiedinger 0:4837c9695a02 225 void ROBOT::heartbeat_stop(void) {
RLRiedinger 0:4837c9695a02 226
RLRiedinger 0:4837c9695a02 227 heartbeat.detach();
RLRiedinger 0:4837c9695a02 228
RLRiedinger 0:4837c9695a02 229 }
RLRiedinger 0:4837c9695a02 230
RLRiedinger 0:4837c9695a02 231 void ROBOT::ESP8266_reset(void) {
RLRiedinger 0:4837c9695a02 232
RLRiedinger 0:4837c9695a02 233 esp8266_rst = 0;
RLRiedinger 0:4837c9695a02 234 wait(0.1);
RLRiedinger 0:4837c9695a02 235 esp8266_rst = 1;
RLRiedinger 0:4837c9695a02 236 wait(1.0);
RLRiedinger 0:4837c9695a02 237
RLRiedinger 0:4837c9695a02 238 while (serial_bridge1.readable()) {
RLRiedinger 0:4837c9695a02 239
RLRiedinger 0:4837c9695a02 240 serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 241 wait(0.005);
RLRiedinger 0:4837c9695a02 242
RLRiedinger 0:4837c9695a02 243 }
RLRiedinger 0:4837c9695a02 244
RLRiedinger 0:4837c9695a02 245 }
RLRiedinger 0:4837c9695a02 246
RLRiedinger 0:4837c9695a02 247 void ROBOT::reset_matchtab(void) {
RLRiedinger 0:4837c9695a02 248
RLRiedinger 0:4837c9695a02 249 for (int i = 0; i < NMATCHES; i++) {
RLRiedinger 0:4837c9695a02 250
RLRiedinger 0:4837c9695a02 251 matchtab[i].pRESPONSE = matchtab[i].RESPONSE;
RLRiedinger 0:4837c9695a02 252
RLRiedinger 0:4837c9695a02 253 }
RLRiedinger 0:4837c9695a02 254
RLRiedinger 0:4837c9695a02 255 }
RLRiedinger 0:4837c9695a02 256
RLRiedinger 0:4837c9695a02 257 int ROBOT::match_RESPONSES(char c, struct match table[], int n) {
RLRiedinger 0:4837c9695a02 258
RLRiedinger 0:4837c9695a02 259 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 260
RLRiedinger 0:4837c9695a02 261 for (int i = 0; i < n; i++) {
RLRiedinger 0:4837c9695a02 262
RLRiedinger 0:4837c9695a02 263 if (c == *table[i].pRESPONSE) {
RLRiedinger 0:4837c9695a02 264
RLRiedinger 0:4837c9695a02 265 ++table[i].pRESPONSE;
RLRiedinger 0:4837c9695a02 266
RLRiedinger 0:4837c9695a02 267 if (*table[i].pRESPONSE == '\0') {
RLRiedinger 0:4837c9695a02 268
RLRiedinger 0:4837c9695a02 269 table[i].pRESPONSE = table[i].RESPONSE;
RLRiedinger 0:4837c9695a02 270
RLRiedinger 0:4837c9695a02 271 return(i + 1);
RLRiedinger 0:4837c9695a02 272
RLRiedinger 0:4837c9695a02 273 }
RLRiedinger 0:4837c9695a02 274
RLRiedinger 0:4837c9695a02 275 } else {
RLRiedinger 0:4837c9695a02 276
RLRiedinger 0:4837c9695a02 277 table[i].pRESPONSE = table[i].RESPONSE;
RLRiedinger 0:4837c9695a02 278
RLRiedinger 0:4837c9695a02 279 if (c == *table[i].pRESPONSE) {
RLRiedinger 0:4837c9695a02 280
RLRiedinger 0:4837c9695a02 281 ++table[i].pRESPONSE;
RLRiedinger 0:4837c9695a02 282
RLRiedinger 0:4837c9695a02 283 if (*table[i].pRESPONSE == '\0') {
RLRiedinger 0:4837c9695a02 284
RLRiedinger 0:4837c9695a02 285 table[i].pRESPONSE = table[i].RESPONSE;
RLRiedinger 0:4837c9695a02 286
RLRiedinger 0:4837c9695a02 287 return(i + 1);
RLRiedinger 0:4837c9695a02 288
RLRiedinger 0:4837c9695a02 289 }
RLRiedinger 0:4837c9695a02 290
RLRiedinger 0:4837c9695a02 291 }
RLRiedinger 0:4837c9695a02 292
RLRiedinger 0:4837c9695a02 293 }
RLRiedinger 0:4837c9695a02 294
RLRiedinger 0:4837c9695a02 295 }
RLRiedinger 0:4837c9695a02 296
RLRiedinger 0:4837c9695a02 297 return(0);
RLRiedinger 0:4837c9695a02 298
RLRiedinger 0:4837c9695a02 299 }
RLRiedinger 0:4837c9695a02 300
RLRiedinger 0:4837c9695a02 301 int ROBOT::echo(int show, int timeout) {
RLRiedinger 0:4837c9695a02 302
RLRiedinger 0:4837c9695a02 303 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 304
RLRiedinger 0:4837c9695a02 305 Timer ReceiveTimer;
RLRiedinger 0:4837c9695a02 306
RLRiedinger 0:4837c9695a02 307 char c = '\0';
RLRiedinger 0:4837c9695a02 308
RLRiedinger 0:4837c9695a02 309 // char IIR_STATUS = 0;
RLRiedinger 0:4837c9695a02 310
RLRiedinger 0:4837c9695a02 311 int return_code = 0;
RLRiedinger 0:4837c9695a02 312
RLRiedinger 0:4837c9695a02 313 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 314
RLRiedinger 0:4837c9695a02 315 ReceiveTimer.start();
RLRiedinger 0:4837c9695a02 316
RLRiedinger 0:4837c9695a02 317 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 318
RLRiedinger 0:4837c9695a02 319 while (!return_code) {
RLRiedinger 0:4837c9695a02 320
RLRiedinger 0:4837c9695a02 321 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 322
RLRiedinger 0:4837c9695a02 323 if (ReceiveTimer.read_ms() > timeout) {
RLRiedinger 0:4837c9695a02 324
RLRiedinger 0:4837c9695a02 325 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 326
RLRiedinger 0:4837c9695a02 327 return_code = -1;
RLRiedinger 0:4837c9695a02 328
RLRiedinger 0:4837c9695a02 329 }
RLRiedinger 0:4837c9695a02 330
RLRiedinger 0:4837c9695a02 331 // From Serial to SPI/I2C
RLRiedinger 0:4837c9695a02 332
RLRiedinger 0:4837c9695a02 333 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 334
RLRiedinger 0:4837c9695a02 335 while (!return_code && serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 336
RLRiedinger 0:4837c9695a02 337 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 338
RLRiedinger 0:4837c9695a02 339 c = serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 340
RLRiedinger 0:4837c9695a02 341 if (c == '#') {
RLRiedinger 0:4837c9695a02 342
RLRiedinger 0:4837c9695a02 343 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 344
RLRiedinger 0:4837c9695a02 345 return_code = QUIT;
RLRiedinger 0:4837c9695a02 346
RLRiedinger 0:4837c9695a02 347 } else {
RLRiedinger 0:4837c9695a02 348
RLRiedinger 0:4837c9695a02 349 serial_bridge1.putc(c);
RLRiedinger 0:4837c9695a02 350
RLRiedinger 0:4837c9695a02 351 }
RLRiedinger 0:4837c9695a02 352
RLRiedinger 0:4837c9695a02 353 }
RLRiedinger 0:4837c9695a02 354
RLRiedinger 0:4837c9695a02 355 #endif
RLRiedinger 0:4837c9695a02 356
RLRiedinger 0:4837c9695a02 357 // From SPI/I2C to serial
RLRiedinger 0:4837c9695a02 358
RLRiedinger 0:4837c9695a02 359 int readable1;
RLRiedinger 0:4837c9695a02 360
RLRiedinger 0:4837c9695a02 361 // int readable1 = serial_bridge1.readableCount();
RLRiedinger 0:4837c9695a02 362
RLRiedinger 0:4837c9695a02 363 // sprintf(FORMAT_BUFFER, "readable1 = %i", readable1);
RLRiedinger 0:4837c9695a02 364 // debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 365
RLRiedinger 0:4837c9695a02 366 while (!return_code && ((readable1 = serial_bridge1.readableCount()) > 0)) {
RLRiedinger 0:4837c9695a02 367
RLRiedinger 0:4837c9695a02 368 // sprintf(FORMAT_BUFFER, "readable1 = %i", readable1);
RLRiedinger 0:4837c9695a02 369 // debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 370
RLRiedinger 0:4837c9695a02 371 ReceiveTimer.reset();
RLRiedinger 0:4837c9695a02 372
RLRiedinger 0:4837c9695a02 373 c = serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 374
RLRiedinger 0:4837c9695a02 375 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 376
RLRiedinger 0:4837c9695a02 377 if (show) {
RLRiedinger 0:4837c9695a02 378
RLRiedinger 0:4837c9695a02 379 serial_bridge2.putc(c);
RLRiedinger 0:4837c9695a02 380
RLRiedinger 0:4837c9695a02 381 sprintf(UART_BUFFER, "%6d %s [%c]\r\n", __LINE__, __FUNCTION__, c);
RLRiedinger 0:4837c9695a02 382 serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 383
RLRiedinger 0:4837c9695a02 384 }
RLRiedinger 0:4837c9695a02 385
RLRiedinger 0:4837c9695a02 386 #endif
RLRiedinger 0:4837c9695a02 387
RLRiedinger 0:4837c9695a02 388 return_code = match_RESPONSES(c, matchtab, NMATCHES);
RLRiedinger 0:4837c9695a02 389
RLRiedinger 0:4837c9695a02 390 }
RLRiedinger 0:4837c9695a02 391
RLRiedinger 0:4837c9695a02 392 }
RLRiedinger 0:4837c9695a02 393
RLRiedinger 0:4837c9695a02 394 sprintf(FORMAT_BUFFER, "return_code = %i", return_code);
RLRiedinger 0:4837c9695a02 395 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 396
RLRiedinger 0:4837c9695a02 397 ReceiveTimer.stop();
RLRiedinger 0:4837c9695a02 398
RLRiedinger 0:4837c9695a02 399 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 400
RLRiedinger 0:4837c9695a02 401 return(return_code);
RLRiedinger 0:4837c9695a02 402
RLRiedinger 0:4837c9695a02 403 }
RLRiedinger 0:4837c9695a02 404
RLRiedinger 0:4837c9695a02 405 // Game Math: Faster Sine & Cosine with Polynomial Curves
RLRiedinger 0:4837c9695a02 406 // http://allenchou.net/2014/02/game-math-faster-sine-cosine-with-polynomial-curves/
RLRiedinger 0:4837c9695a02 407 // Posted on February 28, 2014 by Allen Chou
RLRiedinger 0:4837c9695a02 408
RLRiedinger 0:4837c9695a02 409 double ROBOT::Hill(double x) {
RLRiedinger 0:4837c9695a02 410
RLRiedinger 0:4837c9695a02 411 const float a0 = 1.0f;
RLRiedinger 0:4837c9695a02 412
RLRiedinger 0:4837c9695a02 413 const float a2 = 2.0f / PI - 12.0f / (PI * PI);
RLRiedinger 0:4837c9695a02 414
RLRiedinger 0:4837c9695a02 415 const float a3 = 16.0f / (PI * PI * PI) - 4.0f / (PI * PI);
RLRiedinger 0:4837c9695a02 416
RLRiedinger 0:4837c9695a02 417 const float xx = x * x;
RLRiedinger 0:4837c9695a02 418
RLRiedinger 0:4837c9695a02 419 const float xxx = xx * x;
RLRiedinger 0:4837c9695a02 420
RLRiedinger 0:4837c9695a02 421 return a0 + a2 * xx + a3 * xxx;
RLRiedinger 0:4837c9695a02 422
RLRiedinger 0:4837c9695a02 423 }
RLRiedinger 0:4837c9695a02 424
RLRiedinger 0:4837c9695a02 425 double ROBOT::FastSin(double x) {
RLRiedinger 0:4837c9695a02 426
RLRiedinger 0:4837c9695a02 427 // wrap x within [0, TWO_PI)
RLRiedinger 0:4837c9695a02 428
RLRiedinger 0:4837c9695a02 429 const float a = x * TWO_PI_INV;
RLRiedinger 0:4837c9695a02 430
RLRiedinger 0:4837c9695a02 431 x -= static_cast < int > (a) * TWO_PI;
RLRiedinger 0:4837c9695a02 432
RLRiedinger 0:4837c9695a02 433 if (x < 0.0f) {
RLRiedinger 0:4837c9695a02 434
RLRiedinger 0:4837c9695a02 435 x += TWO_PI;
RLRiedinger 0:4837c9695a02 436
RLRiedinger 0:4837c9695a02 437 }
RLRiedinger 0:4837c9695a02 438
RLRiedinger 0:4837c9695a02 439 // 4 pieces of hills
RLRiedinger 0:4837c9695a02 440
RLRiedinger 0:4837c9695a02 441 if (x < HALF_PI) {
RLRiedinger 0:4837c9695a02 442
RLRiedinger 0:4837c9695a02 443 return Hill(HALF_PI - x);
RLRiedinger 0:4837c9695a02 444
RLRiedinger 0:4837c9695a02 445 } else if (x < PI) {
RLRiedinger 0:4837c9695a02 446
RLRiedinger 0:4837c9695a02 447 return Hill(x - HALF_PI);
RLRiedinger 0:4837c9695a02 448
RLRiedinger 0:4837c9695a02 449 } else if (x < 3.0f * HALF_PI) {
RLRiedinger 0:4837c9695a02 450
RLRiedinger 0:4837c9695a02 451 return -Hill(3.0f * HALF_PI - x);
RLRiedinger 0:4837c9695a02 452
RLRiedinger 0:4837c9695a02 453 } else {
RLRiedinger 0:4837c9695a02 454
RLRiedinger 0:4837c9695a02 455 return -Hill(x - 3.0f * HALF_PI);
RLRiedinger 0:4837c9695a02 456
RLRiedinger 0:4837c9695a02 457 }
RLRiedinger 0:4837c9695a02 458
RLRiedinger 0:4837c9695a02 459 }
RLRiedinger 0:4837c9695a02 460
RLRiedinger 0:4837c9695a02 461
RLRiedinger 0:4837c9695a02 462 double ROBOT::FastCos(double x) {
RLRiedinger 0:4837c9695a02 463
RLRiedinger 0:4837c9695a02 464 return FastSin(x + HALF_PI);
RLRiedinger 0:4837c9695a02 465
RLRiedinger 0:4837c9695a02 466 }
RLRiedinger 0:4837c9695a02 467
RLRiedinger 0:4837c9695a02 468 int ROBOT::process_IPD(char c) {
RLRiedinger 0:4837c9695a02 469
RLRiedinger 0:4837c9695a02 470 sprintf(FORMAT_BUFFER, "c = [%c]", c);
RLRiedinger 0:4837c9695a02 471 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 472
RLRiedinger 0:4837c9695a02 473 int return_code = 0;
RLRiedinger 0:4837c9695a02 474
RLRiedinger 0:4837c9695a02 475 if (!mCOUNT && (c >= '0') && (c <= '9')) {
RLRiedinger 0:4837c9695a02 476
RLRiedinger 0:4837c9695a02 477 COUNT = COUNT * 10 + (int) c - '0';
RLRiedinger 0:4837c9695a02 478 uCOUNT = 0;
RLRiedinger 0:4837c9695a02 479
RLRiedinger 0:4837c9695a02 480 sprintf(FORMAT_BUFFER, "COUNT = %i", COUNT);
RLRiedinger 0:4837c9695a02 481 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 482
RLRiedinger 0:4837c9695a02 483 } else if (!mCOLON && (c == ':')) {
RLRiedinger 0:4837c9695a02 484
RLRiedinger 0:4837c9695a02 485 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 486
RLRiedinger 0:4837c9695a02 487 mCOUNT = 1;
RLRiedinger 0:4837c9695a02 488 mCOLON = 1;
RLRiedinger 0:4837c9695a02 489
RLRiedinger 0:4837c9695a02 490 } else if (mCOLON && (uCOUNT < COUNT) && (uCOUNT < 80)) {
RLRiedinger 0:4837c9695a02 491
RLRiedinger 0:4837c9695a02 492 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 493
RLRiedinger 0:4837c9695a02 494 *pIPD_BUFFER++ = c;
RLRiedinger 0:4837c9695a02 495 *pIPD_BUFFER = 0;
RLRiedinger 0:4837c9695a02 496 uCOUNT++;
RLRiedinger 0:4837c9695a02 497
RLRiedinger 0:4837c9695a02 498 sprintf(FORMAT_BUFFER, "uCOUNT = %i", uCOUNT);
RLRiedinger 0:4837c9695a02 499 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 500
RLRiedinger 0:4837c9695a02 501 if (uCOUNT == COUNT) {
RLRiedinger 0:4837c9695a02 502
RLRiedinger 0:4837c9695a02 503 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 504
RLRiedinger 0:4837c9695a02 505 sprintf(FORMAT_BUFFER, "%6d %s [%s]\r\n", __LINE__, __FUNCTION__, IPD_BUFFER);
RLRiedinger 0:4837c9695a02 506 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 507
RLRiedinger 0:4837c9695a02 508 // [21:40:34.9 1234 1234 1234 12345678 12345678]
RLRiedinger 0:4837c9695a02 509 // [17:14:02.0 336 67 -32 1000677 1000747]
RLRiedinger 0:4837c9695a02 510 // [0123456789012345678901234567890123456789012]
RLRiedinger 0:4837c9695a02 511 // [0000000000111111111122222222223333333333444]
RLRiedinger 0:4837c9695a02 512
RLRiedinger 0:4837c9695a02 513 // int hours, minutes, seconds, tenths, angle;
RLRiedinger 0:4837c9695a02 514 // unsigned int hours, minutes, seconds, tenths, angle;
RLRiedinger 0:4837c9695a02 515 // double CurrentTime, OldTime, ElapsedTime, Angle, Speed;
RLRiedinger 0:4837c9695a02 516 // uint64_t leftencoder, rightencoder, oldleftencoder, oldrightencoder;
RLRiedinger 0:4837c9695a02 517 // int mPROMPT, mIPD, mCOUNT, COUNT, uCOUNT, mCOLON, First;
RLRiedinger 0:4837c9695a02 518
RLRiedinger 0:4837c9695a02 519 IPD_BUFFER[15] = 0;
RLRiedinger 0:4837c9695a02 520 IPD_BUFFER[20] = 0;
RLRiedinger 0:4837c9695a02 521 IPD_BUFFER[25] = 0;
RLRiedinger 0:4837c9695a02 522
RLRiedinger 0:4837c9695a02 523 // IPD_BUFFER[34] = 0;
RLRiedinger 0:4837c9695a02 524 // IPD_BUFFER[43] = 0;
RLRiedinger 0:4837c9695a02 525
RLRiedinger 0:4837c9695a02 526 int hours = 10 * (int)(IPD_BUFFER[0] - '0') + (int)(IPD_BUFFER[1] - '0');
RLRiedinger 0:4837c9695a02 527 int minutes = 10 * (int)(IPD_BUFFER[3] - '0') + (int)(IPD_BUFFER[4] - '0');
RLRiedinger 0:4837c9695a02 528 int seconds = 10 * (int)(IPD_BUFFER[6] - '0') + (int)(IPD_BUFFER[7] - '0');
RLRiedinger 0:4837c9695a02 529 int tenths = (int)(IPD_BUFFER[9] - '0');
RLRiedinger 0:4837c9695a02 530
RLRiedinger 0:4837c9695a02 531 sprintf(FORMAT_BUFFER, "%6d %s [%02d:%02d:%02d.%1d]\r\n",
RLRiedinger 0:4837c9695a02 532 __LINE__, __FUNCTION__, hours, minutes, seconds, tenths);
RLRiedinger 0:4837c9695a02 533 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 534
RLRiedinger 0:4837c9695a02 535 CurrentTime = (double)(60 * ((60 * hours) + minutes) + seconds + (double)(tenths / 10));
RLRiedinger 0:4837c9695a02 536
RLRiedinger 0:4837c9695a02 537 sprintf(FORMAT_BUFFER, "%6d %s CurrentTime = %6.1f\r\n",
RLRiedinger 0:4837c9695a02 538 __LINE__, __FUNCTION__, CurrentTime);
RLRiedinger 0:4837c9695a02 539 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 540
RLRiedinger 0:4837c9695a02 541 Angle = (double)atoi((char *)(IPD_BUFFER + 11));
RLRiedinger 0:4837c9695a02 542
RLRiedinger 0:4837c9695a02 543 sprintf(FORMAT_BUFFER, "%6d %s [Angle = %4.0f\r\n",
RLRiedinger 0:4837c9695a02 544 __LINE__, __FUNCTION__, Angle);
RLRiedinger 0:4837c9695a02 545 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 546
RLRiedinger 0:4837c9695a02 547 #if (0)
RLRiedinger 0:4837c9695a02 548
RLRiedinger 0:4837c9695a02 549 leftencoder = (uint64_t)atol((char *)(IPD_BUFFER + 26));
RLRiedinger 0:4837c9695a02 550 rightencoder = (uint64_t)atol((char *)(IPD_BUFFER + 35));
RLRiedinger 0:4837c9695a02 551
RLRiedinger 0:4837c9695a02 552 sprintf(FORMAT_BUFFER, "%6d %s [LE = %8lld] [RE = %8lld]\r\n",
RLRiedinger 0:4837c9695a02 553 __LINE__, __FUNCTION__, leftencoder, rightencoder);
RLRiedinger 0:4837c9695a02 554 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 555
RLRiedinger 0:4837c9695a02 556 #endif
RLRiedinger 0:4837c9695a02 557
RLRiedinger 0:4837c9695a02 558 if (First) {
RLRiedinger 0:4837c9695a02 559
RLRiedinger 0:4837c9695a02 560 First = 0;
RLRiedinger 0:4837c9695a02 561
RLRiedinger 0:4837c9695a02 562 } else {
RLRiedinger 0:4837c9695a02 563
RLRiedinger 0:4837c9695a02 564 ElapsedTime = CurrentTime - OldTime;
RLRiedinger 0:4837c9695a02 565
RLRiedinger 0:4837c9695a02 566 if (ElapsedTime < 0.0) {
RLRiedinger 0:4837c9695a02 567
RLRiedinger 0:4837c9695a02 568 ElapsedTime += 86400.0;
RLRiedinger 0:4837c9695a02 569
RLRiedinger 0:4837c9695a02 570 }
RLRiedinger 0:4837c9695a02 571
RLRiedinger 0:4837c9695a02 572 sprintf(FORMAT_BUFFER, "%6d %s [ElapsedTime = %6.1f]\r\n",
RLRiedinger 0:4837c9695a02 573 __LINE__, __FUNCTION__, ElapsedTime);
RLRiedinger 0:4837c9695a02 574 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 575
RLRiedinger 0:4837c9695a02 576 #if (0)
RLRiedinger 0:4837c9695a02 577
RLRiedinger 0:4837c9695a02 578 int64_t leftencoderdelta = leftencoder - oldleftencoder;
RLRiedinger 0:4837c9695a02 579 int64_t rightencoderdelta = rightencoder - oldrightencoder;
RLRiedinger 0:4837c9695a02 580
RLRiedinger 0:4837c9695a02 581 if (leftencoderdelta > 0x3fffffff) {
RLRiedinger 0:4837c9695a02 582
RLRiedinger 0:4837c9695a02 583 leftencoderdelta -= 0xffffffff;
RLRiedinger 0:4837c9695a02 584
RLRiedinger 0:4837c9695a02 585 } else if (leftencoderdelta < - 0x3fffffff) {
RLRiedinger 0:4837c9695a02 586
RLRiedinger 0:4837c9695a02 587 leftencoderdelta += 0xffffffff;
RLRiedinger 0:4837c9695a02 588
RLRiedinger 0:4837c9695a02 589 }
RLRiedinger 0:4837c9695a02 590
RLRiedinger 0:4837c9695a02 591 sprintf(FORMAT_BUFFER, "%6d %s [leftencoderdelta = %8lld]\r\n",
RLRiedinger 0:4837c9695a02 592 __LINE__, __FUNCTION__, leftencoderdelta);
RLRiedinger 0:4837c9695a02 593 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 594
RLRiedinger 0:4837c9695a02 595 if (rightencoderdelta > 0x3fffffff) {
RLRiedinger 0:4837c9695a02 596
RLRiedinger 0:4837c9695a02 597 rightencoderdelta -= 0xffffffff;
RLRiedinger 0:4837c9695a02 598
RLRiedinger 0:4837c9695a02 599 } else if (rightencoderdelta < - 0x3fffffff) {
RLRiedinger 0:4837c9695a02 600
RLRiedinger 0:4837c9695a02 601 rightencoderdelta += 0xffffffff;
RLRiedinger 0:4837c9695a02 602
RLRiedinger 0:4837c9695a02 603 }
RLRiedinger 0:4837c9695a02 604
RLRiedinger 0:4837c9695a02 605 sprintf(FORMAT_BUFFER, "%6d %s [rightencoderdelta = %8lld]\r\n",
RLRiedinger 0:4837c9695a02 606 __LINE__, __FUNCTION__, rightencoderdelta);
RLRiedinger 0:4837c9695a02 607 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 608
RLRiedinger 0:4837c9695a02 609 double EncoderDelta = (double) (leftencoderdelta > rightencoderdelta ?
RLRiedinger 0:4837c9695a02 610 leftencoderdelta : rightencoderdelta);
RLRiedinger 0:4837c9695a02 611
RLRiedinger 0:4837c9695a02 612 sprintf(FORMAT_BUFFER, "%6d %s [EncoderDelta = %6.1f]\r\n",
RLRiedinger 0:4837c9695a02 613 __LINE__, __FUNCTION__, EncoderDelta);
RLRiedinger 0:4837c9695a02 614 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 615
RLRiedinger 0:4837c9695a02 616 double Velocity = EncoderDelta / ElapsedTime / 10.0;
RLRiedinger 0:4837c9695a02 617
RLRiedinger 0:4837c9695a02 618 sprintf(FORMAT_BUFFER, "%6d %s [Velocity = %6.2f]\r\n",
RLRiedinger 0:4837c9695a02 619 __LINE__, __FUNCTION__, Velocity);
RLRiedinger 0:4837c9695a02 620 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 621
RLRiedinger 0:4837c9695a02 622 R = Velocity * ElapsedTime;
RLRiedinger 0:4837c9695a02 623
RLRiedinger 0:4837c9695a02 624 #endif
RLRiedinger 0:4837c9695a02 625
RLRiedinger 0:4837c9695a02 626 this->clearBall();
RLRiedinger 0:4837c9695a02 627 this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 628
RLRiedinger 0:4837c9695a02 629 #if (1)
RLRiedinger 0:4837c9695a02 630
RLRiedinger 0:4837c9695a02 631 this->circleX = (int)atoi((char *)(IPD_BUFFER + 16)) + DisplayN18::WIDTH / 2;
RLRiedinger 0:4837c9695a02 632 this->circleY = (int)atoi((char *)(IPD_BUFFER + 21)) + 3 * DisplayN18::HEIGHT / 4;
RLRiedinger 0:4837c9695a02 633
RLRiedinger 0:4837c9695a02 634 #else
RLRiedinger 0:4837c9695a02 635
RLRiedinger 0:4837c9695a02 636 this->ballX += (int) (R * FastCos(Angle * PI / 180.0) + 0.5);
RLRiedinger 0:4837c9695a02 637 this->ballY += (int) (R * FastSin(Angle * PI / 180.0) + 0.5);
RLRiedinger 0:4837c9695a02 638
RLRiedinger 0:4837c9695a02 639 #endif
RLRiedinger 0:4837c9695a02 640
RLRiedinger 0:4837c9695a02 641 if (this->circleX - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 642
RLRiedinger 0:4837c9695a02 643 this->circleX = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 644
RLRiedinger 0:4837c9695a02 645 } else if (this->circleX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
RLRiedinger 0:4837c9695a02 646
RLRiedinger 0:4837c9695a02 647 this->circleX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 648
RLRiedinger 0:4837c9695a02 649 }
RLRiedinger 0:4837c9695a02 650
RLRiedinger 0:4837c9695a02 651 if (this->circleY - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 652
RLRiedinger 0:4837c9695a02 653 this->circleY = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 654
RLRiedinger 0:4837c9695a02 655 } else if (this->circleY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
RLRiedinger 0:4837c9695a02 656
RLRiedinger 0:4837c9695a02 657 this->circleY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 658
RLRiedinger 0:4837c9695a02 659 }
RLRiedinger 0:4837c9695a02 660
RLRiedinger 0:4837c9695a02 661 if (this->ballX - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 662
RLRiedinger 0:4837c9695a02 663 this->ballX = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 664
RLRiedinger 0:4837c9695a02 665 } else if (this->ballX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
RLRiedinger 0:4837c9695a02 666
RLRiedinger 0:4837c9695a02 667 this->ballX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 668
RLRiedinger 0:4837c9695a02 669 }
RLRiedinger 0:4837c9695a02 670
RLRiedinger 0:4837c9695a02 671 if (this->ballY - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 672
RLRiedinger 0:4837c9695a02 673 this->ballY = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 674
RLRiedinger 0:4837c9695a02 675 } else if (this->ballY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
RLRiedinger 0:4837c9695a02 676
RLRiedinger 0:4837c9695a02 677 this->ballY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 678
RLRiedinger 0:4837c9695a02 679 }
RLRiedinger 0:4837c9695a02 680
RLRiedinger 0:4837c9695a02 681 this->drawBall();
RLRiedinger 0:4837c9695a02 682 this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 683 this->DrawBox();
RLRiedinger 0:4837c9695a02 684
RLRiedinger 0:4837c9695a02 685 sprintf(FORMAT_BUFFER, "%6d %s [this->circleX = %4i this->circleY = %4i]\r\n",
RLRiedinger 0:4837c9695a02 686 __LINE__, __FUNCTION__, this->circleX, this->circleY);
RLRiedinger 0:4837c9695a02 687 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 688
RLRiedinger 0:4837c9695a02 689 sprintf(FORMAT_BUFFER, "%6d %s [this->ballX = %4i this->ballY = %4i]\r\n",
RLRiedinger 0:4837c9695a02 690 __LINE__, __FUNCTION__, this->ballX, this->ballY);
RLRiedinger 0:4837c9695a02 691 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 692
RLRiedinger 0:4837c9695a02 693 // this->ballX = DisplayN18::WIDTH / 2 - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 694 // this->ballY = DisplayN18::HEIGHT / 4 - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 695
RLRiedinger 0:4837c9695a02 696 }
RLRiedinger 0:4837c9695a02 697
RLRiedinger 0:4837c9695a02 698 OldTime = CurrentTime;
RLRiedinger 0:4837c9695a02 699 oldleftencoder = leftencoder;
RLRiedinger 0:4837c9695a02 700 oldrightencoder = rightencoder;
RLRiedinger 0:4837c9695a02 701
RLRiedinger 0:4837c9695a02 702 mIPD = 0;
RLRiedinger 0:4837c9695a02 703
RLRiedinger 0:4837c9695a02 704 }
RLRiedinger 0:4837c9695a02 705
RLRiedinger 0:4837c9695a02 706 }
RLRiedinger 0:4837c9695a02 707
RLRiedinger 0:4837c9695a02 708 // debug(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 709
RLRiedinger 0:4837c9695a02 710 return(return_code);
RLRiedinger 0:4837c9695a02 711
RLRiedinger 0:4837c9695a02 712 }
RLRiedinger 0:4837c9695a02 713
RLRiedinger 0:4837c9695a02 714 void ROBOT::process_buttons(void) {
RLRiedinger 0:4837c9695a02 715
RLRiedinger 0:4837c9695a02 716 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 717
RLRiedinger 0:4837c9695a02 718 #if (1)
RLRiedinger 0:4837c9695a02 719
RLRiedinger 0:4837c9695a02 720 if (!square) {
RLRiedinger 0:4837c9695a02 721
RLRiedinger 0:4837c9695a02 722 BUFFER[5] = 'S';
RLRiedinger 0:4837c9695a02 723
RLRiedinger 0:4837c9695a02 724 } else {
RLRiedinger 0:4837c9695a02 725
RLRiedinger 0:4837c9695a02 726 BUFFER[5] = 's';
RLRiedinger 0:4837c9695a02 727
RLRiedinger 0:4837c9695a02 728 }
RLRiedinger 0:4837c9695a02 729
RLRiedinger 0:4837c9695a02 730 sprintf(BUFFER, "%03i %03i %c",
RLRiedinger 0:4837c9695a02 731 this->ballX - DisplayN18::WIDTH / 2, this->ballY - 3 * DisplayN18::HEIGHT / 4, square ? 's' : 'S');
RLRiedinger 0:4837c9695a02 732 sprintf(FORMAT_BUFFER, "%6d %s [%s]\r\n", __LINE__, __FUNCTION__, BUFFER);
RLRiedinger 0:4837c9695a02 733 serial_print2(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 734
RLRiedinger 0:4837c9695a02 735 #else
RLRiedinger 0:4837c9695a02 736
RLRiedinger 0:4837c9695a02 737 if (!left) {
RLRiedinger 0:4837c9695a02 738
RLRiedinger 0:4837c9695a02 739 BUFFER[0] = 'L';
RLRiedinger 0:4837c9695a02 740
RLRiedinger 0:4837c9695a02 741 } else {
RLRiedinger 0:4837c9695a02 742
RLRiedinger 0:4837c9695a02 743 BUFFER[0] = 'l';
RLRiedinger 0:4837c9695a02 744
RLRiedinger 0:4837c9695a02 745 }
RLRiedinger 0:4837c9695a02 746
RLRiedinger 0:4837c9695a02 747 if (!right) {
RLRiedinger 0:4837c9695a02 748
RLRiedinger 0:4837c9695a02 749 BUFFER[1] = 'R';
RLRiedinger 0:4837c9695a02 750
RLRiedinger 0:4837c9695a02 751 } else {
RLRiedinger 0:4837c9695a02 752
RLRiedinger 0:4837c9695a02 753 BUFFER[1] = 'r';
RLRiedinger 0:4837c9695a02 754
RLRiedinger 0:4837c9695a02 755 }
RLRiedinger 0:4837c9695a02 756
RLRiedinger 0:4837c9695a02 757 if (!down) {
RLRiedinger 0:4837c9695a02 758
RLRiedinger 0:4837c9695a02 759 BUFFER[2] = 'D';
RLRiedinger 0:4837c9695a02 760
RLRiedinger 0:4837c9695a02 761 } else {
RLRiedinger 0:4837c9695a02 762
RLRiedinger 0:4837c9695a02 763 BUFFER[2] = 'd';
RLRiedinger 0:4837c9695a02 764
RLRiedinger 0:4837c9695a02 765 }
RLRiedinger 0:4837c9695a02 766
RLRiedinger 0:4837c9695a02 767 if (!up) {
RLRiedinger 0:4837c9695a02 768
RLRiedinger 0:4837c9695a02 769 BUFFER[3] = 'U';
RLRiedinger 0:4837c9695a02 770
RLRiedinger 0:4837c9695a02 771 } else {
RLRiedinger 0:4837c9695a02 772
RLRiedinger 0:4837c9695a02 773 BUFFER[3] = 'u';
RLRiedinger 0:4837c9695a02 774
RLRiedinger 0:4837c9695a02 775 }
RLRiedinger 0:4837c9695a02 776
RLRiedinger 0:4837c9695a02 777 if (!square) {
RLRiedinger 0:4837c9695a02 778
RLRiedinger 0:4837c9695a02 779 BUFFER[5] = 'S';
RLRiedinger 0:4837c9695a02 780
RLRiedinger 0:4837c9695a02 781 } else {
RLRiedinger 0:4837c9695a02 782
RLRiedinger 0:4837c9695a02 783 BUFFER[5] = 's';
RLRiedinger 0:4837c9695a02 784
RLRiedinger 0:4837c9695a02 785 }
RLRiedinger 0:4837c9695a02 786
RLRiedinger 0:4837c9695a02 787 if (!circle) {
RLRiedinger 0:4837c9695a02 788
RLRiedinger 0:4837c9695a02 789 BUFFER[6] = 'C';
RLRiedinger 0:4837c9695a02 790
RLRiedinger 0:4837c9695a02 791 } else {
RLRiedinger 0:4837c9695a02 792
RLRiedinger 0:4837c9695a02 793 BUFFER[6] = 'c';
RLRiedinger 0:4837c9695a02 794
RLRiedinger 0:4837c9695a02 795 }
RLRiedinger 0:4837c9695a02 796
RLRiedinger 0:4837c9695a02 797 BUFFER[7] = 0;
RLRiedinger 0:4837c9695a02 798
RLRiedinger 0:4837c9695a02 799 #endif
RLRiedinger 0:4837c9695a02 800
RLRiedinger 0:4837c9695a02 801 serial_print1(BUFFER);
RLRiedinger 0:4837c9695a02 802
RLRiedinger 0:4837c9695a02 803 //
RLRiedinger 0:4837c9695a02 804 // Should wait for OK ...
RLRiedinger 0:4837c9695a02 805 //
RLRiedinger 0:4837c9695a02 806
RLRiedinger 0:4837c9695a02 807 mPROMPT = 0;
RLRiedinger 0:4837c9695a02 808
RLRiedinger 0:4837c9695a02 809 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 810
RLRiedinger 0:4837c9695a02 811 }
RLRiedinger 0:4837c9695a02 812
RLRiedinger 0:4837c9695a02 813 int ROBOT::terminal_emulator(int echo_comands, int timeout) {
RLRiedinger 0:4837c9695a02 814
RLRiedinger 0:4837c9695a02 815 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 816
RLRiedinger 0:4837c9695a02 817 int return_code = 0;
RLRiedinger 0:4837c9695a02 818 char c = '\0';
RLRiedinger 0:4837c9695a02 819
RLRiedinger 0:4837c9695a02 820 mPROMPT = 0;
RLRiedinger 0:4837c9695a02 821 mIPD = 0;
RLRiedinger 0:4837c9695a02 822
RLRiedinger 0:4837c9695a02 823 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 824 #if (0)
RLRiedinger 0:4837c9695a02 825 int ODOMETER_index = 0;
RLRiedinger 0:4837c9695a02 826 #endif
RLRiedinger 0:4837c9695a02 827 #endif
RLRiedinger 0:4837c9695a02 828
RLRiedinger 0:4837c9695a02 829 Timer ReceiveTimer;
RLRiedinger 0:4837c9695a02 830 Timer ButtonTimer;
RLRiedinger 0:4837c9695a02 831
RLRiedinger 0:4837c9695a02 832 int Button_Timeout = Initial_Button_Timeout;
RLRiedinger 0:4837c9695a02 833
RLRiedinger 0:4837c9695a02 834 ReceiveTimer.start();
RLRiedinger 0:4837c9695a02 835 ButtonTimer.start();
RLRiedinger 0:4837c9695a02 836
RLRiedinger 0:4837c9695a02 837 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 838
RLRiedinger 0:4837c9695a02 839 while (return_code != ERROR) {
RLRiedinger 0:4837c9695a02 840
RLRiedinger 0:4837c9695a02 841 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 842
RLRiedinger 0:4837c9695a02 843 // From Serial to SPI/I2C
RLRiedinger 0:4837c9695a02 844
RLRiedinger 0:4837c9695a02 845 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 846
RLRiedinger 0:4837c9695a02 847 while (!return_code && serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 848
RLRiedinger 0:4837c9695a02 849 ReceiveTimer.reset();
RLRiedinger 0:4837c9695a02 850
RLRiedinger 0:4837c9695a02 851 c = serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 852
RLRiedinger 0:4837c9695a02 853 if (c == '#') {
RLRiedinger 0:4837c9695a02 854
RLRiedinger 0:4837c9695a02 855 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 856
RLRiedinger 0:4837c9695a02 857 return_code = QUIT;
RLRiedinger 0:4837c9695a02 858
RLRiedinger 0:4837c9695a02 859 } else {
RLRiedinger 0:4837c9695a02 860
RLRiedinger 0:4837c9695a02 861 serial_bridge1.putc(c);
RLRiedinger 0:4837c9695a02 862
RLRiedinger 0:4837c9695a02 863 }
RLRiedinger 0:4837c9695a02 864
RLRiedinger 0:4837c9695a02 865 }
RLRiedinger 0:4837c9695a02 866
RLRiedinger 0:4837c9695a02 867 #endif
RLRiedinger 0:4837c9695a02 868
RLRiedinger 0:4837c9695a02 869 // From SPI/I2C to serial
RLRiedinger 0:4837c9695a02 870
RLRiedinger 0:4837c9695a02 871 while ((return_code != ERROR) && serial_bridge1.readable()) {
RLRiedinger 0:4837c9695a02 872
RLRiedinger 0:4837c9695a02 873 if (!mIPD) {
RLRiedinger 0:4837c9695a02 874
RLRiedinger 0:4837c9695a02 875 ReceiveTimer.reset();
RLRiedinger 0:4837c9695a02 876
RLRiedinger 0:4837c9695a02 877 }
RLRiedinger 0:4837c9695a02 878
RLRiedinger 0:4837c9695a02 879 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 880
RLRiedinger 0:4837c9695a02 881 c = serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 882
RLRiedinger 0:4837c9695a02 883 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 884
RLRiedinger 0:4837c9695a02 885 if (echo_comands) {
RLRiedinger 0:4837c9695a02 886
RLRiedinger 0:4837c9695a02 887 serial_bridge2.putc(c);
RLRiedinger 0:4837c9695a02 888
RLRiedinger 0:4837c9695a02 889 sprintf(UART_BUFFER, "%6d %s [%c]\r\n", __LINE__, __FUNCTION__, c);
RLRiedinger 0:4837c9695a02 890 serial_print2(UART_BUFFER);
RLRiedinger 0:4837c9695a02 891
RLRiedinger 0:4837c9695a02 892 }
RLRiedinger 0:4837c9695a02 893
RLRiedinger 0:4837c9695a02 894 #endif
RLRiedinger 0:4837c9695a02 895
RLRiedinger 0:4837c9695a02 896 return_code = match_RESPONSES(c, matchtab, NMATCHES);
RLRiedinger 0:4837c9695a02 897
RLRiedinger 0:4837c9695a02 898 if (return_code != 0) {
RLRiedinger 0:4837c9695a02 899
RLRiedinger 0:4837c9695a02 900 sprintf(FORMAT_BUFFER, "return_code = %i", return_code);
RLRiedinger 0:4837c9695a02 901 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 902
RLRiedinger 0:4837c9695a02 903 }
RLRiedinger 0:4837c9695a02 904
RLRiedinger 0:4837c9695a02 905 if ((return_code == IPD) && !mIPD) {
RLRiedinger 0:4837c9695a02 906
RLRiedinger 0:4837c9695a02 907 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 908
RLRiedinger 0:4837c9695a02 909 return_code = 0;
RLRiedinger 0:4837c9695a02 910 mIPD = 1;
RLRiedinger 0:4837c9695a02 911 mCOUNT = 0;
RLRiedinger 0:4837c9695a02 912 COUNT = 0;
RLRiedinger 0:4837c9695a02 913 uCOUNT = 0;
RLRiedinger 0:4837c9695a02 914 mCOLON = 0;
RLRiedinger 0:4837c9695a02 915 pIPD_BUFFER = IPD_BUFFER;
RLRiedinger 0:4837c9695a02 916
RLRiedinger 0:4837c9695a02 917 } else if (mIPD) {
RLRiedinger 0:4837c9695a02 918
RLRiedinger 0:4837c9695a02 919 led1 = 1;
RLRiedinger 0:4837c9695a02 920
RLRiedinger 0:4837c9695a02 921 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 922
RLRiedinger 0:4837c9695a02 923 process_IPD(c);
RLRiedinger 0:4837c9695a02 924
RLRiedinger 0:4837c9695a02 925 ReceiveTimer.reset();
RLRiedinger 0:4837c9695a02 926
RLRiedinger 0:4837c9695a02 927 led1 = 0;
RLRiedinger 0:4837c9695a02 928
RLRiedinger 0:4837c9695a02 929 } else if (return_code == PROMPT) {
RLRiedinger 0:4837c9695a02 930
RLRiedinger 0:4837c9695a02 931 led1 = 1;
RLRiedinger 0:4837c9695a02 932
RLRiedinger 0:4837c9695a02 933 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 934
RLRiedinger 0:4837c9695a02 935 mPROMPT = 1;
RLRiedinger 0:4837c9695a02 936 return_code = 0;
RLRiedinger 0:4837c9695a02 937
RLRiedinger 0:4837c9695a02 938 process_buttons();
RLRiedinger 0:4837c9695a02 939
RLRiedinger 0:4837c9695a02 940 ButtonTimer.start();
RLRiedinger 0:4837c9695a02 941
RLRiedinger 0:4837c9695a02 942 led1 = 0;
RLRiedinger 0:4837c9695a02 943
RLRiedinger 0:4837c9695a02 944 } else if ((return_code == OK) || (return_code == BUSY)) {
RLRiedinger 0:4837c9695a02 945
RLRiedinger 0:4837c9695a02 946 led1 = 1;
RLRiedinger 0:4837c9695a02 947
RLRiedinger 0:4837c9695a02 948 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 949
RLRiedinger 0:4837c9695a02 950 if (mIPD) {
RLRiedinger 0:4837c9695a02 951
RLRiedinger 0:4837c9695a02 952 mIPD = 0;
RLRiedinger 0:4837c9695a02 953
RLRiedinger 0:4837c9695a02 954 } else if (mPROMPT) {
RLRiedinger 0:4837c9695a02 955
RLRiedinger 0:4837c9695a02 956 mPROMPT = 0;
RLRiedinger 0:4837c9695a02 957
RLRiedinger 0:4837c9695a02 958 }
RLRiedinger 0:4837c9695a02 959
RLRiedinger 0:4837c9695a02 960 led1 = 0;
RLRiedinger 0:4837c9695a02 961
RLRiedinger 0:4837c9695a02 962 } else if (return_code == ERROR) {
RLRiedinger 0:4837c9695a02 963
RLRiedinger 0:4837c9695a02 964 // debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 965
RLRiedinger 0:4837c9695a02 966 }
RLRiedinger 0:4837c9695a02 967
RLRiedinger 0:4837c9695a02 968 }
RLRiedinger 0:4837c9695a02 969
RLRiedinger 0:4837c9695a02 970 if (ReceiveTimer.read_ms() / 100 % 10 == 0) {
RLRiedinger 0:4837c9695a02 971
RLRiedinger 0:4837c9695a02 972 #if (1)
RLRiedinger 0:4837c9695a02 973
RLRiedinger 0:4837c9695a02 974 this->clearBall();
RLRiedinger 0:4837c9695a02 975 this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 976
RLRiedinger 0:4837c9695a02 977 // +---+-----+---+
RLRiedinger 0:4837c9695a02 978 // | 6 | 7 | 8 |
RLRiedinger 0:4837c9695a02 979 // +---+-----+---+
RLRiedinger 0:4837c9695a02 980 // | 3 | 4 | 5 |
RLRiedinger 0:4837c9695a02 981 // +---+-----+---+
RLRiedinger 0:4837c9695a02 982 // | 0 | 1 | 2 |
RLRiedinger 0:4837c9695a02 983 // +---+-----+---+
RLRiedinger 0:4837c9695a02 984
RLRiedinger 0:4837c9695a02 985 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 986
RLRiedinger 0:4837c9695a02 987 sprintf(FORMAT_BUFFER, "%i4 %i4 %i3", this->ballX, this->ballY, ROBOT::BALL_RADIUS);
RLRiedinger 0:4837c9695a02 988 debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 989
RLRiedinger 0:4837c9695a02 990 sprintf(FORMAT_BUFFER, "%i4 %i4", DisplayN18::WIDTH, DisplayN18::HEIGHT);
RLRiedinger 0:4837c9695a02 991 debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 992
RLRiedinger 0:4837c9695a02 993 sprintf(FORMAT_BUFFER, "%i3 %i3", this->BoxWidth, this->BoxHeight);
RLRiedinger 0:4837c9695a02 994 debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 995
RLRiedinger 0:4837c9695a02 996 if (this->ballX + ROBOT::BALL_RADIUS == DisplayN18::WIDTH / 2 - BoxWidth / 2 - 1) {
RLRiedinger 0:4837c9695a02 997
RLRiedinger 0:4837c9695a02 998 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 999
RLRiedinger 0:4837c9695a02 1000 if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
RLRiedinger 0:4837c9695a02 1001
RLRiedinger 0:4837c9695a02 1002 // 0 - No +X (Right) and -Y (Up)
RLRiedinger 0:4837c9695a02 1003
RLRiedinger 0:4837c9695a02 1004 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1005
RLRiedinger 0:4837c9695a02 1006 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1007
RLRiedinger 0:4837c9695a02 1008 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1009
RLRiedinger 0:4837c9695a02 1010 this->ballX--;
RLRiedinger 0:4837c9695a02 1011
RLRiedinger 0:4837c9695a02 1012 } else if (!this->right.read() && this->up.read()) {
RLRiedinger 0:4837c9695a02 1013
RLRiedinger 0:4837c9695a02 1014 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1015
RLRiedinger 0:4837c9695a02 1016 this->ballX++;
RLRiedinger 0:4837c9695a02 1017
RLRiedinger 0:4837c9695a02 1018 }
RLRiedinger 0:4837c9695a02 1019
RLRiedinger 0:4837c9695a02 1020 if (!this->up.read() && this->right.read()) {
RLRiedinger 0:4837c9695a02 1021
RLRiedinger 0:4837c9695a02 1022 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1023
RLRiedinger 0:4837c9695a02 1024 this->ballY--;
RLRiedinger 0:4837c9695a02 1025
RLRiedinger 0:4837c9695a02 1026 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1027
RLRiedinger 0:4837c9695a02 1028 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1029
RLRiedinger 0:4837c9695a02 1030 this->ballY++;
RLRiedinger 0:4837c9695a02 1031
RLRiedinger 0:4837c9695a02 1032 }
RLRiedinger 0:4837c9695a02 1033
RLRiedinger 0:4837c9695a02 1034 } else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
RLRiedinger 0:4837c9695a02 1035 (this->ballY - ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
RLRiedinger 0:4837c9695a02 1036
RLRiedinger 0:4837c9695a02 1037 // 3 - No +X (Right)
RLRiedinger 0:4837c9695a02 1038
RLRiedinger 0:4837c9695a02 1039 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1040
RLRiedinger 0:4837c9695a02 1041 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1042
RLRiedinger 0:4837c9695a02 1043 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1044
RLRiedinger 0:4837c9695a02 1045 this->ballX--;
RLRiedinger 0:4837c9695a02 1046
RLRiedinger 0:4837c9695a02 1047 }
RLRiedinger 0:4837c9695a02 1048
RLRiedinger 0:4837c9695a02 1049 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1050
RLRiedinger 0:4837c9695a02 1051 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1052
RLRiedinger 0:4837c9695a02 1053 this->ballY--;
RLRiedinger 0:4837c9695a02 1054
RLRiedinger 0:4837c9695a02 1055 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1056
RLRiedinger 0:4837c9695a02 1057 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1058
RLRiedinger 0:4837c9695a02 1059 this->ballY++;
RLRiedinger 0:4837c9695a02 1060
RLRiedinger 0:4837c9695a02 1061 }
RLRiedinger 0:4837c9695a02 1062
RLRiedinger 0:4837c9695a02 1063 } else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
RLRiedinger 0:4837c9695a02 1064
RLRiedinger 0:4837c9695a02 1065 // 6 - No +X (Right) and +Y (Down)
RLRiedinger 0:4837c9695a02 1066
RLRiedinger 0:4837c9695a02 1067 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1068
RLRiedinger 0:4837c9695a02 1069 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1070
RLRiedinger 0:4837c9695a02 1071 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1072
RLRiedinger 0:4837c9695a02 1073 this->ballX--;
RLRiedinger 0:4837c9695a02 1074
RLRiedinger 0:4837c9695a02 1075 } else if (!this->right.read() && this->up.read()) {
RLRiedinger 0:4837c9695a02 1076
RLRiedinger 0:4837c9695a02 1077 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1078
RLRiedinger 0:4837c9695a02 1079 this->ballX++;
RLRiedinger 0:4837c9695a02 1080
RLRiedinger 0:4837c9695a02 1081 }
RLRiedinger 0:4837c9695a02 1082
RLRiedinger 0:4837c9695a02 1083 if (!this->up.read() && this->right.read()) {
RLRiedinger 0:4837c9695a02 1084
RLRiedinger 0:4837c9695a02 1085 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1086
RLRiedinger 0:4837c9695a02 1087 this->ballY--;
RLRiedinger 0:4837c9695a02 1088
RLRiedinger 0:4837c9695a02 1089 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1090
RLRiedinger 0:4837c9695a02 1091 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1092
RLRiedinger 0:4837c9695a02 1093 this->ballY++;
RLRiedinger 0:4837c9695a02 1094
RLRiedinger 0:4837c9695a02 1095 }
RLRiedinger 0:4837c9695a02 1096
RLRiedinger 0:4837c9695a02 1097 } else {
RLRiedinger 0:4837c9695a02 1098
RLRiedinger 0:4837c9695a02 1099 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1100
RLRiedinger 0:4837c9695a02 1101 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1102
RLRiedinger 0:4837c9695a02 1103 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1104
RLRiedinger 0:4837c9695a02 1105 this->ballY--;
RLRiedinger 0:4837c9695a02 1106
RLRiedinger 0:4837c9695a02 1107 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1108
RLRiedinger 0:4837c9695a02 1109 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1110
RLRiedinger 0:4837c9695a02 1111 this->ballY++;
RLRiedinger 0:4837c9695a02 1112
RLRiedinger 0:4837c9695a02 1113 }
RLRiedinger 0:4837c9695a02 1114
RLRiedinger 0:4837c9695a02 1115 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1116
RLRiedinger 0:4837c9695a02 1117 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1118
RLRiedinger 0:4837c9695a02 1119 this->ballX--;
RLRiedinger 0:4837c9695a02 1120
RLRiedinger 0:4837c9695a02 1121 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1122
RLRiedinger 0:4837c9695a02 1123 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1124
RLRiedinger 0:4837c9695a02 1125 this->ballX++;
RLRiedinger 0:4837c9695a02 1126
RLRiedinger 0:4837c9695a02 1127 }
RLRiedinger 0:4837c9695a02 1128
RLRiedinger 0:4837c9695a02 1129 }
RLRiedinger 0:4837c9695a02 1130
RLRiedinger 0:4837c9695a02 1131 } else if ((this->ballX - ROBOT::BALL_RADIUS >= DisplayN18::WIDTH / 2 - BoxWidth / 2 - 1) &&
RLRiedinger 0:4837c9695a02 1132 (this->ballX - ROBOT::BALL_RADIUS <= DisplayN18::WIDTH / 2 + BoxWidth / 2 + 1)) {
RLRiedinger 0:4837c9695a02 1133
RLRiedinger 0:4837c9695a02 1134 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1135
RLRiedinger 0:4837c9695a02 1136 if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
RLRiedinger 0:4837c9695a02 1137
RLRiedinger 0:4837c9695a02 1138 // 1 - No -Y (Up)
RLRiedinger 0:4837c9695a02 1139
RLRiedinger 0:4837c9695a02 1140 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1141
RLRiedinger 0:4837c9695a02 1142 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1143
RLRiedinger 0:4837c9695a02 1144 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1145
RLRiedinger 0:4837c9695a02 1146 this->ballX--;
RLRiedinger 0:4837c9695a02 1147
RLRiedinger 0:4837c9695a02 1148 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1149
RLRiedinger 0:4837c9695a02 1150 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1151
RLRiedinger 0:4837c9695a02 1152 this->ballX++;
RLRiedinger 0:4837c9695a02 1153
RLRiedinger 0:4837c9695a02 1154 }
RLRiedinger 0:4837c9695a02 1155
RLRiedinger 0:4837c9695a02 1156 if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1157
RLRiedinger 0:4837c9695a02 1158 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1159
RLRiedinger 0:4837c9695a02 1160 this->ballY++;
RLRiedinger 0:4837c9695a02 1161
RLRiedinger 0:4837c9695a02 1162 }
RLRiedinger 0:4837c9695a02 1163
RLRiedinger 0:4837c9695a02 1164 } else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
RLRiedinger 0:4837c9695a02 1165 (this->ballY + ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
RLRiedinger 0:4837c9695a02 1166
RLRiedinger 0:4837c9695a02 1167 // 4 - Oops inside box allow him to leave
RLRiedinger 0:4837c9695a02 1168
RLRiedinger 0:4837c9695a02 1169 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1170
RLRiedinger 0:4837c9695a02 1171 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1172
RLRiedinger 0:4837c9695a02 1173 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1174
RLRiedinger 0:4837c9695a02 1175 this->ballY--;
RLRiedinger 0:4837c9695a02 1176
RLRiedinger 0:4837c9695a02 1177 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1178
RLRiedinger 0:4837c9695a02 1179 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1180
RLRiedinger 0:4837c9695a02 1181 this->ballY++;
RLRiedinger 0:4837c9695a02 1182
RLRiedinger 0:4837c9695a02 1183 }
RLRiedinger 0:4837c9695a02 1184
RLRiedinger 0:4837c9695a02 1185 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1186
RLRiedinger 0:4837c9695a02 1187 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1188
RLRiedinger 0:4837c9695a02 1189 this->ballX--;
RLRiedinger 0:4837c9695a02 1190
RLRiedinger 0:4837c9695a02 1191 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1192
RLRiedinger 0:4837c9695a02 1193 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1194
RLRiedinger 0:4837c9695a02 1195 this->ballX++;
RLRiedinger 0:4837c9695a02 1196
RLRiedinger 0:4837c9695a02 1197 }
RLRiedinger 0:4837c9695a02 1198
RLRiedinger 0:4837c9695a02 1199 } else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
RLRiedinger 0:4837c9695a02 1200
RLRiedinger 0:4837c9695a02 1201 // 7 - No +Y (Down)
RLRiedinger 0:4837c9695a02 1202
RLRiedinger 0:4837c9695a02 1203 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1204
RLRiedinger 0:4837c9695a02 1205 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1206
RLRiedinger 0:4837c9695a02 1207 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1208
RLRiedinger 0:4837c9695a02 1209 this->ballX--;
RLRiedinger 0:4837c9695a02 1210
RLRiedinger 0:4837c9695a02 1211 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1212
RLRiedinger 0:4837c9695a02 1213 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1214
RLRiedinger 0:4837c9695a02 1215 this->ballX++;
RLRiedinger 0:4837c9695a02 1216
RLRiedinger 0:4837c9695a02 1217 }
RLRiedinger 0:4837c9695a02 1218
RLRiedinger 0:4837c9695a02 1219 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1220
RLRiedinger 0:4837c9695a02 1221 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1222
RLRiedinger 0:4837c9695a02 1223 this->ballY--;
RLRiedinger 0:4837c9695a02 1224
RLRiedinger 0:4837c9695a02 1225 }
RLRiedinger 0:4837c9695a02 1226
RLRiedinger 0:4837c9695a02 1227 } else {
RLRiedinger 0:4837c9695a02 1228
RLRiedinger 0:4837c9695a02 1229 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1230
RLRiedinger 0:4837c9695a02 1231 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1232
RLRiedinger 0:4837c9695a02 1233 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1234
RLRiedinger 0:4837c9695a02 1235 this->ballY--;
RLRiedinger 0:4837c9695a02 1236
RLRiedinger 0:4837c9695a02 1237 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1238
RLRiedinger 0:4837c9695a02 1239 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1240
RLRiedinger 0:4837c9695a02 1241 this->ballY++;
RLRiedinger 0:4837c9695a02 1242
RLRiedinger 0:4837c9695a02 1243 }
RLRiedinger 0:4837c9695a02 1244
RLRiedinger 0:4837c9695a02 1245 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1246
RLRiedinger 0:4837c9695a02 1247 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1248
RLRiedinger 0:4837c9695a02 1249 this->ballX--;
RLRiedinger 0:4837c9695a02 1250
RLRiedinger 0:4837c9695a02 1251 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1252
RLRiedinger 0:4837c9695a02 1253 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1254
RLRiedinger 0:4837c9695a02 1255 this->ballX++;
RLRiedinger 0:4837c9695a02 1256
RLRiedinger 0:4837c9695a02 1257 }
RLRiedinger 0:4837c9695a02 1258
RLRiedinger 0:4837c9695a02 1259 }
RLRiedinger 0:4837c9695a02 1260
RLRiedinger 0:4837c9695a02 1261 } else if (this->ballX - ROBOT::BALL_RADIUS == DisplayN18::WIDTH / 2 + BoxWidth / 2 + 1) {
RLRiedinger 0:4837c9695a02 1262
RLRiedinger 0:4837c9695a02 1263 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1264
RLRiedinger 0:4837c9695a02 1265 if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
RLRiedinger 0:4837c9695a02 1266
RLRiedinger 0:4837c9695a02 1267 // 2 - No -X (Left) and +Y (Down)
RLRiedinger 0:4837c9695a02 1268
RLRiedinger 0:4837c9695a02 1269 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1270
RLRiedinger 0:4837c9695a02 1271 if (!this->left.read() && this->down.read()) {
RLRiedinger 0:4837c9695a02 1272
RLRiedinger 0:4837c9695a02 1273 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1274
RLRiedinger 0:4837c9695a02 1275 this->ballX--;
RLRiedinger 0:4837c9695a02 1276
RLRiedinger 0:4837c9695a02 1277 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1278
RLRiedinger 0:4837c9695a02 1279 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1280
RLRiedinger 0:4837c9695a02 1281 this->ballX++;
RLRiedinger 0:4837c9695a02 1282
RLRiedinger 0:4837c9695a02 1283 }
RLRiedinger 0:4837c9695a02 1284
RLRiedinger 0:4837c9695a02 1285 if (!this->up.read() && this->left.read()) {
RLRiedinger 0:4837c9695a02 1286
RLRiedinger 0:4837c9695a02 1287 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1288
RLRiedinger 0:4837c9695a02 1289 this->ballY--;
RLRiedinger 0:4837c9695a02 1290
RLRiedinger 0:4837c9695a02 1291 } else if (!this->down.read() && this->left.read()) {
RLRiedinger 0:4837c9695a02 1292
RLRiedinger 0:4837c9695a02 1293 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1294
RLRiedinger 0:4837c9695a02 1295 this->ballY++;
RLRiedinger 0:4837c9695a02 1296
RLRiedinger 0:4837c9695a02 1297 }
RLRiedinger 0:4837c9695a02 1298
RLRiedinger 0:4837c9695a02 1299 } else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
RLRiedinger 0:4837c9695a02 1300 (this->ballY - ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
RLRiedinger 0:4837c9695a02 1301
RLRiedinger 0:4837c9695a02 1302 // 5 - No -X (Left)
RLRiedinger 0:4837c9695a02 1303
RLRiedinger 0:4837c9695a02 1304 if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1305
RLRiedinger 0:4837c9695a02 1306 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1307
RLRiedinger 0:4837c9695a02 1308 this->ballX++;
RLRiedinger 0:4837c9695a02 1309
RLRiedinger 0:4837c9695a02 1310 }
RLRiedinger 0:4837c9695a02 1311
RLRiedinger 0:4837c9695a02 1312 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1313
RLRiedinger 0:4837c9695a02 1314 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1315
RLRiedinger 0:4837c9695a02 1316 this->ballY--;
RLRiedinger 0:4837c9695a02 1317
RLRiedinger 0:4837c9695a02 1318 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1319
RLRiedinger 0:4837c9695a02 1320 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1321
RLRiedinger 0:4837c9695a02 1322 this->ballY++;
RLRiedinger 0:4837c9695a02 1323
RLRiedinger 0:4837c9695a02 1324 }
RLRiedinger 0:4837c9695a02 1325
RLRiedinger 0:4837c9695a02 1326 } else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
RLRiedinger 0:4837c9695a02 1327
RLRiedinger 0:4837c9695a02 1328 // 8 - No -X (Left) and +Y (Down)
RLRiedinger 0:4837c9695a02 1329
RLRiedinger 0:4837c9695a02 1330 if (!this->left.read() && this->down.read()) {
RLRiedinger 0:4837c9695a02 1331
RLRiedinger 0:4837c9695a02 1332 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1333
RLRiedinger 0:4837c9695a02 1334 this->ballX--;
RLRiedinger 0:4837c9695a02 1335
RLRiedinger 0:4837c9695a02 1336 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1337
RLRiedinger 0:4837c9695a02 1338 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1339
RLRiedinger 0:4837c9695a02 1340 this->ballX++;
RLRiedinger 0:4837c9695a02 1341
RLRiedinger 0:4837c9695a02 1342 }
RLRiedinger 0:4837c9695a02 1343
RLRiedinger 0:4837c9695a02 1344 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1345
RLRiedinger 0:4837c9695a02 1346 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1347
RLRiedinger 0:4837c9695a02 1348 this->ballY--;
RLRiedinger 0:4837c9695a02 1349
RLRiedinger 0:4837c9695a02 1350 } else if (!this->down.read() && this->left.read()) {
RLRiedinger 0:4837c9695a02 1351
RLRiedinger 0:4837c9695a02 1352 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1353
RLRiedinger 0:4837c9695a02 1354 this->ballY++;
RLRiedinger 0:4837c9695a02 1355
RLRiedinger 0:4837c9695a02 1356 }
RLRiedinger 0:4837c9695a02 1357
RLRiedinger 0:4837c9695a02 1358 } else {
RLRiedinger 0:4837c9695a02 1359
RLRiedinger 0:4837c9695a02 1360 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1361
RLRiedinger 0:4837c9695a02 1362 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1363
RLRiedinger 0:4837c9695a02 1364 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1365
RLRiedinger 0:4837c9695a02 1366 this->ballY--;
RLRiedinger 0:4837c9695a02 1367
RLRiedinger 0:4837c9695a02 1368 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1369
RLRiedinger 0:4837c9695a02 1370 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1371
RLRiedinger 0:4837c9695a02 1372 this->ballY++;
RLRiedinger 0:4837c9695a02 1373
RLRiedinger 0:4837c9695a02 1374 }
RLRiedinger 0:4837c9695a02 1375
RLRiedinger 0:4837c9695a02 1376 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1377
RLRiedinger 0:4837c9695a02 1378 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1379
RLRiedinger 0:4837c9695a02 1380 this->ballX--;
RLRiedinger 0:4837c9695a02 1381
RLRiedinger 0:4837c9695a02 1382 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1383
RLRiedinger 0:4837c9695a02 1384 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1385
RLRiedinger 0:4837c9695a02 1386 this->ballX++;
RLRiedinger 0:4837c9695a02 1387
RLRiedinger 0:4837c9695a02 1388 }
RLRiedinger 0:4837c9695a02 1389
RLRiedinger 0:4837c9695a02 1390
RLRiedinger 0:4837c9695a02 1391 }
RLRiedinger 0:4837c9695a02 1392
RLRiedinger 0:4837c9695a02 1393 } else {
RLRiedinger 0:4837c9695a02 1394
RLRiedinger 0:4837c9695a02 1395 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1396
RLRiedinger 0:4837c9695a02 1397 if (!this->up.read()) {
RLRiedinger 0:4837c9695a02 1398
RLRiedinger 0:4837c9695a02 1399 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1400
RLRiedinger 0:4837c9695a02 1401 this->ballY--;
RLRiedinger 0:4837c9695a02 1402
RLRiedinger 0:4837c9695a02 1403 } else if (!this->down.read()) {
RLRiedinger 0:4837c9695a02 1404
RLRiedinger 0:4837c9695a02 1405 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1406
RLRiedinger 0:4837c9695a02 1407 this->ballY++;
RLRiedinger 0:4837c9695a02 1408
RLRiedinger 0:4837c9695a02 1409 }
RLRiedinger 0:4837c9695a02 1410
RLRiedinger 0:4837c9695a02 1411 if (!this->left.read()) {
RLRiedinger 0:4837c9695a02 1412
RLRiedinger 0:4837c9695a02 1413 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1414
RLRiedinger 0:4837c9695a02 1415 this->ballX--;
RLRiedinger 0:4837c9695a02 1416
RLRiedinger 0:4837c9695a02 1417 } else if (!this->right.read()) {
RLRiedinger 0:4837c9695a02 1418
RLRiedinger 0:4837c9695a02 1419 debug_print2(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1420
RLRiedinger 0:4837c9695a02 1421 this->ballX++;
RLRiedinger 0:4837c9695a02 1422
RLRiedinger 0:4837c9695a02 1423 }
RLRiedinger 0:4837c9695a02 1424
RLRiedinger 0:4837c9695a02 1425 }
RLRiedinger 0:4837c9695a02 1426
RLRiedinger 0:4837c9695a02 1427 if (this->ballX - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 1428
RLRiedinger 0:4837c9695a02 1429 this->ballX = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 1430
RLRiedinger 0:4837c9695a02 1431 } else if (this->ballX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
RLRiedinger 0:4837c9695a02 1432
RLRiedinger 0:4837c9695a02 1433 this->ballX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 1434
RLRiedinger 0:4837c9695a02 1435 }
RLRiedinger 0:4837c9695a02 1436
RLRiedinger 0:4837c9695a02 1437 if (this->ballY - ROBOT::BALL_RADIUS < 0) {
RLRiedinger 0:4837c9695a02 1438
RLRiedinger 0:4837c9695a02 1439 this->ballY = ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 1440
RLRiedinger 0:4837c9695a02 1441 } else if (this->ballY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
RLRiedinger 0:4837c9695a02 1442
RLRiedinger 0:4837c9695a02 1443 this->ballY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 1444
RLRiedinger 0:4837c9695a02 1445 }
RLRiedinger 0:4837c9695a02 1446
RLRiedinger 0:4837c9695a02 1447 this->drawBall();
RLRiedinger 0:4837c9695a02 1448 this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 1449
RLRiedinger 0:4837c9695a02 1450 #endif
RLRiedinger 0:4837c9695a02 1451
RLRiedinger 0:4837c9695a02 1452 sprintf(FORMAT_BUFFER, "ReceiveTimer.read_ms() = %i", ReceiveTimer.read_ms());
RLRiedinger 0:4837c9695a02 1453 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 1454
RLRiedinger 0:4837c9695a02 1455 sprintf(FORMAT_BUFFER, "ButtonTimer.read_ms() = %i", ButtonTimer.read_ms());
RLRiedinger 0:4837c9695a02 1456 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 1457
RLRiedinger 0:4837c9695a02 1458 sprintf(FORMAT_BUFFER, "mIPD = %i", mIPD);
RLRiedinger 0:4837c9695a02 1459 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 1460
RLRiedinger 0:4837c9695a02 1461 sprintf(FORMAT_BUFFER, "mPROMPT = %i", mPROMPT);
RLRiedinger 0:4837c9695a02 1462 debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 1463
RLRiedinger 0:4837c9695a02 1464 }
RLRiedinger 0:4837c9695a02 1465
RLRiedinger 0:4837c9695a02 1466 if (ReceiveTimer.read_ms() > timeout) {
RLRiedinger 0:4837c9695a02 1467
RLRiedinger 0:4837c9695a02 1468 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1469
RLRiedinger 0:4837c9695a02 1470 return_code = ERROR;
RLRiedinger 0:4837c9695a02 1471
RLRiedinger 0:4837c9695a02 1472 } else if (!mIPD && !mPROMPT && (ButtonTimer.read_ms() > Button_Timeout)) {
RLRiedinger 0:4837c9695a02 1473
RLRiedinger 0:4837c9695a02 1474 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1475
RLRiedinger 0:4837c9695a02 1476 led1 = 0;
RLRiedinger 0:4837c9695a02 1477 serial_print1("AT+CIPSEND=9\r");
RLRiedinger 0:4837c9695a02 1478 led1 = 1;
RLRiedinger 0:4837c9695a02 1479 ButtonTimer.reset();
RLRiedinger 0:4837c9695a02 1480 ButtonTimer.stop();
RLRiedinger 0:4837c9695a02 1481 ReceiveTimer.reset();
RLRiedinger 0:4837c9695a02 1482
RLRiedinger 0:4837c9695a02 1483 }
RLRiedinger 0:4837c9695a02 1484
RLRiedinger 0:4837c9695a02 1485 }
RLRiedinger 0:4837c9695a02 1486
RLRiedinger 0:4837c9695a02 1487 ReceiveTimer.stop();
RLRiedinger 0:4837c9695a02 1488
RLRiedinger 0:4837c9695a02 1489 ButtonTimer.stop();
RLRiedinger 0:4837c9695a02 1490
RLRiedinger 0:4837c9695a02 1491 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1492
RLRiedinger 0:4837c9695a02 1493 return(return_code);
RLRiedinger 0:4837c9695a02 1494 }
RLRiedinger 0:4837c9695a02 1495
RLRiedinger 0:4837c9695a02 1496 void ROBOT::DrawBox(void) {
RLRiedinger 0:4837c9695a02 1497
RLRiedinger 0:4837c9695a02 1498 this->disp.drawLine(DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2,
RLRiedinger 0:4837c9695a02 1499 DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2, DisplayN18::GREEN);
RLRiedinger 0:4837c9695a02 1500
RLRiedinger 0:4837c9695a02 1501 this->disp.drawLine(DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2,
RLRiedinger 0:4837c9695a02 1502 DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2, DisplayN18::GREEN);
RLRiedinger 0:4837c9695a02 1503
RLRiedinger 0:4837c9695a02 1504 this->disp.drawLine(DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2,
RLRiedinger 0:4837c9695a02 1505 DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2, DisplayN18::GREEN);
RLRiedinger 0:4837c9695a02 1506
RLRiedinger 0:4837c9695a02 1507 this->disp.drawLine(DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2,
RLRiedinger 0:4837c9695a02 1508 DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2, DisplayN18::GREEN);
RLRiedinger 0:4837c9695a02 1509
RLRiedinger 0:4837c9695a02 1510 }
RLRiedinger 0:4837c9695a02 1511 int ROBOT::test(void) {
RLRiedinger 0:4837c9695a02 1512
RLRiedinger 0:4837c9695a02 1513 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1514
RLRiedinger 0:4837c9695a02 1515 First = 1;
RLRiedinger 0:4837c9695a02 1516
RLRiedinger 0:4837c9695a02 1517 this->circleX = DisplayN18::WIDTH / 2;
RLRiedinger 0:4837c9695a02 1518 this->circleY = 3 * DisplayN18::HEIGHT / 4;
RLRiedinger 0:4837c9695a02 1519
RLRiedinger 0:4837c9695a02 1520 this->ballX = DisplayN18::WIDTH / 2;
RLRiedinger 0:4837c9695a02 1521 this->ballY = 3 * DisplayN18::HEIGHT / 4;
RLRiedinger 0:4837c9695a02 1522
RLRiedinger 0:4837c9695a02 1523 this->disp.clear();
RLRiedinger 0:4837c9695a02 1524 this->drawBall();
RLRiedinger 0:4837c9695a02 1525
RLRiedinger 0:4837c9695a02 1526 this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 1527
RLRiedinger 0:4837c9695a02 1528 this->DrawBox();
RLRiedinger 0:4837c9695a02 1529
RLRiedinger 0:4837c9695a02 1530 int return_code;
RLRiedinger 0:4837c9695a02 1531
RLRiedinger 0:4837c9695a02 1532 reset_matchtab();
RLRiedinger 0:4837c9695a02 1533
RLRiedinger 0:4837c9695a02 1534 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1535
RLRiedinger 0:4837c9695a02 1536 blink(led1);
RLRiedinger 0:4837c9695a02 1537
RLRiedinger 0:4837c9695a02 1538 serial_print1("AT\r");
RLRiedinger 0:4837c9695a02 1539
RLRiedinger 0:4837c9695a02 1540 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1541
RLRiedinger 0:4837c9695a02 1542 if ((return_code = echo(echo_comands, 10000)) == OK) {
RLRiedinger 0:4837c9695a02 1543
RLRiedinger 0:4837c9695a02 1544 blink(led1);
RLRiedinger 0:4837c9695a02 1545
RLRiedinger 0:4837c9695a02 1546 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1547
RLRiedinger 0:4837c9695a02 1548 serial_print1("AT+CWMODE=1\r");
RLRiedinger 0:4837c9695a02 1549
RLRiedinger 0:4837c9695a02 1550 if ((return_code = echo(echo_comands, 10000)) == OK) {
RLRiedinger 0:4837c9695a02 1551
RLRiedinger 0:4837c9695a02 1552 blink(led1);
RLRiedinger 0:4837c9695a02 1553
RLRiedinger 0:4837c9695a02 1554 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1555
RLRiedinger 0:4837c9695a02 1556 sprintf(FORMAT_BUFFER, "AT+CWJAP=\"%s\",\"%s\"\r", SSID, PASSWORD);
RLRiedinger 0:4837c9695a02 1557 serial_print1(FORMAT_BUFFER);
RLRiedinger 0:4837c9695a02 1558
RLRiedinger 0:4837c9695a02 1559 if ((return_code = echo(echo_comands, 10000)) == OK) {
RLRiedinger 0:4837c9695a02 1560
RLRiedinger 0:4837c9695a02 1561 blink(led1);
RLRiedinger 0:4837c9695a02 1562
RLRiedinger 0:4837c9695a02 1563 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1564
RLRiedinger 0:4837c9695a02 1565 serial_print1("AT+CIPMUX=0\r");
RLRiedinger 0:4837c9695a02 1566
RLRiedinger 0:4837c9695a02 1567 if ((return_code = echo(echo_comands, 10000)) == OK) {
RLRiedinger 0:4837c9695a02 1568
RLRiedinger 0:4837c9695a02 1569 blink(led1);
RLRiedinger 0:4837c9695a02 1570
RLRiedinger 0:4837c9695a02 1571 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1572
RLRiedinger 0:4837c9695a02 1573 serial_bridge1.printf("AT+CIPSTART=\"TCP\",\"%s\",5001\r", Address);
RLRiedinger 0:4837c9695a02 1574
RLRiedinger 0:4837c9695a02 1575 if ((return_code = echo(echo_comands, 30000)) == OK) {
RLRiedinger 0:4837c9695a02 1576
RLRiedinger 0:4837c9695a02 1577 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1578
RLRiedinger 0:4837c9695a02 1579 for (int i = 0; i < 20; i++) {
RLRiedinger 0:4837c9695a02 1580
RLRiedinger 0:4837c9695a02 1581
RLRiedinger 0:4837c9695a02 1582 blink(led1);
RLRiedinger 0:4837c9695a02 1583 serial_bridge1.flush();
RLRiedinger 0:4837c9695a02 1584 wait(0.05);
RLRiedinger 0:4837c9695a02 1585
RLRiedinger 0:4837c9695a02 1586 }
RLRiedinger 0:4837c9695a02 1587
RLRiedinger 0:4837c9695a02 1588 return_code = terminal_emulator(echo_comands, 10000);
RLRiedinger 0:4837c9695a02 1589
RLRiedinger 0:4837c9695a02 1590 }
RLRiedinger 0:4837c9695a02 1591
RLRiedinger 0:4837c9695a02 1592 }
RLRiedinger 0:4837c9695a02 1593
RLRiedinger 0:4837c9695a02 1594 }
RLRiedinger 0:4837c9695a02 1595
RLRiedinger 0:4837c9695a02 1596 }
RLRiedinger 0:4837c9695a02 1597
RLRiedinger 0:4837c9695a02 1598 }
RLRiedinger 0:4837c9695a02 1599
RLRiedinger 0:4837c9695a02 1600 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1601
RLRiedinger 0:4837c9695a02 1602 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 1603
RLRiedinger 0:4837c9695a02 1604 if (return_code != QUIT) {
RLRiedinger 0:4837c9695a02 1605
RLRiedinger 0:4837c9695a02 1606 serial_print2("ERROR\r\n");
RLRiedinger 0:4837c9695a02 1607
RLRiedinger 0:4837c9695a02 1608 }
RLRiedinger 0:4837c9695a02 1609
RLRiedinger 0:4837c9695a02 1610 #endif
RLRiedinger 0:4837c9695a02 1611
RLRiedinger 0:4837c9695a02 1612 led1 = 1;
RLRiedinger 0:4837c9695a02 1613
RLRiedinger 0:4837c9695a02 1614 wait(1.0);
RLRiedinger 0:4837c9695a02 1615
RLRiedinger 0:4837c9695a02 1616 return return_code;
RLRiedinger 0:4837c9695a02 1617
RLRiedinger 0:4837c9695a02 1618 }
RLRiedinger 0:4837c9695a02 1619
RLRiedinger 0:4837c9695a02 1620 void ROBOT::test2(void) {
RLRiedinger 0:4837c9695a02 1621
RLRiedinger 0:4837c9695a02 1622 while (1) {
RLRiedinger 0:4837c9695a02 1623
RLRiedinger 0:4837c9695a02 1624 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1625
RLRiedinger 0:4837c9695a02 1626 }
RLRiedinger 0:4837c9695a02 1627
RLRiedinger 0:4837c9695a02 1628 }
RLRiedinger 0:4837c9695a02 1629
RLRiedinger 0:4837c9695a02 1630 int ROBOT::get_options(void) {
RLRiedinger 0:4837c9695a02 1631
RLRiedinger 0:4837c9695a02 1632 #ifndef AUTO_START
RLRiedinger 0:4837c9695a02 1633
RLRiedinger 0:4837c9695a02 1634 int return_code;
RLRiedinger 0:4837c9695a02 1635
RLRiedinger 0:4837c9695a02 1636 bool running = true;
RLRiedinger 0:4837c9695a02 1637 bool running_test = true;
RLRiedinger 0:4837c9695a02 1638 char command, ch;
RLRiedinger 0:4837c9695a02 1639
RLRiedinger 0:4837c9695a02 1640 while (!serial_bridge2.readable());
RLRiedinger 0:4837c9695a02 1641
RLRiedinger 0:4837c9695a02 1642 command = serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 1643
RLRiedinger 0:4837c9695a02 1644 #if defined(TARGET_LPC11U24)
RLRiedinger 0:4837c9695a02 1645 serial_print2("\r\nHello World from LPC11U24\r\n");
RLRiedinger 0:4837c9695a02 1646 #endif
RLRiedinger 0:4837c9695a02 1647
RLRiedinger 0:4837c9695a02 1648 #if defined(TARGET_LPC1768)
RLRiedinger 0:4837c9695a02 1649 serial_print2("\r\nHello World from LPC1768\r\n");
RLRiedinger 0:4837c9695a02 1650 #endif
RLRiedinger 0:4837c9695a02 1651
RLRiedinger 0:4837c9695a02 1652 #if defined(TARGET_KL25Z)
RLRiedinger 0:4837c9695a02 1653 serial_print2("\r\nHello World from KL25Z\r\n");
RLRiedinger 0:4837c9695a02 1654 #endif
RLRiedinger 0:4837c9695a02 1655
RLRiedinger 0:4837c9695a02 1656 #if defined(TARGET_LPC812)
RLRiedinger 0:4837c9695a02 1657 serial_print2("\r\nHello World from LPC812\r\n");
RLRiedinger 0:4837c9695a02 1658 #endif
RLRiedinger 0:4837c9695a02 1659
RLRiedinger 0:4837c9695a02 1660 int i = 0;
RLRiedinger 0:4837c9695a02 1661
RLRiedinger 0:4837c9695a02 1662 #endif
RLRiedinger 0:4837c9695a02 1663
RLRiedinger 0:4837c9695a02 1664 heartbeat_start();
RLRiedinger 0:4837c9695a02 1665
RLRiedinger 0:4837c9695a02 1666 // myled1 = 1; // LED Off
RLRiedinger 0:4837c9695a02 1667
RLRiedinger 0:4837c9695a02 1668 // We need to enable flow control or we overflow buffers and
RLRiedinger 0:4837c9695a02 1669 // lose data when used with the WiFly. Note that flow control
RLRiedinger 0:4837c9695a02 1670 // needs to be enabled on the WiFly for this to work but it's
RLRiedinger 0:4837c9695a02 1671 // possible to do that with flow control enabled here but not
RLRiedinger 0:4837c9695a02 1672 // there.
RLRiedinger 0:4837c9695a02 1673
RLRiedinger 0:4837c9695a02 1674 // serial_bridge1.set_flow_control(SC16IS750::RTSCTS);
RLRiedinger 0:4837c9695a02 1675
RLRiedinger 0:4837c9695a02 1676 serial_bridge1.ioSetDirection(0xFF); // All outputs
RLRiedinger 0:4837c9695a02 1677 serial_bridge1.ioSetState(0xFF); // All On
RLRiedinger 0:4837c9695a02 1678
RLRiedinger 0:4837c9695a02 1679 #ifdef AUTO_START
RLRiedinger 0:4837c9695a02 1680
RLRiedinger 0:4837c9695a02 1681 Address = Address_T;
RLRiedinger 0:4837c9695a02 1682
RLRiedinger 0:4837c9695a02 1683 while (1) {
RLRiedinger 0:4837c9695a02 1684
RLRiedinger 0:4837c9695a02 1685 test();
RLRiedinger 0:4837c9695a02 1686
RLRiedinger 0:4837c9695a02 1687 }
RLRiedinger 0:4837c9695a02 1688
RLRiedinger 0:4837c9695a02 1689 #else
RLRiedinger 0:4837c9695a02 1690
RLRiedinger 0:4837c9695a02 1691 show_menu();
RLRiedinger 0:4837c9695a02 1692
RLRiedinger 0:4837c9695a02 1693 while (serial_bridge2.writableCount() < 64);
RLRiedinger 0:4837c9695a02 1694
RLRiedinger 0:4837c9695a02 1695 serial_bridge2.flush();
RLRiedinger 0:4837c9695a02 1696
RLRiedinger 0:4837c9695a02 1697 serial_print2(">");
RLRiedinger 0:4837c9695a02 1698
RLRiedinger 0:4837c9695a02 1699 while (running) {
RLRiedinger 0:4837c9695a02 1700
RLRiedinger 0:4837c9695a02 1701 if (serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 1702
RLRiedinger 0:4837c9695a02 1703 command = serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 1704
RLRiedinger 0:4837c9695a02 1705 serial_bridge2.printf("command = %c\n\r", command);
RLRiedinger 0:4837c9695a02 1706
RLRiedinger 0:4837c9695a02 1707 switch (command) {
RLRiedinger 0:4837c9695a02 1708
RLRiedinger 0:4837c9695a02 1709 case '0' :
RLRiedinger 0:4837c9695a02 1710
RLRiedinger 0:4837c9695a02 1711 serial_print2("Done\n\r");
RLRiedinger 0:4837c9695a02 1712 running = false;
RLRiedinger 0:4837c9695a02 1713
RLRiedinger 0:4837c9695a02 1714 break;
RLRiedinger 0:4837c9695a02 1715
RLRiedinger 0:4837c9695a02 1716 case '1' :
RLRiedinger 0:4837c9695a02 1717
RLRiedinger 0:4837c9695a02 1718 show_menu();
RLRiedinger 0:4837c9695a02 1719
RLRiedinger 0:4837c9695a02 1720 break;
RLRiedinger 0:4837c9695a02 1721
RLRiedinger 0:4837c9695a02 1722 case '2' :
RLRiedinger 0:4837c9695a02 1723
RLRiedinger 0:4837c9695a02 1724 serial_print2("Hardware Reset\n\r");
RLRiedinger 0:4837c9695a02 1725
RLRiedinger 0:4837c9695a02 1726 serial_bridge1.hwReset(); // test
RLRiedinger 0:4837c9695a02 1727
RLRiedinger 0:4837c9695a02 1728 serial_print2("Init\n\r");
RLRiedinger 0:4837c9695a02 1729
RLRiedinger 0:4837c9695a02 1730 serial_bridge1._init();
RLRiedinger 0:4837c9695a02 1731
RLRiedinger 0:4837c9695a02 1732 ESP8266_reset();
RLRiedinger 0:4837c9695a02 1733
RLRiedinger 0:4837c9695a02 1734 serial_print2("Hardware Init done\n\r");
RLRiedinger 0:4837c9695a02 1735
RLRiedinger 0:4837c9695a02 1736 break;
RLRiedinger 0:4837c9695a02 1737
RLRiedinger 0:4837c9695a02 1738 case '3' :
RLRiedinger 0:4837c9695a02 1739
RLRiedinger 0:4837c9695a02 1740 serial_print2("IO Port Out\n\r");
RLRiedinger 0:4837c9695a02 1741
RLRiedinger 0:4837c9695a02 1742 i = 0;
RLRiedinger 0:4837c9695a02 1743
RLRiedinger 0:4837c9695a02 1744 while (!serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 1745
RLRiedinger 0:4837c9695a02 1746 serial_bridge1.ioSetState(i);
RLRiedinger 0:4837c9695a02 1747
RLRiedinger 0:4837c9695a02 1748 // serial_bridge1.ioGetState() ; // test
RLRiedinger 0:4837c9695a02 1749 // wait(0.5);
RLRiedinger 0:4837c9695a02 1750 // pc.putc('*');
RLRiedinger 0:4837c9695a02 1751
RLRiedinger 0:4837c9695a02 1752 i = (i + 1) & 0xFF;
RLRiedinger 0:4837c9695a02 1753
RLRiedinger 0:4837c9695a02 1754 }
RLRiedinger 0:4837c9695a02 1755
RLRiedinger 0:4837c9695a02 1756 serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 1757 serial_print2("IO Port Out Done\n\r");
RLRiedinger 0:4837c9695a02 1758
RLRiedinger 0:4837c9695a02 1759 break;
RLRiedinger 0:4837c9695a02 1760
RLRiedinger 0:4837c9695a02 1761 case '4' :
RLRiedinger 0:4837c9695a02 1762
RLRiedinger 0:4837c9695a02 1763 serial_print2("Transparant Mode, Enter '#' to quit...\n\r");
RLRiedinger 0:4837c9695a02 1764
RLRiedinger 0:4837c9695a02 1765 running_test = true;
RLRiedinger 0:4837c9695a02 1766
RLRiedinger 0:4837c9695a02 1767 while (running_test) {
RLRiedinger 0:4837c9695a02 1768
RLRiedinger 0:4837c9695a02 1769 // From SPI/I2C to serial
RLRiedinger 0:4837c9695a02 1770
RLRiedinger 0:4837c9695a02 1771 while (serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 1772
RLRiedinger 0:4837c9695a02 1773 ch = serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 1774 running_test = (ch != '#');
RLRiedinger 0:4837c9695a02 1775 serial_bridge1.putc(ch);
RLRiedinger 0:4837c9695a02 1776
RLRiedinger 0:4837c9695a02 1777 }
RLRiedinger 0:4837c9695a02 1778
RLRiedinger 0:4837c9695a02 1779 // From Serial to SPI/I2C
RLRiedinger 0:4837c9695a02 1780
RLRiedinger 0:4837c9695a02 1781 while (running_test && serial_bridge1.readable()) {
RLRiedinger 0:4837c9695a02 1782
RLRiedinger 0:4837c9695a02 1783 ch = serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 1784 running_test = (ch != '#');
RLRiedinger 0:4837c9695a02 1785 serial_bridge2.putc(ch);
RLRiedinger 0:4837c9695a02 1786
RLRiedinger 0:4837c9695a02 1787 }
RLRiedinger 0:4837c9695a02 1788
RLRiedinger 0:4837c9695a02 1789 }
RLRiedinger 0:4837c9695a02 1790
RLRiedinger 0:4837c9695a02 1791 serial_print2("\n\rTransparant Mode done\n\r");
RLRiedinger 0:4837c9695a02 1792
RLRiedinger 0:4837c9695a02 1793 break;
RLRiedinger 0:4837c9695a02 1794
RLRiedinger 0:4837c9695a02 1795 case '5' :
RLRiedinger 0:4837c9695a02 1796
RLRiedinger 0:4837c9695a02 1797 serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1798 serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount());
RLRiedinger 0:4837c9695a02 1799
RLRiedinger 0:4837c9695a02 1800 serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1801 serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
RLRiedinger 0:4837c9695a02 1802
RLRiedinger 0:4837c9695a02 1803 break;
RLRiedinger 0:4837c9695a02 1804
RLRiedinger 0:4837c9695a02 1805 #if (0)
RLRiedinger 0:4837c9695a02 1806
RLRiedinger 0:4837c9695a02 1807 case '6' :
RLRiedinger 0:4837c9695a02 1808
RLRiedinger 0:4837c9695a02 1809 serial_print2("Enable RTS/CTS\n\r");
RLRiedinger 0:4837c9695a02 1810 serial_bridge1.set_flow_control(SC16IS750::RTSCTS);
RLRiedinger 0:4837c9695a02 1811
RLRiedinger 0:4837c9695a02 1812 break;
RLRiedinger 0:4837c9695a02 1813
RLRiedinger 0:4837c9695a02 1814 case '7' :
RLRiedinger 0:4837c9695a02 1815
RLRiedinger 0:4837c9695a02 1816 serial_print2("Disable RTS/CTS\n\r");
RLRiedinger 0:4837c9695a02 1817 serial_bridge1.set_flow_control(SC16IS750::Disabled);
RLRiedinger 0:4837c9695a02 1818
RLRiedinger 0:4837c9695a02 1819 break;
RLRiedinger 0:4837c9695a02 1820
RLRiedinger 0:4837c9695a02 1821 case '8' :
RLRiedinger 0:4837c9695a02 1822
RLRiedinger 0:4837c9695a02 1823 serial_print2("Write block\n\r");
RLRiedinger 0:4837c9695a02 1824 serial_bridge1.writeString("Hello World from mbed and SC16IS750 ");
RLRiedinger 0:4837c9695a02 1825
RLRiedinger 0:4837c9695a02 1826 break;
RLRiedinger 0:4837c9695a02 1827
RLRiedinger 0:4837c9695a02 1828 case '9' :
RLRiedinger 0:4837c9695a02 1829
RLRiedinger 0:4837c9695a02 1830 serial_print2("Baudrate = 9600\n\r");
RLRiedinger 0:4837c9695a02 1831 serial_bridge2.baud(14745600UL, 9600);
RLRiedinger 0:4837c9695a02 1832
RLRiedinger 0:4837c9695a02 1833 break;
RLRiedinger 0:4837c9695a02 1834
RLRiedinger 0:4837c9695a02 1835 case 'A' :
RLRiedinger 0:4837c9695a02 1836
RLRiedinger 0:4837c9695a02 1837 serial_print2("Baudrate = 115200\n\r");
RLRiedinger 0:4837c9695a02 1838 serial_bridge2.baud(14745600UL, 115200);
RLRiedinger 0:4837c9695a02 1839
RLRiedinger 0:4837c9695a02 1840 break;
RLRiedinger 0:4837c9695a02 1841
RLRiedinger 0:4837c9695a02 1842 case 'B' :
RLRiedinger 0:4837c9695a02 1843 serial_print2("Transparent Mode with buffer display, Enter '#' to quit...\n\r");
RLRiedinger 0:4837c9695a02 1844
RLRiedinger 0:4837c9695a02 1845 running_test=true;
RLRiedinger 0:4837c9695a02 1846
RLRiedinger 0:4837c9695a02 1847 while (running_test) {
RLRiedinger 0:4837c9695a02 1848
RLRiedinger 0:4837c9695a02 1849 // From SPI/I2C to serial
RLRiedinger 0:4837c9695a02 1850
RLRiedinger 0:4837c9695a02 1851 while (running_test && serial_bridge2.readable()) {
RLRiedinger 0:4837c9695a02 1852
RLRiedinger 0:4837c9695a02 1853 ch = serial_bridge2.getc();
RLRiedinger 0:4837c9695a02 1854 running_test = (ch != '#');
RLRiedinger 0:4837c9695a02 1855 serial_bridge1.putc(ch);
RLRiedinger 0:4837c9695a02 1856
RLRiedinger 0:4837c9695a02 1857 // Show buffers when character was entered
RLRiedinger 0:4837c9695a02 1858
RLRiedinger 0:4837c9695a02 1859 serial_print2("\n\r");
RLRiedinger 0:4837c9695a02 1860 serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1861 serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount());
RLRiedinger 0:4837c9695a02 1862
RLRiedinger 0:4837c9695a02 1863 serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1864 serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
RLRiedinger 0:4837c9695a02 1865
RLRiedinger 0:4837c9695a02 1866 }
RLRiedinger 0:4837c9695a02 1867
RLRiedinger 0:4837c9695a02 1868 // From Serial to SPI/I2C
RLRiedinger 0:4837c9695a02 1869
RLRiedinger 0:4837c9695a02 1870 while (running_test && serial_bridge1.readable()) {
RLRiedinger 0:4837c9695a02 1871
RLRiedinger 0:4837c9695a02 1872 ch = serial_bridge1.getc();
RLRiedinger 0:4837c9695a02 1873 running_test = (ch != '#');
RLRiedinger 0:4837c9695a02 1874 serial_bridge2.putc(ch);
RLRiedinger 0:4837c9695a02 1875
RLRiedinger 0:4837c9695a02 1876 }
RLRiedinger 0:4837c9695a02 1877
RLRiedinger 0:4837c9695a02 1878 }
RLRiedinger 0:4837c9695a02 1879
RLRiedinger 0:4837c9695a02 1880 serial_print2("\n\rTransparant Mode done\n\r");
RLRiedinger 0:4837c9695a02 1881
RLRiedinger 0:4837c9695a02 1882 break;
RLRiedinger 0:4837c9695a02 1883
RLRiedinger 0:4837c9695a02 1884 #endif
RLRiedinger 0:4837c9695a02 1885
RLRiedinger 0:4837c9695a02 1886 #if (0)
RLRiedinger 0:4837c9695a02 1887
RLRiedinger 0:4837c9695a02 1888 case 'C' :
RLRiedinger 0:4837c9695a02 1889
RLRiedinger 0:4837c9695a02 1890 serial_print2("Test printf()\n\r");
RLRiedinger 0:4837c9695a02 1891
RLRiedinger 0:4837c9695a02 1892 serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1893 serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount() );
RLRiedinger 0:4837c9695a02 1894
RLRiedinger 0:4837c9695a02 1895 serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
RLRiedinger 0:4837c9695a02 1896 serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
RLRiedinger 0:4837c9695a02 1897
RLRiedinger 0:4837c9695a02 1898 serial_print2("\n\rTest printf() done\n\r");
RLRiedinger 0:4837c9695a02 1899
RLRiedinger 0:4837c9695a02 1900 break;
RLRiedinger 0:4837c9695a02 1901
RLRiedinger 0:4837c9695a02 1902 #endif
RLRiedinger 0:4837c9695a02 1903
RLRiedinger 0:4837c9695a02 1904 case 'D' :
RLRiedinger 0:4837c9695a02 1905
RLRiedinger 0:4837c9695a02 1906 serial_print2("Test connected\n\r");
RLRiedinger 0:4837c9695a02 1907
RLRiedinger 0:4837c9695a02 1908 if (!serial_bridge1.connected()){
RLRiedinger 0:4837c9695a02 1909
RLRiedinger 0:4837c9695a02 1910 serial_print2("Failed to detect UART bridge\r\n");
RLRiedinger 0:4837c9695a02 1911
RLRiedinger 0:4837c9695a02 1912 } else {
RLRiedinger 0:4837c9695a02 1913
RLRiedinger 0:4837c9695a02 1914 serial_print2("Found UART bridge!\r\n");
RLRiedinger 0:4837c9695a02 1915
RLRiedinger 0:4837c9695a02 1916 }
RLRiedinger 0:4837c9695a02 1917
RLRiedinger 0:4837c9695a02 1918 break;
RLRiedinger 0:4837c9695a02 1919
RLRiedinger 0:4837c9695a02 1920 case 'T' :
RLRiedinger 0:4837c9695a02 1921
RLRiedinger 0:4837c9695a02 1922 serial_print2("Test ESP8266 192.168.0.6\n\r");
RLRiedinger 0:4837c9695a02 1923
RLRiedinger 0:4837c9695a02 1924 Address = Address_T;
RLRiedinger 0:4837c9695a02 1925 return_code = test();
RLRiedinger 0:4837c9695a02 1926
RLRiedinger 0:4837c9695a02 1927 serial_bridge2.printf("return_code = %6i\r\n", return_code);
RLRiedinger 0:4837c9695a02 1928
RLRiedinger 0:4837c9695a02 1929
RLRiedinger 0:4837c9695a02 1930 break;
RLRiedinger 0:4837c9695a02 1931
RLRiedinger 0:4837c9695a02 1932 #if (0)
RLRiedinger 0:4837c9695a02 1933
RLRiedinger 0:4837c9695a02 1934 case 'U' :
RLRiedinger 0:4837c9695a02 1935
RLRiedinger 0:4837c9695a02 1936 serial_print2("Test ESP8266 192.168.0.14\n\r");
RLRiedinger 0:4837c9695a02 1937
RLRiedinger 0:4837c9695a02 1938 Address = Address_U;
RLRiedinger 0:4837c9695a02 1939 return_code = test();
RLRiedinger 0:4837c9695a02 1940
RLRiedinger 0:4837c9695a02 1941 serial_bridge2.printf("return_code = %6i\r\n", return_code);
RLRiedinger 0:4837c9695a02 1942
RLRiedinger 0:4837c9695a02 1943
RLRiedinger 0:4837c9695a02 1944 break;
RLRiedinger 0:4837c9695a02 1945
RLRiedinger 0:4837c9695a02 1946 case 'V' :
RLRiedinger 0:4837c9695a02 1947
RLRiedinger 0:4837c9695a02 1948 serial_print2("Test ESP8266 192.168.0.22\n\r");
RLRiedinger 0:4837c9695a02 1949
RLRiedinger 0:4837c9695a02 1950 Address = Address_V;
RLRiedinger 0:4837c9695a02 1951 return_code = test();
RLRiedinger 0:4837c9695a02 1952
RLRiedinger 0:4837c9695a02 1953 serial_bridge2.printf("return_code = %6i\r\n", return_code);
RLRiedinger 0:4837c9695a02 1954
RLRiedinger 0:4837c9695a02 1955
RLRiedinger 0:4837c9695a02 1956 break;
RLRiedinger 0:4837c9695a02 1957
RLRiedinger 0:4837c9695a02 1958 case 'W' :
RLRiedinger 0:4837c9695a02 1959
RLRiedinger 0:4837c9695a02 1960 serial_print2("Test ESP8266 192.168.0.23\n\r");
RLRiedinger 0:4837c9695a02 1961
RLRiedinger 0:4837c9695a02 1962 Address = Address_W;
RLRiedinger 0:4837c9695a02 1963 return_code = test();
RLRiedinger 0:4837c9695a02 1964
RLRiedinger 0:4837c9695a02 1965 serial_bridge2.printf("return_code = %6i\r\n", return_code);
RLRiedinger 0:4837c9695a02 1966
RLRiedinger 0:4837c9695a02 1967 break;
RLRiedinger 0:4837c9695a02 1968
RLRiedinger 0:4837c9695a02 1969 #endif
RLRiedinger 0:4837c9695a02 1970
RLRiedinger 0:4837c9695a02 1971 case 'W' :
RLRiedinger 0:4837c9695a02 1972
RLRiedinger 0:4837c9695a02 1973 serial_print2("Toggle ECHO mode\n\r");
RLRiedinger 0:4837c9695a02 1974
RLRiedinger 0:4837c9695a02 1975 echo_comands = ~echo_comands;
RLRiedinger 0:4837c9695a02 1976
RLRiedinger 0:4837c9695a02 1977 serial_bridge2.printf("ECHO is %s\n\r", echo_comands ? "on" : "off");
RLRiedinger 0:4837c9695a02 1978
RLRiedinger 0:4837c9695a02 1979 break;
RLRiedinger 0:4837c9695a02 1980
RLRiedinger 0:4837c9695a02 1981 case 'Z' :
RLRiedinger 0:4837c9695a02 1982
RLRiedinger 0:4837c9695a02 1983 serial_print2("Toggle DEBUG mode\n\r");
RLRiedinger 0:4837c9695a02 1984
RLRiedinger 0:4837c9695a02 1985 DEBUG = ~DEBUG;
RLRiedinger 0:4837c9695a02 1986
RLRiedinger 0:4837c9695a02 1987 serial_bridge2.printf("DEBUG is %s\n\r", DEBUG ? "on" : "off");
RLRiedinger 0:4837c9695a02 1988
RLRiedinger 0:4837c9695a02 1989 while (0) {
RLRiedinger 0:4837c9695a02 1990
RLRiedinger 0:4837c9695a02 1991 serial_bridge2.printf("%6d %s\n\r", __LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1992
RLRiedinger 0:4837c9695a02 1993 debug_print(__LINE__, __FUNCTION__);
RLRiedinger 0:4837c9695a02 1994
RLRiedinger 0:4837c9695a02 1995 }
RLRiedinger 0:4837c9695a02 1996
RLRiedinger 0:4837c9695a02 1997 break;
RLRiedinger 0:4837c9695a02 1998
RLRiedinger 0:4837c9695a02 1999 default :
RLRiedinger 0:4837c9695a02 2000
RLRiedinger 0:4837c9695a02 2001 break;
RLRiedinger 0:4837c9695a02 2002
RLRiedinger 0:4837c9695a02 2003 } // switch
RLRiedinger 0:4837c9695a02 2004
RLRiedinger 0:4837c9695a02 2005 serial_print2(">");
RLRiedinger 0:4837c9695a02 2006
RLRiedinger 0:4837c9695a02 2007 } // if
RLRiedinger 0:4837c9695a02 2008
RLRiedinger 0:4837c9695a02 2009 } // while
RLRiedinger 0:4837c9695a02 2010
RLRiedinger 0:4837c9695a02 2011 #endif
RLRiedinger 0:4837c9695a02 2012
RLRiedinger 0:4837c9695a02 2013 return(0);
RLRiedinger 0:4837c9695a02 2014
RLRiedinger 0:4837c9695a02 2015 }
RLRiedinger 0:4837c9695a02 2016
RLRiedinger 0:4837c9695a02 2017 double ROBOT::convert(char* buffer) {
RLRiedinger 0:4837c9695a02 2018
RLRiedinger 0:4837c9695a02 2019 double val = ((buffer[0] << 2) | (buffer[1] >> 6));
RLRiedinger 0:4837c9695a02 2020
RLRiedinger 0:4837c9695a02 2021 if (val > 511.0)
RLRiedinger 0:4837c9695a02 2022 val -= 1024.0;
RLRiedinger 0:4837c9695a02 2023
RLRiedinger 0:4837c9695a02 2024 return val / 512.0;
RLRiedinger 0:4837c9695a02 2025
RLRiedinger 0:4837c9695a02 2026 }
RLRiedinger 0:4837c9695a02 2027
RLRiedinger 0:4837c9695a02 2028 void ROBOT::printDouble(double value, int x, int y) {
RLRiedinger 0:4837c9695a02 2029
RLRiedinger 0:4837c9695a02 2030 char buffer[10];
RLRiedinger 0:4837c9695a02 2031 int len = sprintf(buffer, "%.1f", value);
RLRiedinger 0:4837c9695a02 2032
RLRiedinger 0:4837c9695a02 2033 this->disp.drawString(x, y, buffer, DisplayN18::WHITE, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 2034
RLRiedinger 0:4837c9695a02 2035 }
RLRiedinger 0:4837c9695a02 2036
RLRiedinger 0:4837c9695a02 2037 void ROBOT::drawAxes() {
RLRiedinger 0:4837c9695a02 2038
RLRiedinger 0:4837c9695a02 2039 for (int i = 0; i < 3; i++) {
RLRiedinger 0:4837c9695a02 2040
RLRiedinger 0:4837c9695a02 2041 this->disp.drawLine(0, i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING), 0,
RLRiedinger 0:4837c9695a02 2042 i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 2043
RLRiedinger 0:4837c9695a02 2044 this->disp.drawLine(0, i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT / 2, DisplayN18::WIDTH,
RLRiedinger 0:4837c9695a02 2045 i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT / 2, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 2046
RLRiedinger 0:4837c9695a02 2047 }
RLRiedinger 0:4837c9695a02 2048
RLRiedinger 0:4837c9695a02 2049 }
RLRiedinger 0:4837c9695a02 2050
RLRiedinger 0:4837c9695a02 2051 void ROBOT::drawPoint(int axis, double value) {
RLRiedinger 0:4837c9695a02 2052
RLRiedinger 0:4837c9695a02 2053 if (value < -1.0)
RLRiedinger 0:4837c9695a02 2054 value = -1.0;
RLRiedinger 0:4837c9695a02 2055
RLRiedinger 0:4837c9695a02 2056 if (value > 1.0)
RLRiedinger 0:4837c9695a02 2057 value = 1.0;
RLRiedinger 0:4837c9695a02 2058
RLRiedinger 0:4837c9695a02 2059 value += 1.0;
RLRiedinger 0:4837c9695a02 2060 value /= 2.0;
RLRiedinger 0:4837c9695a02 2061 value = 1.0 - value;
RLRiedinger 0:4837c9695a02 2062 value *= ROBOT::GRAPH_HEIGHT;
RLRiedinger 0:4837c9695a02 2063
RLRiedinger 0:4837c9695a02 2064 this->disp.setPixel(this->graphX, axis * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + (int)value, this->colors[axis]);
RLRiedinger 0:4837c9695a02 2065
RLRiedinger 0:4837c9695a02 2066 }
RLRiedinger 0:4837c9695a02 2067
RLRiedinger 0:4837c9695a02 2068 void ROBOT::checkGraphReset() {
RLRiedinger 0:4837c9695a02 2069
RLRiedinger 0:4837c9695a02 2070 if (this->graphX > DisplayN18::WIDTH) {
RLRiedinger 0:4837c9695a02 2071
RLRiedinger 0:4837c9695a02 2072 this->graphX = 0;
RLRiedinger 0:4837c9695a02 2073 this->disp.clear();
RLRiedinger 0:4837c9695a02 2074 this->drawAxes();
RLRiedinger 0:4837c9695a02 2075
RLRiedinger 0:4837c9695a02 2076 }
RLRiedinger 0:4837c9695a02 2077
RLRiedinger 0:4837c9695a02 2078 }
RLRiedinger 0:4837c9695a02 2079
RLRiedinger 0:4837c9695a02 2080 void ROBOT::initialize() {
RLRiedinger 0:4837c9695a02 2081
RLRiedinger 0:4837c9695a02 2082 this->initializeBall();
RLRiedinger 0:4837c9695a02 2083
RLRiedinger 0:4837c9695a02 2084 this->paddleX = DisplayN18::WIDTH / 2 - ROBOT::PADDLE_WIDTH / 2;
RLRiedinger 0:4837c9695a02 2085 this->pwmTicksLeft = 0;
RLRiedinger 0:4837c9695a02 2086 this->lives = 4;
RLRiedinger 0:4837c9695a02 2087
RLRiedinger 0:4837c9695a02 2088 this->pwm.period_ms(1);
RLRiedinger 0:4837c9695a02 2089 this->pwm.write(0.00);
RLRiedinger 0:4837c9695a02 2090
RLRiedinger 0:4837c9695a02 2091 this->disp.clear();
RLRiedinger 0:4837c9695a02 2092
RLRiedinger 0:4837c9695a02 2093 }
RLRiedinger 0:4837c9695a02 2094
RLRiedinger 0:4837c9695a02 2095 void ROBOT::initializeBall() {
RLRiedinger 0:4837c9695a02 2096
RLRiedinger 0:4837c9695a02 2097 this->ballX = DisplayN18::WIDTH / 2 - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 2098 this->ballY = DisplayN18::HEIGHT / 4 - ROBOT::BALL_RADIUS;
RLRiedinger 0:4837c9695a02 2099
RLRiedinger 0:4837c9695a02 2100 this->ballSpeedX = rand() % 2 ? 1 : -1;
RLRiedinger 0:4837c9695a02 2101 this->ballSpeedY = rand() % 2 ? 1 : -1;
RLRiedinger 0:4837c9695a02 2102
RLRiedinger 0:4837c9695a02 2103 }
RLRiedinger 0:4837c9695a02 2104
RLRiedinger 0:4837c9695a02 2105 void ROBOT::tick() {
RLRiedinger 0:4837c9695a02 2106
RLRiedinger 0:4837c9695a02 2107 this->checkButtons();
RLRiedinger 0:4837c9695a02 2108
RLRiedinger 0:4837c9695a02 2109 this->clearPaddle();
RLRiedinger 0:4837c9695a02 2110 this->clearBall();
RLRiedinger 0:4837c9695a02 2111
RLRiedinger 0:4837c9695a02 2112 this->updatePaddle();
RLRiedinger 0:4837c9695a02 2113 this->updateBall();
RLRiedinger 0:4837c9695a02 2114
RLRiedinger 0:4837c9695a02 2115 this->checkCollision();
RLRiedinger 0:4837c9695a02 2116
RLRiedinger 0:4837c9695a02 2117 this->drawPaddle();
RLRiedinger 0:4837c9695a02 2118 this->drawBall();
RLRiedinger 0:4837c9695a02 2119
RLRiedinger 0:4837c9695a02 2120 this->checkPwm();
RLRiedinger 0:4837c9695a02 2121
RLRiedinger 0:4837c9695a02 2122 this->checkLives();
RLRiedinger 0:4837c9695a02 2123
RLRiedinger 0:4837c9695a02 2124 wait_ms(25);
RLRiedinger 0:4837c9695a02 2125
RLRiedinger 0:4837c9695a02 2126 }
RLRiedinger 0:4837c9695a02 2127
RLRiedinger 0:4837c9695a02 2128 void ROBOT::checkButtons() {
RLRiedinger 0:4837c9695a02 2129
RLRiedinger 0:4837c9695a02 2130 if (!this->square.read()) {
RLRiedinger 0:4837c9695a02 2131
RLRiedinger 0:4837c9695a02 2132 this->mode = !this->mode;
RLRiedinger 0:4837c9695a02 2133
RLRiedinger 0:4837c9695a02 2134 this->disp.clear();
RLRiedinger 0:4837c9695a02 2135
RLRiedinger 0:4837c9695a02 2136 if (!this->mode) {
RLRiedinger 0:4837c9695a02 2137
RLRiedinger 0:4837c9695a02 2138 this->graphX = 0;
RLRiedinger 0:4837c9695a02 2139 this->drawAxes();
RLRiedinger 0:4837c9695a02 2140
RLRiedinger 0:4837c9695a02 2141 }
RLRiedinger 0:4837c9695a02 2142
RLRiedinger 0:4837c9695a02 2143 this->led1.write(this->mode);
RLRiedinger 0:4837c9695a02 2144 this->led2.write(!this->mode);
RLRiedinger 0:4837c9695a02 2145
RLRiedinger 0:4837c9695a02 2146 }
RLRiedinger 0:4837c9695a02 2147
RLRiedinger 0:4837c9695a02 2148 bool xDir = this->ballSpeedX > 0;
RLRiedinger 0:4837c9695a02 2149 bool yDir = this->ballSpeedY > 0;
RLRiedinger 0:4837c9695a02 2150 bool isUp = !this->up.read();
RLRiedinger 0:4837c9695a02 2151 bool isDown = !this->down.read();
RLRiedinger 0:4837c9695a02 2152
RLRiedinger 0:4837c9695a02 2153 if (isUp && isDown) goto end;
RLRiedinger 0:4837c9695a02 2154 if (!isUp && !isDown) goto end;
RLRiedinger 0:4837c9695a02 2155
RLRiedinger 0:4837c9695a02 2156 if (isUp && this->lastUp) goto end;
RLRiedinger 0:4837c9695a02 2157 if (isDown && this->lastDown) goto end;
RLRiedinger 0:4837c9695a02 2158
RLRiedinger 0:4837c9695a02 2159 if (!xDir) this->ballSpeedX *= -1;
RLRiedinger 0:4837c9695a02 2160 if (!yDir) this->ballSpeedY *= -1;
RLRiedinger 0:4837c9695a02 2161
RLRiedinger 0:4837c9695a02 2162 if (isUp) {
RLRiedinger 0:4837c9695a02 2163
RLRiedinger 0:4837c9695a02 2164 if (++this->ballSpeedX > 5) this->ballSpeedX = 5;
RLRiedinger 0:4837c9695a02 2165 if (++this->ballSpeedY > 5) this->ballSpeedY = 5;
RLRiedinger 0:4837c9695a02 2166
RLRiedinger 0:4837c9695a02 2167 } else if (isDown) {
RLRiedinger 0:4837c9695a02 2168
RLRiedinger 0:4837c9695a02 2169 if (--this->ballSpeedX == 0) this->ballSpeedX = 1;
RLRiedinger 0:4837c9695a02 2170 if (--this->ballSpeedY == 0) this->ballSpeedY = 1;
RLRiedinger 0:4837c9695a02 2171
RLRiedinger 0:4837c9695a02 2172 }
RLRiedinger 0:4837c9695a02 2173
RLRiedinger 0:4837c9695a02 2174 if (!xDir) this->ballSpeedX *= -1;
RLRiedinger 0:4837c9695a02 2175 if (!yDir) this->ballSpeedY *= -1;
RLRiedinger 0:4837c9695a02 2176
RLRiedinger 0:4837c9695a02 2177 end:
RLRiedinger 0:4837c9695a02 2178
RLRiedinger 0:4837c9695a02 2179 this->lastUp = isUp;
RLRiedinger 0:4837c9695a02 2180 this->lastDown = isDown;
RLRiedinger 0:4837c9695a02 2181
RLRiedinger 0:4837c9695a02 2182 }
RLRiedinger 0:4837c9695a02 2183
RLRiedinger 0:4837c9695a02 2184 void ROBOT::drawString(const char* str, int y) {
RLRiedinger 0:4837c9695a02 2185
RLRiedinger 0:4837c9695a02 2186 this->disp.drawString(DisplayN18::WIDTH / 2 - (DisplayN18::CHAR_WIDTH + DisplayN18::CHAR_SPACING) * strlen(str) / 2, y,
RLRiedinger 0:4837c9695a02 2187 str, DisplayN18::WHITE, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 2188
RLRiedinger 0:4837c9695a02 2189 }
RLRiedinger 0:4837c9695a02 2190
RLRiedinger 0:4837c9695a02 2191 void ROBOT::showSplashScreen() {
RLRiedinger 0:4837c9695a02 2192
RLRiedinger 0:4837c9695a02 2193 this->disp.drawLine(0, 0, DisplayN18::WIDTH, DisplayN18::HEIGHT, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 2194 this->disp.drawLine(0, DisplayN18::HEIGHT, DisplayN18::WIDTH, 0, DisplayN18::WHITE);
RLRiedinger 0:4837c9695a02 2195
RLRiedinger 0:4837c9695a02 2196 while (this->circle.read())
RLRiedinger 0:4837c9695a02 2197 wait_ms(1);
RLRiedinger 0:4837c9695a02 2198
RLRiedinger 0:4837c9695a02 2199 this->disp.clear();
RLRiedinger 0:4837c9695a02 2200
RLRiedinger 0:4837c9695a02 2201 }
RLRiedinger 0:4837c9695a02 2202
RLRiedinger 0:4837c9695a02 2203 void ROBOT::clearPaddle() {
RLRiedinger 0:4837c9695a02 2204
RLRiedinger 0:4837c9695a02 2205 this->disp.fillRect(this->paddleX, DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT, ROBOT::PADDLE_WIDTH, ROBOT::PADDLE_HEIGHT, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 2206
RLRiedinger 0:4837c9695a02 2207 }
RLRiedinger 0:4837c9695a02 2208
RLRiedinger 0:4837c9695a02 2209 void ROBOT::drawPaddle() {
RLRiedinger 0:4837c9695a02 2210
RLRiedinger 0:4837c9695a02 2211 this->disp.fillRect(this->paddleX, DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT, ROBOT::PADDLE_WIDTH, ROBOT::PADDLE_HEIGHT, DisplayN18::BLUE);
RLRiedinger 0:4837c9695a02 2212
RLRiedinger 0:4837c9695a02 2213 }
RLRiedinger 0:4837c9695a02 2214
RLRiedinger 0:4837c9695a02 2215 void ROBOT::updatePaddle() {
RLRiedinger 0:4837c9695a02 2216
RLRiedinger 0:4837c9695a02 2217 if (this->left.read())
RLRiedinger 0:4837c9695a02 2218 this->paddleX += ROBOT::PADDLE_SPEED;
RLRiedinger 0:4837c9695a02 2219
RLRiedinger 0:4837c9695a02 2220 if (this->right.read())
RLRiedinger 0:4837c9695a02 2221 this->paddleX -= ROBOT::PADDLE_SPEED;
RLRiedinger 0:4837c9695a02 2222
RLRiedinger 0:4837c9695a02 2223 }
RLRiedinger 0:4837c9695a02 2224
RLRiedinger 0:4837c9695a02 2225 void ROBOT::clearBall() {
RLRiedinger 0:4837c9695a02 2226
RLRiedinger 0:4837c9695a02 2227 this->disp.fillRect(this->ballX - ROBOT::BALL_RADIUS, ballY - ROBOT::BALL_RADIUS,
RLRiedinger 0:4837c9695a02 2228 ROBOT::BALL_RADIUS * 2, ROBOT::BALL_RADIUS * 2, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 2229
RLRiedinger 0:4837c9695a02 2230 }
RLRiedinger 0:4837c9695a02 2231
RLRiedinger 0:4837c9695a02 2232 void ROBOT::drawBall() {
RLRiedinger 0:4837c9695a02 2233
RLRiedinger 0:4837c9695a02 2234 this->disp.fillRect(this->ballX - ROBOT::BALL_RADIUS, ballY - ROBOT::BALL_RADIUS,
RLRiedinger 0:4837c9695a02 2235 ROBOT::BALL_RADIUS * 2, ROBOT::BALL_RADIUS * 2, DisplayN18::RED);
RLRiedinger 0:4837c9695a02 2236
RLRiedinger 0:4837c9695a02 2237 }
RLRiedinger 0:4837c9695a02 2238
RLRiedinger 0:4837c9695a02 2239 void ROBOT::updateBall() {
RLRiedinger 0:4837c9695a02 2240
RLRiedinger 0:4837c9695a02 2241 this->ballX += this->ballSpeedX;
RLRiedinger 0:4837c9695a02 2242 this->ballY += this->ballSpeedY;
RLRiedinger 0:4837c9695a02 2243
RLRiedinger 0:4837c9695a02 2244 }
RLRiedinger 0:4837c9695a02 2245
RLRiedinger 0:4837c9695a02 2246 void ROBOT::checkCollision() {
RLRiedinger 0:4837c9695a02 2247
RLRiedinger 0:4837c9695a02 2248 if (this->paddleX < 0)
RLRiedinger 0:4837c9695a02 2249 this->paddleX = 0;
RLRiedinger 0:4837c9695a02 2250
RLRiedinger 0:4837c9695a02 2251 if (this->paddleX + ROBOT::PADDLE_WIDTH > DisplayN18::WIDTH)
RLRiedinger 0:4837c9695a02 2252 this->paddleX = DisplayN18::WIDTH - ROBOT::PADDLE_WIDTH;
RLRiedinger 0:4837c9695a02 2253
RLRiedinger 0:4837c9695a02 2254 if ((this->ballX - ROBOT::BALL_RADIUS < 0 && this->ballSpeedX < 0) ||
RLRiedinger 0:4837c9695a02 2255 (this->ballX + ROBOT::BALL_RADIUS >= DisplayN18::WIDTH && this->ballSpeedX > 0))
RLRiedinger 0:4837c9695a02 2256 this->ballSpeedX *= -1;
RLRiedinger 0:4837c9695a02 2257
RLRiedinger 0:4837c9695a02 2258 if (this->ballY - ROBOT::BALL_RADIUS < 0 && this->ballSpeedY < 0)
RLRiedinger 0:4837c9695a02 2259 this->ballSpeedY *= -1;
RLRiedinger 0:4837c9695a02 2260
RLRiedinger 0:4837c9695a02 2261 if (this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT && this->ballSpeedY > 0) {
RLRiedinger 0:4837c9695a02 2262
RLRiedinger 0:4837c9695a02 2263 if (this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT) {
RLRiedinger 0:4837c9695a02 2264
RLRiedinger 0:4837c9695a02 2265 this->initializeBall();
RLRiedinger 0:4837c9695a02 2266 this->lives--;
RLRiedinger 0:4837c9695a02 2267
RLRiedinger 0:4837c9695a02 2268 } else if (this->ballX > this->paddleX && this->ballX < this->paddleX + ROBOT::PADDLE_WIDTH) {
RLRiedinger 0:4837c9695a02 2269
RLRiedinger 0:4837c9695a02 2270 this->ballSpeedY *= -1;
RLRiedinger 0:4837c9695a02 2271 this->pwmTicksLeft = ROBOT::BOUNCE_SOUND_TICKS;
RLRiedinger 0:4837c9695a02 2272
RLRiedinger 0:4837c9695a02 2273 }
RLRiedinger 0:4837c9695a02 2274 }
RLRiedinger 0:4837c9695a02 2275
RLRiedinger 0:4837c9695a02 2276 }
RLRiedinger 0:4837c9695a02 2277
RLRiedinger 0:4837c9695a02 2278 void ROBOT::checkPwm() {
RLRiedinger 0:4837c9695a02 2279
RLRiedinger 0:4837c9695a02 2280 if (this->pwmTicksLeft == 0) {
RLRiedinger 0:4837c9695a02 2281
RLRiedinger 0:4837c9695a02 2282 this->pwm.write(0.0);
RLRiedinger 0:4837c9695a02 2283
RLRiedinger 0:4837c9695a02 2284 } else {
RLRiedinger 0:4837c9695a02 2285
RLRiedinger 0:4837c9695a02 2286 this->pwmTicksLeft--;
RLRiedinger 0:4837c9695a02 2287
RLRiedinger 0:4837c9695a02 2288 this->pwm.write(0.5);
RLRiedinger 0:4837c9695a02 2289
RLRiedinger 0:4837c9695a02 2290 }
RLRiedinger 0:4837c9695a02 2291
RLRiedinger 0:4837c9695a02 2292 }
RLRiedinger 0:4837c9695a02 2293
RLRiedinger 0:4837c9695a02 2294 void ROBOT::checkLives() {
RLRiedinger 0:4837c9695a02 2295
RLRiedinger 0:4837c9695a02 2296 if (this->lives == 0) {
RLRiedinger 0:4837c9695a02 2297 this->disp.clear();
RLRiedinger 0:4837c9695a02 2298
RLRiedinger 0:4837c9695a02 2299 this->drawString(ROBOT::LOSE_1, DisplayN18::HEIGHT / 2 - DisplayN18::CHAR_HEIGHT);
RLRiedinger 0:4837c9695a02 2300 this->drawString(ROBOT::LOSE_2, DisplayN18::HEIGHT / 2);
RLRiedinger 0:4837c9695a02 2301
RLRiedinger 0:4837c9695a02 2302 while (this->circle.read())
RLRiedinger 0:4837c9695a02 2303 wait_ms(1);
RLRiedinger 0:4837c9695a02 2304
RLRiedinger 0:4837c9695a02 2305 this->initialize();
RLRiedinger 0:4837c9695a02 2306
RLRiedinger 0:4837c9695a02 2307 } else {
RLRiedinger 0:4837c9695a02 2308
RLRiedinger 0:4837c9695a02 2309 this->disp.drawCharacter(0, 0, static_cast<char>(this->lives + '0'), DisplayN18::WHITE, DisplayN18::BLACK);
RLRiedinger 0:4837c9695a02 2310
RLRiedinger 0:4837c9695a02 2311 }
RLRiedinger 0:4837c9695a02 2312
RLRiedinger 0:4837c9695a02 2313 }