Small code refactor and began using freertos tasks

src/main.cpp

@@ -1,35 +1,34 @@
 // Other peoples libraries
 #include <Arduino.h>
 #include <BluetoothSerial.h>
+#include <FreeRTOS.h>
+
+// Static Defines
+#include "PINOUT.h"
+#include "BOARD-DEFINITIONS.h"
+#include "COMMANDS.h"
 
 // project specific libraries
 #include "BluetoothSerial.h"
 #include "SerialMessage.h"
 #include "BoardLayout.h"
-#include "BOARD-DEFINITIONS.h"
 #include "Color.h"
 #include "ColorManager.h"
-
-// --------------------------------------------------
-// ----------------- Types ----------------------
-// --------------------------------------------------
-enum Commands : uint8_t{
-  BoardState = 0,
-  PING = 1,
-  SetStackColors = 2,
-  GoToIdle = 3
-};
+#include "GlobalPrint.h"
 
 // --------------------------------------------------
 // ----------------- VARIABLES ----------------------
 // --------------------------------------------------
+TaskHandle_t updateCommunicaitonTask;
+TaskHandle_t updateBoardTask;
+
 uint32_t boardStateTimer{0};
 bool boardStateHasChanged{false};
 uint32_t boardStateMaxUpdatePeriod{34}; // this is a little slower than 30fps
 
 // BluetoothSerial SerialBT;
 // BluetoothSerialMessage serialMessageBT(&SerialBT);
-SerialMessage<500> serialMessage(&Serial);
+SerialMessage<500, 10> serialMessage(&Serial);
 BoardLayout board(BOARD_WIDTH, BOARD_LENGTH, BOARD_HEIGHT, stacks);
 
 // Temporary thing until we can get bluetooth color management working on the quest
@@ -38,6 +37,11 @@ ColorManager colorManager(&board);
 // --------------------------------------------------
 // ----------------- FUNCTIONS ----------------------
 // --------------------------------------------------
