Randal Riedinger
RETRO ROBOT E
ROBOT.cpp
- Committer:
- RLRiedinger
- Date:
- 2015-03-02
- Revision:
- 0:4837c9695a02
File content as of revision 0:4837c9695a02:
#include "ROBOT.h"
#define AUTO_START
const char* ROBOT::LOSE_1 = "";
const char* ROBOT::LOSE_2 = "";
const char* ROBOT::SPLASH_1 = "";
const char* ROBOT::SPLASH_2 = "";
const char* ROBOT::SSID = "XXXXXXXX";
const char* ROBOT::PASSWORD = "XXXXXXXX";
struct match {
char *RESPONSE;
char *pRESPONSE;
} matchtab[] = {
{ "+IPD," },
{ "\r\n> " },
{ "busy s..." },
{ "OK\r\n" },
{ "+ERROR\r\n" },
};
ROBOT::ROBOT() : left(P0_14, PullUp), right(P0_11, PullUp),
down(P0_12, PullUp), up(P0_13, PullUp),
square(P0_16, PullUp), circle(P0_1, PullUp),
led1(P0_9), led2(P0_8),
pwm(P0_18),
i2c(D_SDA, D_SCL),
serial_bridge1(&i2c, SC16IS750_SA0),
serial_bridge2(&i2c, SC16IS750_SA1),
esp8266_rst(ESP8266_RST) {
pIPD_BUFFER = IPD_BUFFER;
DEBUG = 0;
heartbeatflag = false;
Address_T = "192.168.0.32";
Address = Address_T;
#ifdef AUTO_START
echo_comands = 0;
#else
echo_comands = 0;
#endif
mPROMPT = 0;
mIPD = 0;
mCOUNT = 0;
COUNT = 0;
uCOUNT = 0;
mCOLON = 0;
this->serial_bridge1.baud(14745600UL, 9600);
this->serial_bridge2.baud(14745600UL, 19200);
this->serial_bridge1.format(8, Serial::None, 1);
this->serial_bridge2.format(8, Serial::None, 1);
this->serial_bridge1.set_fifo_control();
this->serial_bridge2.set_fifo_control();
this->serial_bridge1.flush();
this->serial_bridge2.flush();
this->serial_bridge1.set_modem_control();
this->serial_bridge2.set_modem_control();
this->serial_bridge1.set_flow_triggers();
this->serial_bridge2.set_flow_triggers();
this->ESP8266_reset();
srand(0);
this->lastUp = false;
this->lastDown = false;
this->mode = true;
this->colors[0] = DisplayN18::RED;
this->colors[1] = DisplayN18::GREEN;
this->colors[2] = DisplayN18::BLUE;
this->initialize();
}
void ROBOT::serial_print1(char * BUFFER) {
while (*BUFFER != '\0') {
serial_bridge1.putc((int) *BUFFER++);
}
}
void ROBOT::serial_print2(char * BUFFER) {
while (*BUFFER != '\0') {
serial_bridge2.putc((int) *BUFFER++);
}
}
void ROBOT::debug_print(unsigned int line, const char* function) {
if (DEBUG) {
sprintf(UART_BUFFER, "%6d %s\r\n", line, function);
this->serial_print2(UART_BUFFER);
}
}
void ROBOT::debug_print_string(unsigned int line, const char* function, char * pBUFFER) {
if (DEBUG) {
sprintf(UART_BUFFER, "%6d %s [%s]\r\n", line, function, pBUFFER);
this->serial_print2(UART_BUFFER);
}
}
void ROBOT::debug_print2(unsigned int line, const char* function) {
if (DEBUG) {
sprintf(UART_BUFFER, "%6d %s\r\n", line, function);
this->serial_print2(UART_BUFFER);
}
}
void ROBOT::debug_print_string2(unsigned int line, const char* function, char * pBUFFER) {
if (DEBUG) {
sprintf(UART_BUFFER, "%6d %s [%s]\r\n", line, function, pBUFFER);
this->serial_print2(UART_BUFFER);
}
}
void ROBOT::show_menu(void) {
serial_print2("0: Exit\n\r");
serial_print2("1: Show Menu\n\r");
serial_print2("2: Init\n\r");
serial_print2("3: IO Port Out\n\r");
serial_print2("4: Transparant mode\n\r");
serial_print2("5: Free bufferspace\n\r");
#if (0)
serial_print2("6: Enable RTS/CTS\n\r");
serial_print2("7: Disable RTS/CTS\n\r");
serial_print2("8: Write block\n\r");
serial_print2("9: Baudrate 9600\n\r");
serial_print2("A: Baudrate 115200\n\r");
serial_print2("B: Transparant mode with buffer display\n\r");
#endif
#if (0)
serial_print2("C: Test printf\n\r");
#endif
serial_print2("D: Test connected\n\r");
#if(0)
serial_print2("D: \n\r");
serial_print2("P: \n\r");
#endif
serial_print2("T: Test ESP8266 192.168.0.6\n\r");
#if (0)
serial_print2("U: Test ESP8266 192.168.0.14\n\r");
serial_print2("V: Test ESP8266 192.168.0.22\n\r");
#endif
serial_print2("W: Toggle ECHO mode\n\r");
serial_print2("Z: Toggle DEBUG mode\n\r");
serial_print2("\n\r");
while (serial_bridge2.writableCount() < 64);
First = 1;
}
void ROBOT::blink (DigitalOut LED) {
LED = 1;
wait(0.05);
LED = 0;
}
// Heartbeat monitor
void ROBOT::pulse(void) {
led2 = !led2;
}
void ROBOT::heartbeat_start(void) {
heartbeat.attach(this, &ROBOT::pulse, 0.5);
}
void ROBOT::heartbeat_stop(void) {
heartbeat.detach();
}
void ROBOT::ESP8266_reset(void) {
esp8266_rst = 0;
wait(0.1);
esp8266_rst = 1;
wait(1.0);
while (serial_bridge1.readable()) {
serial_bridge1.getc();
wait(0.005);
}
}
void ROBOT::reset_matchtab(void) {
for (int i = 0; i < NMATCHES; i++) {
matchtab[i].pRESPONSE = matchtab[i].RESPONSE;
}
}
int ROBOT::match_RESPONSES(char c, struct match table[], int n) {
// debug_print(__LINE__, __FUNCTION__);
for (int i = 0; i < n; i++) {
if (c == *table[i].pRESPONSE) {
++table[i].pRESPONSE;
if (*table[i].pRESPONSE == '\0') {
table[i].pRESPONSE = table[i].RESPONSE;
return(i + 1);
}
} else {
table[i].pRESPONSE = table[i].RESPONSE;
if (c == *table[i].pRESPONSE) {
++table[i].pRESPONSE;
if (*table[i].pRESPONSE == '\0') {
table[i].pRESPONSE = table[i].RESPONSE;
return(i + 1);
}
}
}
}
return(0);
}
int ROBOT::echo(int show, int timeout) {
debug_print(__LINE__, __FUNCTION__);
Timer ReceiveTimer;
char c = '\0';
// char IIR_STATUS = 0;
int return_code = 0;
debug_print(__LINE__, __FUNCTION__);
ReceiveTimer.start();
debug_print(__LINE__, __FUNCTION__);
while (!return_code) {
// debug_print(__LINE__, __FUNCTION__);
if (ReceiveTimer.read_ms() > timeout) {
debug_print(__LINE__, __FUNCTION__);
return_code = -1;
}
// From Serial to SPI/I2C
#ifndef AUTO_START
while (!return_code && serial_bridge2.readable()) {
debug_print(__LINE__, __FUNCTION__);
c = serial_bridge2.getc();
if (c == '#') {
debug_print(__LINE__, __FUNCTION__);
return_code = QUIT;
} else {
serial_bridge1.putc(c);
}
}
#endif
// From SPI/I2C to serial
int readable1;
// int readable1 = serial_bridge1.readableCount();
// sprintf(FORMAT_BUFFER, "readable1 = %i", readable1);
// debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
while (!return_code && ((readable1 = serial_bridge1.readableCount()) > 0)) {
// sprintf(FORMAT_BUFFER, "readable1 = %i", readable1);
// debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
ReceiveTimer.reset();
c = serial_bridge1.getc();
#ifndef AUTO_START
if (show) {
serial_bridge2.putc(c);
sprintf(UART_BUFFER, "%6d %s [%c]\r\n", __LINE__, __FUNCTION__, c);
serial_print2(UART_BUFFER);
}
#endif
return_code = match_RESPONSES(c, matchtab, NMATCHES);
}
}
sprintf(FORMAT_BUFFER, "return_code = %i", return_code);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
ReceiveTimer.stop();
debug_print(__LINE__, __FUNCTION__);
return(return_code);
}
// Game Math: Faster Sine & Cosine with Polynomial Curves
// http://allenchou.net/2014/02/game-math-faster-sine-cosine-with-polynomial-curves/
// Posted on February 28, 2014 by Allen Chou
double ROBOT::Hill(double x) {
const float a0 = 1.0f;
const float a2 = 2.0f / PI - 12.0f / (PI * PI);
const float a3 = 16.0f / (PI * PI * PI) - 4.0f / (PI * PI);
const float xx = x * x;
const float xxx = xx * x;
return a0 + a2 * xx + a3 * xxx;
}
double ROBOT::FastSin(double x) {
// wrap x within [0, TWO_PI)
const float a = x * TWO_PI_INV;
x -= static_cast < int > (a) * TWO_PI;
if (x < 0.0f) {
x += TWO_PI;
}
// 4 pieces of hills
if (x < HALF_PI) {
return Hill(HALF_PI - x);
} else if (x < PI) {
return Hill(x - HALF_PI);
} else if (x < 3.0f * HALF_PI) {
return -Hill(3.0f * HALF_PI - x);
} else {
return -Hill(x - 3.0f * HALF_PI);
}
}
double ROBOT::FastCos(double x) {
return FastSin(x + HALF_PI);
}
int ROBOT::process_IPD(char c) {
sprintf(FORMAT_BUFFER, "c = [%c]", c);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
int return_code = 0;
if (!mCOUNT && (c >= '0') && (c <= '9')) {
COUNT = COUNT * 10 + (int) c - '0';
uCOUNT = 0;
sprintf(FORMAT_BUFFER, "COUNT = %i", COUNT);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
} else if (!mCOLON && (c == ':')) {
debug_print(__LINE__, __FUNCTION__);
mCOUNT = 1;
mCOLON = 1;
} else if (mCOLON && (uCOUNT < COUNT) && (uCOUNT < 80)) {
debug_print(__LINE__, __FUNCTION__);
*pIPD_BUFFER++ = c;
*pIPD_BUFFER = 0;
uCOUNT++;
sprintf(FORMAT_BUFFER, "uCOUNT = %i", uCOUNT);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
if (uCOUNT == COUNT) {
debug_print(__LINE__, __FUNCTION__);
sprintf(FORMAT_BUFFER, "%6d %s [%s]\r\n", __LINE__, __FUNCTION__, IPD_BUFFER);
serial_print2(FORMAT_BUFFER);
// [21:40:34.9 1234 1234 1234 12345678 12345678]
// [17:14:02.0 336 67 -32 1000677 1000747]
// [0123456789012345678901234567890123456789012]
// [0000000000111111111122222222223333333333444]
// int hours, minutes, seconds, tenths, angle;
// unsigned int hours, minutes, seconds, tenths, angle;
// double CurrentTime, OldTime, ElapsedTime, Angle, Speed;
// uint64_t leftencoder, rightencoder, oldleftencoder, oldrightencoder;
// int mPROMPT, mIPD, mCOUNT, COUNT, uCOUNT, mCOLON, First;
IPD_BUFFER[15] = 0;
IPD_BUFFER[20] = 0;
IPD_BUFFER[25] = 0;
// IPD_BUFFER[34] = 0;
// IPD_BUFFER[43] = 0;
int hours = 10 * (int)(IPD_BUFFER[0] - '0') + (int)(IPD_BUFFER[1] - '0');
int minutes = 10 * (int)(IPD_BUFFER[3] - '0') + (int)(IPD_BUFFER[4] - '0');
int seconds = 10 * (int)(IPD_BUFFER[6] - '0') + (int)(IPD_BUFFER[7] - '0');
int tenths = (int)(IPD_BUFFER[9] - '0');
sprintf(FORMAT_BUFFER, "%6d %s [%02d:%02d:%02d.%1d]\r\n",
__LINE__, __FUNCTION__, hours, minutes, seconds, tenths);
serial_print2(FORMAT_BUFFER);
CurrentTime = (double)(60 * ((60 * hours) + minutes) + seconds + (double)(tenths / 10));
sprintf(FORMAT_BUFFER, "%6d %s CurrentTime = %6.1f\r\n",
__LINE__, __FUNCTION__, CurrentTime);
serial_print2(FORMAT_BUFFER);
Angle = (double)atoi((char *)(IPD_BUFFER + 11));
sprintf(FORMAT_BUFFER, "%6d %s [Angle = %4.0f\r\n",
__LINE__, __FUNCTION__, Angle);
serial_print2(FORMAT_BUFFER);
#if (0)
leftencoder = (uint64_t)atol((char *)(IPD_BUFFER + 26));
rightencoder = (uint64_t)atol((char *)(IPD_BUFFER + 35));
sprintf(FORMAT_BUFFER, "%6d %s [LE = %8lld] [RE = %8lld]\r\n",
__LINE__, __FUNCTION__, leftencoder, rightencoder);
serial_print2(FORMAT_BUFFER);
#endif
if (First) {
First = 0;
} else {
ElapsedTime = CurrentTime - OldTime;
if (ElapsedTime < 0.0) {
ElapsedTime += 86400.0;
}
sprintf(FORMAT_BUFFER, "%6d %s [ElapsedTime = %6.1f]\r\n",
__LINE__, __FUNCTION__, ElapsedTime);
serial_print2(FORMAT_BUFFER);
#if (0)
int64_t leftencoderdelta = leftencoder - oldleftencoder;
int64_t rightencoderdelta = rightencoder - oldrightencoder;
if (leftencoderdelta > 0x3fffffff) {
leftencoderdelta -= 0xffffffff;
} else if (leftencoderdelta < - 0x3fffffff) {
leftencoderdelta += 0xffffffff;
}
sprintf(FORMAT_BUFFER, "%6d %s [leftencoderdelta = %8lld]\r\n",
__LINE__, __FUNCTION__, leftencoderdelta);
serial_print2(FORMAT_BUFFER);
if (rightencoderdelta > 0x3fffffff) {
rightencoderdelta -= 0xffffffff;
} else if (rightencoderdelta < - 0x3fffffff) {
rightencoderdelta += 0xffffffff;
}
sprintf(FORMAT_BUFFER, "%6d %s [rightencoderdelta = %8lld]\r\n",
__LINE__, __FUNCTION__, rightencoderdelta);
serial_print2(FORMAT_BUFFER);
double EncoderDelta = (double) (leftencoderdelta > rightencoderdelta ?
leftencoderdelta : rightencoderdelta);
sprintf(FORMAT_BUFFER, "%6d %s [EncoderDelta = %6.1f]\r\n",
__LINE__, __FUNCTION__, EncoderDelta);
serial_print2(FORMAT_BUFFER);
double Velocity = EncoderDelta / ElapsedTime / 10.0;
sprintf(FORMAT_BUFFER, "%6d %s [Velocity = %6.2f]\r\n",
__LINE__, __FUNCTION__, Velocity);
serial_print2(FORMAT_BUFFER);
R = Velocity * ElapsedTime;
#endif
this->clearBall();
this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::BLACK);
#if (1)
this->circleX = (int)atoi((char *)(IPD_BUFFER + 16)) + DisplayN18::WIDTH / 2;
this->circleY = (int)atoi((char *)(IPD_BUFFER + 21)) + 3 * DisplayN18::HEIGHT / 4;
#else
this->ballX += (int) (R * FastCos(Angle * PI / 180.0) + 0.5);
this->ballY += (int) (R * FastSin(Angle * PI / 180.0) + 0.5);
#endif
if (this->circleX - ROBOT::BALL_RADIUS < 0) {
this->circleX = ROBOT::BALL_RADIUS;
} else if (this->circleX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
this->circleX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
}
if (this->circleY - ROBOT::BALL_RADIUS < 0) {
this->circleY = ROBOT::BALL_RADIUS;
} else if (this->circleY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
this->circleY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
}
if (this->ballX - ROBOT::BALL_RADIUS < 0) {
this->ballX = ROBOT::BALL_RADIUS;
} else if (this->ballX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
this->ballX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
}
if (this->ballY - ROBOT::BALL_RADIUS < 0) {
this->ballY = ROBOT::BALL_RADIUS;
} else if (this->ballY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
this->ballY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
}
this->drawBall();
this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
this->DrawBox();
sprintf(FORMAT_BUFFER, "%6d %s [this->circleX = %4i this->circleY = %4i]\r\n",
__LINE__, __FUNCTION__, this->circleX, this->circleY);
serial_print2(FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "%6d %s [this->ballX = %4i this->ballY = %4i]\r\n",
__LINE__, __FUNCTION__, this->ballX, this->ballY);
serial_print2(FORMAT_BUFFER);
// this->ballX = DisplayN18::WIDTH / 2 - ROBOT::BALL_RADIUS;
// this->ballY = DisplayN18::HEIGHT / 4 - ROBOT::BALL_RADIUS;
}
OldTime = CurrentTime;
oldleftencoder = leftencoder;
oldrightencoder = rightencoder;
mIPD = 0;
}
}
// debug(__LINE__, __FUNCTION__);
return(return_code);
}
void ROBOT::process_buttons(void) {
debug_print(__LINE__, __FUNCTION__);
#if (1)
if (!square) {
BUFFER[5] = 'S';
} else {
BUFFER[5] = 's';
}
sprintf(BUFFER, "%03i %03i %c",
this->ballX - DisplayN18::WIDTH / 2, this->ballY - 3 * DisplayN18::HEIGHT / 4, square ? 's' : 'S');
sprintf(FORMAT_BUFFER, "%6d %s [%s]\r\n", __LINE__, __FUNCTION__, BUFFER);
serial_print2(FORMAT_BUFFER);
#else
if (!left) {
BUFFER[0] = 'L';
} else {
BUFFER[0] = 'l';
}
if (!right) {
BUFFER[1] = 'R';
} else {
BUFFER[1] = 'r';
}
if (!down) {
BUFFER[2] = 'D';
} else {
BUFFER[2] = 'd';
}
if (!up) {
BUFFER[3] = 'U';
} else {
BUFFER[3] = 'u';
}
if (!square) {
BUFFER[5] = 'S';
} else {
BUFFER[5] = 's';
}
if (!circle) {
BUFFER[6] = 'C';
} else {
BUFFER[6] = 'c';
}
BUFFER[7] = 0;
#endif
serial_print1(BUFFER);
//
// Should wait for OK ...
//
mPROMPT = 0;
debug_print(__LINE__, __FUNCTION__);
}
int ROBOT::terminal_emulator(int echo_comands, int timeout) {
debug_print(__LINE__, __FUNCTION__);
int return_code = 0;
char c = '\0';
mPROMPT = 0;
mIPD = 0;
#ifndef AUTO_START
#if (0)
int ODOMETER_index = 0;
#endif
#endif
Timer ReceiveTimer;
Timer ButtonTimer;
int Button_Timeout = Initial_Button_Timeout;
ReceiveTimer.start();
ButtonTimer.start();
debug_print(__LINE__, __FUNCTION__);
while (return_code != ERROR) {
// debug_print(__LINE__, __FUNCTION__);
// From Serial to SPI/I2C
#ifndef AUTO_START
while (!return_code && serial_bridge2.readable()) {
ReceiveTimer.reset();
c = serial_bridge2.getc();
if (c == '#') {
// debug_print(__LINE__, __FUNCTION__);
return_code = QUIT;
} else {
serial_bridge1.putc(c);
}
}
#endif
// From SPI/I2C to serial
while ((return_code != ERROR) && serial_bridge1.readable()) {
if (!mIPD) {
ReceiveTimer.reset();
}
// debug_print(__LINE__, __FUNCTION__);
c = serial_bridge1.getc();
#ifndef AUTO_START
if (echo_comands) {
serial_bridge2.putc(c);
sprintf(UART_BUFFER, "%6d %s [%c]\r\n", __LINE__, __FUNCTION__, c);
serial_print2(UART_BUFFER);
}
#endif
return_code = match_RESPONSES(c, matchtab, NMATCHES);
if (return_code != 0) {
sprintf(FORMAT_BUFFER, "return_code = %i", return_code);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
}
if ((return_code == IPD) && !mIPD) {
debug_print(__LINE__, __FUNCTION__);
return_code = 0;
mIPD = 1;
mCOUNT = 0;
COUNT = 0;
uCOUNT = 0;
mCOLON = 0;
pIPD_BUFFER = IPD_BUFFER;
} else if (mIPD) {
led1 = 1;
debug_print(__LINE__, __FUNCTION__);
process_IPD(c);
ReceiveTimer.reset();
led1 = 0;
} else if (return_code == PROMPT) {
led1 = 1;
debug_print(__LINE__, __FUNCTION__);
mPROMPT = 1;
return_code = 0;
process_buttons();
ButtonTimer.start();
led1 = 0;
} else if ((return_code == OK) || (return_code == BUSY)) {
led1 = 1;
debug_print(__LINE__, __FUNCTION__);
if (mIPD) {
mIPD = 0;
} else if (mPROMPT) {
mPROMPT = 0;
}
led1 = 0;
} else if (return_code == ERROR) {
// debug_print(__LINE__, __FUNCTION__);
}
}
if (ReceiveTimer.read_ms() / 100 % 10 == 0) {
#if (1)
this->clearBall();
this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::BLACK);
// +---+-----+---+
// | 6 | 7 | 8 |
// +---+-----+---+
// | 3 | 4 | 5 |
// +---+-----+---+
// | 0 | 1 | 2 |
// +---+-----+---+
debug_print(__LINE__, __FUNCTION__);
sprintf(FORMAT_BUFFER, "%i4 %i4 %i3", this->ballX, this->ballY, ROBOT::BALL_RADIUS);
debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "%i4 %i4", DisplayN18::WIDTH, DisplayN18::HEIGHT);
debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "%i3 %i3", this->BoxWidth, this->BoxHeight);
debug_print_string2(__LINE__, __FUNCTION__, FORMAT_BUFFER);
if (this->ballX + ROBOT::BALL_RADIUS == DisplayN18::WIDTH / 2 - BoxWidth / 2 - 1) {
debug_print2(__LINE__, __FUNCTION__);
if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
// 0 - No +X (Right) and -Y (Up)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read() && this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read() && this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
(this->ballY - ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
// 3 - No +X (Right)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
}
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
// 6 - No +X (Right) and +Y (Down)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read() && this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read() && this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else {
debug_print2(__LINE__, __FUNCTION__);
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
}
} else if ((this->ballX - ROBOT::BALL_RADIUS >= DisplayN18::WIDTH / 2 - BoxWidth / 2 - 1) &&
(this->ballX - ROBOT::BALL_RADIUS <= DisplayN18::WIDTH / 2 + BoxWidth / 2 + 1)) {
debug_print2(__LINE__, __FUNCTION__);
if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
// 1 - No -Y (Up)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
(this->ballY + ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
// 4 - Oops inside box allow him to leave
debug_print2(__LINE__, __FUNCTION__);
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
} else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
// 7 - No +Y (Down)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
}
} else {
debug_print2(__LINE__, __FUNCTION__);
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
}
} else if (this->ballX - ROBOT::BALL_RADIUS == DisplayN18::WIDTH / 2 + BoxWidth / 2 + 1) {
debug_print2(__LINE__, __FUNCTION__);
if (this->ballY - ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 + BoxHeight / 2 + 1) {
// 2 - No -X (Left) and +Y (Down)
debug_print2(__LINE__, __FUNCTION__);
if (!this->left.read() && this->down.read()) {
debug_print(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read() && this->left.read()) {
debug_print(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read() && this->left.read()) {
debug_print(__LINE__, __FUNCTION__);
this->ballY++;
}
} else if ((this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT / 2 - BoxHeight / 2) &&
(this->ballY - ROBOT::BALL_RADIUS <= DisplayN18::HEIGHT / 2 + BoxHeight / 2)) {
// 5 - No -X (Left)
if (!this->right.read()) {
debug_print(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else if (this->ballY + ROBOT::BALL_RADIUS == DisplayN18::HEIGHT / 2 - BoxHeight / 2 - 1) {
// 8 - No -X (Left) and +Y (Down)
if (!this->left.read() && this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read() && this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
} else {
debug_print2(__LINE__, __FUNCTION__);
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
}
} else {
debug_print2(__LINE__, __FUNCTION__);
if (!this->up.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY--;
} else if (!this->down.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballY++;
}
if (!this->left.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX--;
} else if (!this->right.read()) {
debug_print2(__LINE__, __FUNCTION__);
this->ballX++;
}
}
if (this->ballX - ROBOT::BALL_RADIUS < 0) {
this->ballX = ROBOT::BALL_RADIUS;
} else if (this->ballX + ROBOT::BALL_RADIUS > DisplayN18::WIDTH) {
this->ballX = DisplayN18::WIDTH - ROBOT::BALL_RADIUS;
}
if (this->ballY - ROBOT::BALL_RADIUS < 0) {
this->ballY = ROBOT::BALL_RADIUS;
} else if (this->ballY + ROBOT::BALL_RADIUS > DisplayN18::HEIGHT) {
this->ballY = DisplayN18::HEIGHT - ROBOT::BALL_RADIUS;
}
this->drawBall();
this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
#endif
sprintf(FORMAT_BUFFER, "ReceiveTimer.read_ms() = %i", ReceiveTimer.read_ms());
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "ButtonTimer.read_ms() = %i", ButtonTimer.read_ms());
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "mIPD = %i", mIPD);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
sprintf(FORMAT_BUFFER, "mPROMPT = %i", mPROMPT);
debug_print_string(__LINE__, __FUNCTION__, FORMAT_BUFFER);
}
if (ReceiveTimer.read_ms() > timeout) {
debug_print(__LINE__, __FUNCTION__);
return_code = ERROR;
} else if (!mIPD && !mPROMPT && (ButtonTimer.read_ms() > Button_Timeout)) {
debug_print(__LINE__, __FUNCTION__);
led1 = 0;
serial_print1("AT+CIPSEND=9\r");
led1 = 1;
ButtonTimer.reset();
ButtonTimer.stop();
ReceiveTimer.reset();
}
}
ReceiveTimer.stop();
ButtonTimer.stop();
debug_print(__LINE__, __FUNCTION__);
return(return_code);
}
void ROBOT::DrawBox(void) {
this->disp.drawLine(DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2,
DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2, DisplayN18::GREEN);
this->disp.drawLine(DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2,
DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2, DisplayN18::GREEN);
this->disp.drawLine(DisplayN18::WIDTH / 2 + BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2,
DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2, DisplayN18::GREEN);
this->disp.drawLine(DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 + BoxHeight / 2,
DisplayN18::WIDTH / 2 - BoxWidth / 2, DisplayN18::HEIGHT / 2 - BoxHeight / 2, DisplayN18::GREEN);
}
int ROBOT::test(void) {
debug_print(__LINE__, __FUNCTION__);
First = 1;
this->circleX = DisplayN18::WIDTH / 2;
this->circleY = 3 * DisplayN18::HEIGHT / 4;
this->ballX = DisplayN18::WIDTH / 2;
this->ballY = 3 * DisplayN18::HEIGHT / 4;
this->disp.clear();
this->drawBall();
this->disp.drawCircle(this->circleX, this->circleY, 2 * ROBOT::BALL_RADIUS, DisplayN18::WHITE);
this->DrawBox();
int return_code;
reset_matchtab();
debug_print(__LINE__, __FUNCTION__);
blink(led1);
serial_print1("AT\r");
debug_print(__LINE__, __FUNCTION__);
if ((return_code = echo(echo_comands, 10000)) == OK) {
blink(led1);
debug_print(__LINE__, __FUNCTION__);
serial_print1("AT+CWMODE=1\r");
if ((return_code = echo(echo_comands, 10000)) == OK) {
blink(led1);
debug_print(__LINE__, __FUNCTION__);
sprintf(FORMAT_BUFFER, "AT+CWJAP=\"%s\",\"%s\"\r", SSID, PASSWORD);
serial_print1(FORMAT_BUFFER);
if ((return_code = echo(echo_comands, 10000)) == OK) {
blink(led1);
debug_print(__LINE__, __FUNCTION__);
serial_print1("AT+CIPMUX=0\r");
if ((return_code = echo(echo_comands, 10000)) == OK) {
blink(led1);
debug_print(__LINE__, __FUNCTION__);
serial_bridge1.printf("AT+CIPSTART=\"TCP\",\"%s\",5001\r", Address);
if ((return_code = echo(echo_comands, 30000)) == OK) {
debug_print(__LINE__, __FUNCTION__);
for (int i = 0; i < 20; i++) {
blink(led1);
serial_bridge1.flush();
wait(0.05);
}
return_code = terminal_emulator(echo_comands, 10000);
}
}
}
}
}
debug_print(__LINE__, __FUNCTION__);
#ifndef AUTO_START
if (return_code != QUIT) {
serial_print2("ERROR\r\n");
}
#endif
led1 = 1;
wait(1.0);
return return_code;
}
void ROBOT::test2(void) {
while (1) {
debug_print(__LINE__, __FUNCTION__);
}
}
int ROBOT::get_options(void) {
#ifndef AUTO_START
int return_code;
bool running = true;
bool running_test = true;
char command, ch;
while (!serial_bridge2.readable());
command = serial_bridge1.getc();
#if defined(TARGET_LPC11U24)
serial_print2("\r\nHello World from LPC11U24\r\n");
#endif
#if defined(TARGET_LPC1768)
serial_print2("\r\nHello World from LPC1768\r\n");
#endif
#if defined(TARGET_KL25Z)
serial_print2("\r\nHello World from KL25Z\r\n");
#endif
#if defined(TARGET_LPC812)
serial_print2("\r\nHello World from LPC812\r\n");
#endif
int i = 0;
#endif
heartbeat_start();
// myled1 = 1; // LED Off
// We need to enable flow control or we overflow buffers and
// lose data when used with the WiFly. Note that flow control
// needs to be enabled on the WiFly for this to work but it's
// possible to do that with flow control enabled here but not
// there.
// serial_bridge1.set_flow_control(SC16IS750::RTSCTS);
serial_bridge1.ioSetDirection(0xFF); // All outputs
serial_bridge1.ioSetState(0xFF); // All On
#ifdef AUTO_START
Address = Address_T;
while (1) {
test();
}
#else
show_menu();
while (serial_bridge2.writableCount() < 64);
serial_bridge2.flush();
serial_print2(">");
while (running) {
if (serial_bridge2.readable()) {
command = serial_bridge2.getc();
serial_bridge2.printf("command = %c\n\r", command);
switch (command) {
case '0' :
serial_print2("Done\n\r");
running = false;
break;
case '1' :
show_menu();
break;
case '2' :
serial_print2("Hardware Reset\n\r");
serial_bridge1.hwReset(); // test
serial_print2("Init\n\r");
serial_bridge1._init();
ESP8266_reset();
serial_print2("Hardware Init done\n\r");
break;
case '3' :
serial_print2("IO Port Out\n\r");
i = 0;
while (!serial_bridge2.readable()) {
serial_bridge1.ioSetState(i);
// serial_bridge1.ioGetState() ; // test
// wait(0.5);
// pc.putc('*');
i = (i + 1) & 0xFF;
}
serial_bridge2.getc();
serial_print2("IO Port Out Done\n\r");
break;
case '4' :
serial_print2("Transparant Mode, Enter '#' to quit...\n\r");
running_test = true;
while (running_test) {
// From SPI/I2C to serial
while (serial_bridge2.readable()) {
ch = serial_bridge2.getc();
running_test = (ch != '#');
serial_bridge1.putc(ch);
}
// From Serial to SPI/I2C
while (running_test && serial_bridge1.readable()) {
ch = serial_bridge1.getc();
running_test = (ch != '#');
serial_bridge2.putc(ch);
}
}
serial_print2("\n\rTransparant Mode done\n\r");
break;
case '5' :
serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount());
serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
break;
#if (0)
case '6' :
serial_print2("Enable RTS/CTS\n\r");
serial_bridge1.set_flow_control(SC16IS750::RTSCTS);
break;
case '7' :
serial_print2("Disable RTS/CTS\n\r");
serial_bridge1.set_flow_control(SC16IS750::Disabled);
break;
case '8' :
serial_print2("Write block\n\r");
serial_bridge1.writeString("Hello World from mbed and SC16IS750 ");
break;
case '9' :
serial_print2("Baudrate = 9600\n\r");
serial_bridge2.baud(14745600UL, 9600);
break;
case 'A' :
serial_print2("Baudrate = 115200\n\r");
serial_bridge2.baud(14745600UL, 115200);
break;
case 'B' :
serial_print2("Transparent Mode with buffer display, Enter '#' to quit...\n\r");
running_test=true;
while (running_test) {
// From SPI/I2C to serial
while (running_test && serial_bridge2.readable()) {
ch = serial_bridge2.getc();
running_test = (ch != '#');
serial_bridge1.putc(ch);
// Show buffers when character was entered
serial_print2("\n\r");
serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount());
serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
}
// From Serial to SPI/I2C
while (running_test && serial_bridge1.readable()) {
ch = serial_bridge1.getc();
running_test = (ch != '#');
serial_bridge2.putc(ch);
}
}
serial_print2("\n\rTransparant Mode done\n\r");
break;
#endif
#if (0)
case 'C' :
serial_print2("Test printf()\n\r");
serial_bridge2.printf("Available for Reading = %3d (Free Space = %3d)\n\r",
serial_bridge1.readableCount(), SC16IS750_FIFO_RX - serial_bridge1.readableCount() );
serial_bridge2.printf("Available for Writing = %3d (Used Space = %3d)\n\r",
serial_bridge1.writableCount(), SC16IS750_FIFO_TX - serial_bridge1.writableCount());
serial_print2("\n\rTest printf() done\n\r");
break;
#endif
case 'D' :
serial_print2("Test connected\n\r");
if (!serial_bridge1.connected()){
serial_print2("Failed to detect UART bridge\r\n");
} else {
serial_print2("Found UART bridge!\r\n");
}
break;
case 'T' :
serial_print2("Test ESP8266 192.168.0.6\n\r");
Address = Address_T;
return_code = test();
serial_bridge2.printf("return_code = %6i\r\n", return_code);
break;
#if (0)
case 'U' :
serial_print2("Test ESP8266 192.168.0.14\n\r");
Address = Address_U;
return_code = test();
serial_bridge2.printf("return_code = %6i\r\n", return_code);
break;
case 'V' :
serial_print2("Test ESP8266 192.168.0.22\n\r");
Address = Address_V;
return_code = test();
serial_bridge2.printf("return_code = %6i\r\n", return_code);
break;
case 'W' :
serial_print2("Test ESP8266 192.168.0.23\n\r");
Address = Address_W;
return_code = test();
serial_bridge2.printf("return_code = %6i\r\n", return_code);
break;
#endif
case 'W' :
serial_print2("Toggle ECHO mode\n\r");
echo_comands = ~echo_comands;
serial_bridge2.printf("ECHO is %s\n\r", echo_comands ? "on" : "off");
break;
case 'Z' :
serial_print2("Toggle DEBUG mode\n\r");
DEBUG = ~DEBUG;
serial_bridge2.printf("DEBUG is %s\n\r", DEBUG ? "on" : "off");
while (0) {
serial_bridge2.printf("%6d %s\n\r", __LINE__, __FUNCTION__);
debug_print(__LINE__, __FUNCTION__);
}
break;
default :
break;
} // switch
serial_print2(">");
} // if
} // while
#endif
return(0);
}
double ROBOT::convert(char* buffer) {
double val = ((buffer[0] << 2) | (buffer[1] >> 6));
if (val > 511.0)
val -= 1024.0;
return val / 512.0;
}
void ROBOT::printDouble(double value, int x, int y) {
char buffer[10];
int len = sprintf(buffer, "%.1f", value);
this->disp.drawString(x, y, buffer, DisplayN18::WHITE, DisplayN18::BLACK);
}
void ROBOT::drawAxes() {
for (int i = 0; i < 3; i++) {
this->disp.drawLine(0, i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING), 0,
i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT, DisplayN18::WHITE);
this->disp.drawLine(0, i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT / 2, DisplayN18::WIDTH,
i * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + ROBOT::GRAPH_HEIGHT / 2, DisplayN18::WHITE);
}
}
void ROBOT::drawPoint(int axis, double value) {
if (value < -1.0)
value = -1.0;
if (value > 1.0)
value = 1.0;
value += 1.0;
value /= 2.0;
value = 1.0 - value;
value *= ROBOT::GRAPH_HEIGHT;
this->disp.setPixel(this->graphX, axis * (ROBOT::GRAPH_HEIGHT + ROBOT::GRAPH_SPACING) + (int)value, this->colors[axis]);
}
void ROBOT::checkGraphReset() {
if (this->graphX > DisplayN18::WIDTH) {
this->graphX = 0;
this->disp.clear();
this->drawAxes();
}
}
void ROBOT::initialize() {
this->initializeBall();
this->paddleX = DisplayN18::WIDTH / 2 - ROBOT::PADDLE_WIDTH / 2;
this->pwmTicksLeft = 0;
this->lives = 4;
this->pwm.period_ms(1);
this->pwm.write(0.00);
this->disp.clear();
}
void ROBOT::initializeBall() {
this->ballX = DisplayN18::WIDTH / 2 - ROBOT::BALL_RADIUS;
this->ballY = DisplayN18::HEIGHT / 4 - ROBOT::BALL_RADIUS;
this->ballSpeedX = rand() % 2 ? 1 : -1;
this->ballSpeedY = rand() % 2 ? 1 : -1;
}
void ROBOT::tick() {
this->checkButtons();
this->clearPaddle();
this->clearBall();
this->updatePaddle();
this->updateBall();
this->checkCollision();
this->drawPaddle();
this->drawBall();
this->checkPwm();
this->checkLives();
wait_ms(25);
}
void ROBOT::checkButtons() {
if (!this->square.read()) {
this->mode = !this->mode;
this->disp.clear();
if (!this->mode) {
this->graphX = 0;
this->drawAxes();
}
this->led1.write(this->mode);
this->led2.write(!this->mode);
}
bool xDir = this->ballSpeedX > 0;
bool yDir = this->ballSpeedY > 0;
bool isUp = !this->up.read();
bool isDown = !this->down.read();
if (isUp && isDown) goto end;
if (!isUp && !isDown) goto end;
if (isUp && this->lastUp) goto end;
if (isDown && this->lastDown) goto end;
if (!xDir) this->ballSpeedX *= -1;
if (!yDir) this->ballSpeedY *= -1;
if (isUp) {
if (++this->ballSpeedX > 5) this->ballSpeedX = 5;
if (++this->ballSpeedY > 5) this->ballSpeedY = 5;
} else if (isDown) {
if (--this->ballSpeedX == 0) this->ballSpeedX = 1;
if (--this->ballSpeedY == 0) this->ballSpeedY = 1;
}
if (!xDir) this->ballSpeedX *= -1;
if (!yDir) this->ballSpeedY *= -1;
end:
this->lastUp = isUp;
this->lastDown = isDown;
}
void ROBOT::drawString(const char* str, int y) {
this->disp.drawString(DisplayN18::WIDTH / 2 - (DisplayN18::CHAR_WIDTH + DisplayN18::CHAR_SPACING) * strlen(str) / 2, y,
str, DisplayN18::WHITE, DisplayN18::BLACK);
}
void ROBOT::showSplashScreen() {
this->disp.drawLine(0, 0, DisplayN18::WIDTH, DisplayN18::HEIGHT, DisplayN18::WHITE);
this->disp.drawLine(0, DisplayN18::HEIGHT, DisplayN18::WIDTH, 0, DisplayN18::WHITE);
while (this->circle.read())
wait_ms(1);
this->disp.clear();
}
void ROBOT::clearPaddle() {
this->disp.fillRect(this->paddleX, DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT, ROBOT::PADDLE_WIDTH, ROBOT::PADDLE_HEIGHT, DisplayN18::BLACK);
}
void ROBOT::drawPaddle() {
this->disp.fillRect(this->paddleX, DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT, ROBOT::PADDLE_WIDTH, ROBOT::PADDLE_HEIGHT, DisplayN18::BLUE);
}
void ROBOT::updatePaddle() {
if (this->left.read())
this->paddleX += ROBOT::PADDLE_SPEED;
if (this->right.read())
this->paddleX -= ROBOT::PADDLE_SPEED;
}
void ROBOT::clearBall() {
this->disp.fillRect(this->ballX - ROBOT::BALL_RADIUS, ballY - ROBOT::BALL_RADIUS,
ROBOT::BALL_RADIUS * 2, ROBOT::BALL_RADIUS * 2, DisplayN18::BLACK);
}
void ROBOT::drawBall() {
this->disp.fillRect(this->ballX - ROBOT::BALL_RADIUS, ballY - ROBOT::BALL_RADIUS,
ROBOT::BALL_RADIUS * 2, ROBOT::BALL_RADIUS * 2, DisplayN18::RED);
}
void ROBOT::updateBall() {
this->ballX += this->ballSpeedX;
this->ballY += this->ballSpeedY;
}
void ROBOT::checkCollision() {
if (this->paddleX < 0)
this->paddleX = 0;
if (this->paddleX + ROBOT::PADDLE_WIDTH > DisplayN18::WIDTH)
this->paddleX = DisplayN18::WIDTH - ROBOT::PADDLE_WIDTH;
if ((this->ballX - ROBOT::BALL_RADIUS < 0 && this->ballSpeedX < 0) ||
(this->ballX + ROBOT::BALL_RADIUS >= DisplayN18::WIDTH && this->ballSpeedX > 0))
this->ballSpeedX *= -1;
if (this->ballY - ROBOT::BALL_RADIUS < 0 && this->ballSpeedY < 0)
this->ballSpeedY *= -1;
if (this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT - ROBOT::PADDLE_HEIGHT && this->ballSpeedY > 0) {
if (this->ballY + ROBOT::BALL_RADIUS >= DisplayN18::HEIGHT) {
this->initializeBall();
this->lives--;
} else if (this->ballX > this->paddleX && this->ballX < this->paddleX + ROBOT::PADDLE_WIDTH) {
this->ballSpeedY *= -1;
this->pwmTicksLeft = ROBOT::BOUNCE_SOUND_TICKS;
}
}
}
void ROBOT::checkPwm() {
if (this->pwmTicksLeft == 0) {
this->pwm.write(0.0);
} else {
this->pwmTicksLeft--;
this->pwm.write(0.5);
}
}
void ROBOT::checkLives() {
if (this->lives == 0) {
this->disp.clear();
this->drawString(ROBOT::LOSE_1, DisplayN18::HEIGHT / 2 - DisplayN18::CHAR_HEIGHT);
this->drawString(ROBOT::LOSE_2, DisplayN18::HEIGHT / 2);
while (this->circle.read())
wait_ms(1);
this->initialize();
} else {
this->disp.drawCharacter(0, 0, static_cast<char>(this->lives + '0'), DisplayN18::WHITE, DisplayN18::BLACK);
}
}