diff --git a/Core/Src/Components/SSL_Display.cpp b/Core/Src/Components/SSL_Display.cpp index f689a85..b0f65cb 100644 --- a/Core/Src/Components/SSL_Display.cpp +++ b/Core/Src/Components/SSL_Display.cpp @@ -27,7 +27,7 @@ int32_t SSL_Display::init() { status0.connected = true; status0.robotId = i; status0.status = 0; - status0.team = 0; + status0.team = i%2; robots[i].setRobotStatus(status0); } diff --git a/Core/Src/Components/SSL_GFX.cpp b/Core/Src/Components/SSL_GFX.cpp index acdb7df..ea95e94 100644 --- a/Core/Src/Components/SSL_GFX.cpp +++ b/Core/Src/Components/SSL_GFX.cpp @@ -43,6 +43,8 @@ int32_t SSL_GFX::draw() { errors += drawRobotSilhouette(colorRobot); errors += drawId(colorRobot); errors += drawBatteryBar(); + errors += drawRobotLid(); + errors += drawRobotTeam(); errors += hDisplay->setPosition(posX, posY, sizeX, sizeY); errors += hDisplay->write((uint8_t*)hBuffer, 19200); return errors; @@ -157,6 +159,169 @@ int32_t SSL_GFX::drawRobotSilhouette(uint8_t color[3]) { int32_t SSL_GFX::drawRobotLid() { uint32_t errors = 0; + uint8_t colorPurple[3] = {0xFC, 0x00, 0xFC}; + uint8_t colorGreen[3] = {0x00, 0xFC, 0x00}; + uint8_t* colorCircle[4]; + switch(robotStatus.robotId){ + case 0: + colorCircle[0] = colorPurple; + colorCircle[1] = colorPurple; + colorCircle[2] = colorGreen; + colorCircle[3] = colorPurple; + break; + case 1: + colorCircle[0] = colorGreen; + colorCircle[1] = colorPurple; + colorCircle[2] = colorGreen; + colorCircle[3] = colorPurple; + break; + case 2: + colorCircle[0] = colorGreen; + colorCircle[1] = colorGreen; + colorCircle[2] = colorGreen; + colorCircle[3] = colorPurple; + break; + case 3: + colorCircle[0] = colorPurple; + colorCircle[1] = colorGreen; + colorCircle[2] = colorGreen; + colorCircle[3] = colorPurple; + break; + case 4: + colorCircle[0] = colorPurple; + colorCircle[1] = colorPurple; + colorCircle[2] = colorPurple; + colorCircle[3] = colorGreen; + break; + case 5: + colorCircle[0] = colorGreen; + colorCircle[1] = colorPurple; + colorCircle[2] = colorPurple; + colorCircle[3] = colorGreen; + break; + case 6: + colorCircle[0] = colorGreen; + colorCircle[1] = colorGreen; + colorCircle[2] = colorPurple; + colorCircle[3] = colorGreen; + break; + case 7: + colorCircle[0] = colorPurple; + colorCircle[1] = colorGreen; + colorCircle[2] = colorPurple; + colorCircle[3] = colorGreen; + break; + case 8: + colorCircle[0] = colorGreen; + colorCircle[1] = colorGreen; + colorCircle[2] = colorGreen; + colorCircle[3] = colorGreen; + break; + case 9: + colorCircle[0] = colorPurple; + colorCircle[1] = colorPurple; + colorCircle[2] = colorPurple; + colorCircle[3] = colorPurple; + break; + case 10: + colorCircle[0] = colorPurple; + colorCircle[1] = colorPurple; + colorCircle[2] = colorGreen; + colorCircle[3] = colorGreen; + break; + case 11: + colorCircle[0] = colorGreen; + colorCircle[1] = colorGreen; + colorCircle[2] = colorPurple; + colorCircle[3] = colorPurple; + break; + } + errors += drawCircle(colorCircle[0], 28, 33, 4); + errors += drawCircle(colorCircle[1], 52, 33, 4); + errors += drawCircle(colorCircle[2], 32, 52, 4); + errors += drawCircle(colorCircle[3], 48, 52, 4); + return errors; +} + +int32_t SSL_GFX::drawRobotTeam(){ + uint8_t colorTobotTeam[3]; + if(robotStatus.team){ + colorTobotTeam[0] = 0b11111100; + colorTobotTeam[1] = 0b11111100; + colorTobotTeam[2] = 0; + }else{ + colorTobotTeam[0] = 0; + colorTobotTeam[1] = 0; + colorTobotTeam[2] = 0b11111100; + } + return drawCircle(colorTobotTeam, 40, 40, 5); +} + +int32_t SSL_GFX::drawCircle(uint8_t color[3], uint16_t posX, uint16_t posY, uint16_t radius) { + uint32_t errors = 0; + uint8_t corners = 3; + uint8_t delta = 0; + int16_t x0 = posX; + int16_t y0 = posY; + int16_t r = radius; + int16_t f = 1 - r; + int16_t ddF_x = 1; + int16_t ddF_y = -2 * r; + int16_t x = 0; + int16_t y = r; + int16_t px = x; + int16_t py = y; + + delta++; // Avoid some +1's in the loop + + for(uint32_t i=0; i < 2*r + 1; i++){ + hBuffer[i + y0 - r][x0][0] = color[0]; + hBuffer[i + y0 - r][x0][1] = color[1]; + hBuffer[i + y0 - r][x0][2] = color[2]; + } + + while (x < y) { + if (f >= 0) { + y--; + ddF_y += 2; + f += ddF_y; + } + x++; + ddF_x += 2; + f += ddF_x; + // These checks avoid double-drawing certain lines, important + // for the SSD1306 library which has an INVERT drawing mode. + if (x < (y + 1)) { + if (corners & 1) + for(uint32_t i=0; i<2*y + delta; i++){ + hBuffer[i + y0 - y][x0 + x][0] = color[0]; + hBuffer[i + y0 - y][x0 + x][1] = color[1]; + hBuffer[i + y0 - y][x0 + x][2] = color[2]; + } + if (corners & 2) + for(uint32_t i=0; i<2*y + delta; i++){ + hBuffer[i + y0 - y][x0 - x][0] = color[0]; + hBuffer[i + y0 - y][x0 - x][1] = color[1]; + hBuffer[i + y0 - y][x0 - x][2] = color[2]; + } + } + if (y != py) { + if (corners & 1) + for(uint32_t i=0; i<2*px + delta; i++){ + hBuffer[i + y0 - px][x0 + py][0] = color[0]; + hBuffer[i + y0 - px][x0 + py][1] = color[1]; + hBuffer[i + y0 - px][x0 + py][2] = color[2]; + } + if (corners & 2) + for(uint32_t i=0; i<2*px + delta; i++){ + hBuffer[i + y0 - px][x0 - py][0] = color[0]; + hBuffer[i + y0 - px][x0 - py][1] = color[1]; + hBuffer[i + y0 - px][x0 - py][2] = color[2]; + } + py = y; + } + px = x; + } return errors; } diff --git a/Core/Src/Components/SSL_GFX.hpp b/Core/Src/Components/SSL_GFX.hpp index 570acc1..b7c551a 100644 --- a/Core/Src/Components/SSL_GFX.hpp +++ b/Core/Src/Components/SSL_GFX.hpp @@ -38,9 +38,11 @@ private: uint8_t (*hBuffer)[sizeX][3]; uint32_t size; int32_t drawText(uint8_t color[3], uint16_t posX, uint16_t posY, uint8_t* string, uint32_t size); + int32_t drawCircle(uint8_t color[3], uint16_t posX, uint16_t posY, uint16_t radius); int32_t drawBoundingBox(uint8_t color[3]); int32_t drawRobotSilhouette(uint8_t color[3]); int32_t drawRobotLid(); + int32_t drawRobotTeam(); int32_t drawId(uint8_t color[3]); int32_t drawBatteryBar(); };