From 5fcfc681b3f5dff51dd0fef2f2ab414c3e51ac18 Mon Sep 17 00:00:00 2001
From: Gabriel <gabrielmartinslima@gmail.com>
Date: Mon, 12 Feb 2024 17:59:32 -0300
Subject: [PATCH] SSL_Display OK

---
 Core/Src/Components/SSL_Display.cpp | 37 +++++++++++++++++++++++++----
 Core/Src/Components/SSL_Display.hpp | 13 +++++++++-
 Core/Src/Components/SSL_GFX.cpp     |  6 ++---
 Core/Src/Components/start.cpp       |  9 +++++++
 4 files changed, 56 insertions(+), 9 deletions(-)

diff --git a/Core/Src/Components/SSL_Display.cpp b/Core/Src/Components/SSL_Display.cpp
index 72dd999..ac2f76a 100644
--- a/Core/Src/Components/SSL_Display.cpp
+++ b/Core/Src/Components/SSL_Display.cpp
@@ -17,22 +17,49 @@ SSL_Display::~SSL_Display() {
 
 }
 
+// Public methods
+
 int32_t SSL_Display::init() {
 	int32_t errors = 0;
 	errors += hDisplay->init();
 	errors += hDisplay->setBacklight(1);
 	RobotStatus status0;
 	for(uint32_t i = 0; i<12; i++){
-		status0.batteryLevel = ((float)i)/12;
-		status0.connected = i%2;
+		status0.batteryLevel = 0;
+		status0.connected = false;
 		status0.robotId = i;
-		status0.status = i%2;
-		status0.team = i%2;
+		status0.status = 0;
+		status0.team = 0;
 		robots[i].setRobotStatus(status0);
 	}
-
 	for(uint32_t i = 0; i<12; i++){
 		robots[i].draw();
 	}
+	hQueueDisplay = xQueueCreateStatic(queueDisplayLength, sizeof(RobotStatus), queueDisplayStorage, &queueDisplayStructure);
+	hTaskDisplay = xTaskCreateStatic((void(*)(void*))taskDisplayStatic, "taskDisplay", taskDisplayStackSize, (void*)this, 24, taskDisplayStack, &taskDisplayTCB);
 	return errors;
 }
+
+int32_t SSL_Display::update(RobotStatus status){
+	if(xQueueSend(hQueueDisplay, &status, 0)){
+		return 0;
+	}else{
+		// Queue full
+		return -1;
+	}
+}
+
+// Private methods
+
+void SSL_Display::taskDisplayStatic(SSL_Display* obj){
+	obj->taskDisplay();
+}
+
+void SSL_Display::taskDisplay(){
+	RobotStatus status;
+	while(true){
+		xQueueReceive(hQueueDisplay, &status, portMAX_DELAY);
+		robots[status.robotId].setRobotStatus(status);
+		robots[status.robotId].draw();
+	}
+}
diff --git a/Core/Src/Components/SSL_Display.hpp b/Core/Src/Components/SSL_Display.hpp
index 1ee3d1c..f18ae50 100644
--- a/Core/Src/Components/SSL_Display.hpp
+++ b/Core/Src/Components/SSL_Display.hpp
@@ -9,6 +9,7 @@
 #define SRC_COMPONENTS_SSL_DISPLAY_HPP_
 
 #include <FreeRTOS.h>
+#include <queue.h>
 #include "ILI9341.hpp"
 #include "SSL_GFX.hpp"
 
@@ -17,7 +18,12 @@ public:
 	SSL_Display(ILI9341* hDisplay);
 	virtual ~SSL_Display();
 	int32_t init();
+	int32_t update(RobotStatus status);
 private:
+	static void taskDisplayStatic(SSL_Display* obj);
+	void taskDisplay();
+	static constexpr size_t taskDisplayStackSize = 128;
+	static constexpr size_t queueDisplayLength = 12;
 	ILI9341* hDisplay;
 	SSL_GFX robots[12] = {
 			SSL_GFX(hDisplay, 0, 0, buffer, 19200),
@@ -34,7 +40,12 @@ private:
 			SSL_GFX(hDisplay, 160, 240, buffer, 19200),
 	};
 	uint8_t buffer[80][80][3];
-
+	TaskHandle_t hTaskDisplay;
+	StackType_t taskDisplayStack[taskDisplayStackSize];
+	StaticTask_t taskDisplayTCB;
+	QueueHandle_t hQueueDisplay;
+	uint8_t queueDisplayStorage[queueDisplayLength*sizeof(RobotStatus)];
+	StaticQueue_t queueDisplayStructure;
 };
 
 #endif /* SRC_COMPONENTS_SSL_DISPLAY_HPP_ */
diff --git a/Core/Src/Components/SSL_GFX.cpp b/Core/Src/Components/SSL_GFX.cpp
index 5c3c909..0d05e37 100644
--- a/Core/Src/Components/SSL_GFX.cpp
+++ b/Core/Src/Components/SSL_GFX.cpp
@@ -250,8 +250,8 @@ int32_t SSL_GFX::drawId(uint8_t color[3]) {
 	uint32_t errors = 0;
 	errors += drawText(color, 3, 71, (uint8_t*)"ID = ", 5);
 	if(robotStatus.robotId<10){
-		uint8_t robotIdChar = robotStatus.robotId+48;
-		errors += drawText(color, 33, 71, &robotIdChar, 1);
+		uint8_t robotIdChar[2] = {robotStatus.robotId+48, 32};
+		errors += drawText(color, 33, 71, robotIdChar, 2);
 	}else{
 		uint8_t robotIdChar[2] = {49, robotStatus.robotId+(uint8_t)38};
 		errors += drawText(color, 33, 71, robotIdChar, 2);
@@ -263,7 +263,7 @@ int32_t SSL_GFX::drawBatteryBar() {
 	uint32_t errors = 0;
 	for(uint32_t j = 3; j<7; j++){
 		for(uint32_t k = 3; k<77; k++){
-			if(robotStatus.batteryLevel*74 > k){
+			if(robotStatus.batteryLevel*74 > k-3){
 				if(robotStatus.batteryLevel>0.5){
 					hBuffer[j][k][0] = ((uint8_t)(512-robotStatus.batteryLevel*512))&0xFC;
 					hBuffer[j][k][1] = 255&0xFC;
diff --git a/Core/Src/Components/start.cpp b/Core/Src/Components/start.cpp
index a6f9bf4..d4929ec 100644
--- a/Core/Src/Components/start.cpp
+++ b/Core/Src/Components/start.cpp
@@ -34,6 +34,15 @@ SSL_Display sslDisplay(&ili9341);
 void start(){
 	spi_a.init();
 	sslDisplay.init();
+	RobotStatus status;
+	for(uint32_t i=0; i<12; i++){
+		status.batteryLevel = ((float)i)/11;
+		status.connected = i%2;
+		status.robotId = i;
+		status.status = i%2;
+		status.team = i%2;
+		sslDisplay.update(status);
+	}
 	for(uint32_t i=0; i<16; i++){
 		discoveryLeds.set(i);
 		vTaskDelay(1000);