+/**
+ * @brief Send programming commands to the serial to bluetooth adapter so 
+ * it is set up as expected for the VR headset
+ * @post the serial baud rate will be set to 9600
+ */
 void SetupBluetoothModule(){
   Serial.begin(38400);
   Serial.print("AT+UART=9600,0,0\r\n"); // set baud rate to 9600
@@ -52,37 +56,33 @@ void SetupBluetoothModule(){
   Serial.print("AT+ROLE=0\r\n"); // set to slave
   delay(100);
 
-  
-
   // exit at mode and go into pairing mode
   Serial.print("AT+INIT\r\n");
   Serial.begin(9600);
   delay(100);
 }
+
+
 void printBoardState(){
   // create a buffer to hold the board state
   uint16_t boardState[BOARD_WIDTH * BOARD_LENGTH];
   // read in the board state
   board.GetBoardState(boardState);
 
-  Serial.print("!0,");
-  // SerialBT.print("!0,");
+  GlobalPrint::Print("!0,");
 
   for(int i = 0; i < (BOARD_WIDTH * BOARD_LENGTH); i++){
-    Serial.print(boardState[i]);
-    // SerialBT.print(boardState[i]);
+    GlobalPrint::Print(String(boardState[i]));
     if(i == (BOARD_WIDTH * BOARD_LENGTH) - 1){
       break;
     }
-    Serial.print(",");
-    // SerialBT.print(",");
+    GlobalPrint::Print(",");
   }
 
-  Serial.println(";");
-  // SerialBT.println(";");
+  GlobalPrint::Println(";");
 }
 
-void setStackColor(uint32_t * args, int argsLength){
+void SetStackColor(uint32_t * args, int argsLength){
   uint32_t stackNum = args[1];
   uint32_t numColors = (argsLength - 2) / 3;
   Color colors[numColors];
@@ -95,80 +95,110 @@ void setStackColor(uint32_t * args, int argsLength){
   }
 
   board.SetStackColors(stackNum, colors);
-
-
 }
 
-void parseData(uint32_t * args, int argsLength){
+void parseData(Message<500, 10> &message){
+  int32_t * args{message.GetArgs()};
+  uint32_t argsLength{message.GetArgsLength()};
   uint32_t command = args[0];
   switch(command){
     case Commands::BoardState:
       printBoardState();
       break;
     case Commands::PING:
-      Serial.print("!");
-      // SerialBT.print("!");
-      Serial.print(Commands::PING);
-      // SerialBT.print(Commands::PING);
-      Serial.println(";");
-      // SerialBT.println(";");
+      GlobalPrint::Println("!" + String(Commands::PING) + ";");
       break;
     case Commands::SetStackColors:
-      Serial.println("!2;");
-      // SerialBT.println("!2;");
+      GlobalPrint::Println("!2;");
       colorManager.Enable(false);
-      setStackColor(args, argsLength);
+      SetStackColor(reinterpret_cast<uint32_t *>(args), argsLength);
       break;
     case Commands::GoToIdle:
-      Serial.println("!3;");
-      // SerialBT.println("!3;");
+      GlobalPrint::Println("!3;");
       colorManager.Enable(true);
       break;
     default:
-      Serial.println("INVALID COMMAND");
-      // SerialBT.println("INVALID COMMAND");
+      GlobalPrint::Println("INVALID COMMAND");
       break;
   }
+
+  // now that we have run the command we can clear the data for the next command.
+  serialMessage.ClearNewData();
 }
 
+// --------------------------------------------------
+// ----------------- FREERTOS TASKS -----------------
+// --------------------------------------------------
+void UpdateCommunication(void * params){
+  Serial.println("Spawning UpdateCommunication task");
+  for(;;){
+    // DO serial processing
+    serialMessage.Update();
+    if(serialMessage.IsNewData()){
+      parseData(serialMessage);
+    }
+    // serialMessageBT.Update();
+    // if(serialMessageBT.IsNewData()){
+    //   parseData(serialMessageBT.GetArgs(), serialMessageBT.GetArgsLength());
+    //   serialMessageBT.ClearNewData();
+    // }
+    vTaskDelay(3);
+  }
+  Serial.println("UpdateCommunication task has ended unexpectedly!");
+
+}
+
+void UpdateBoard(void * params){
+  Serial.println("Spawning UpdateBoard task");
+  for(;;){
+    if(board.BoardStateHasChanged()){
+      boardStateHasChanged = true;
+    }
+
+    if(millis() - boardStateTimer > boardStateMaxUpdatePeriod && boardStateHasChanged){
+      boardStateTimer = millis();
+      printBoardState();
+      boardStateHasChanged = false;
+    }
+
+    colorManager.Update();
+
+    vTaskDelay(5);
+  }
+  Serial.println("UpdateBoard task has ended unexpectedly!");
+}
+
+
 // --------------------------------------------------
 // ----------------- SETUP AND LOOP -----------------
 // --------------------------------------------------
 
 void setup() {
+  // delay a little bit to get the serial monitor a chance to capture the next log messages.
   delay(1000);
+
+  Serial.begin(9600);
+  Serial.println("Beginning Setup");
+  Serial.println("Configuring Bluetooth Adapter");
   SetupBluetoothModule();
   Serial.begin(9600);
+
+  Serial.println("Configuring communication methods");
+  serialMessage.Init(9600);
   // SerialBT.begin("blockPartyBT");
+  xTaskCreate(UpdateCommunication, "UpdateCommunication", 10000, NULL, 0, &updateCommunicaitonTask);
+
+  Serial.println("Beginning Board Initializaiton");
+  xTaskCreate(UpdateBoard, "UpdateBoard", 10000, NULL, 0, &updateBoardTask);
   Color colors[] = {Color(255, 0, 0), Color(0, 0, 0), Color(0, 0, 0)};
   board.SetStackColors(2, colors);
-
   boardStateTimer = millis();
-  
+
+  Serial.println("Setup Complete");
 }
 
 
 void loop() {
-  if(board.BoardStateHasChanged()){
-    boardStateHasChanged = true;
-  }
-
-  if(millis() - boardStateTimer > boardStateMaxUpdatePeriod && boardStateHasChanged){
-    boardStateTimer = millis();
-    printBoardState();
-    boardStateHasChanged = false;
-  }
-
-  // DO serial processing
-  serialMessage.Update();
-  if(serialMessage.IsNewData()){
-    parseData(serialMessage.GetArgs(), serialMessage.GetArgsLength());
-    serialMessage.ClearNewData();
-  }
-  // serialMessageBT.Update();
-  // if(serialMessageBT.IsNewData()){
-  //   parseData(serialMessageBT.GetArgs(), serialMessageBT.GetArgsLength());
-  //   serialMessageBT.ClearNewData();
-  // }
-  colorManager.Update();
+  // delete the loop task because we don't use it
+  vTaskDelete(NULL);
 }