diff --git a/lib/CommandHandler/CommandHandler.cpp b/lib/CommandHandler/CommandHandler.cpp
index ccd64c8..9d49114 100644
--- a/lib/CommandHandler/CommandHandler.cpp
+++ b/lib/CommandHandler/CommandHandler.cpp
@@ -3,7 +3,7 @@
 CommandHandler::CommandHandler(){
     for(uint32_t i = 0; i < MAX_COMMAND_SIZE; i++){
         commandCallbacks[i] = nullptr;
-        commandCallbackIDs[i] = 0;
+        commandCallbackIDs[i] = -1;
     }
 }
 
@@ -30,6 +30,10 @@ bool CommandHandler::RemoveCommand(uint32_t commandID){
 }
 
 CommandHandler::CommandStatus CommandHandler::ProcessCommand(uint32_t * command, uint32_t commandSize){
+    if(commandSize == 0){
+        return CommandStatus::INVALID;
+    }
+    
     uint32_t commandID{command[0]};
     // get a pointer to the second element in the array because the first element is the command ID
     uint32_t * args{&(command[1])};
diff --git a/lib/CommandHandler/CommandHandler.h b/lib/CommandHandler/CommandHandler.h
index 0cdffd3..1486ed7 100644
--- a/lib/CommandHandler/CommandHandler.h
+++ b/lib/CommandHandler/CommandHandler.h
@@ -39,8 +39,6 @@ public:
 
 private:
     static constexpr uint32_t MAX_COMMAND_SIZE{10};
-    // pointer to the head of the command array
-    uint32_t * command{nullptr};
 
     // an array of command callbacks
     CommandCallback commandCallbacks[MAX_COMMAND_SIZE];
diff --git a/src/main.cpp b/src/main.cpp
index 4020b65..35150d1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -104,6 +104,34 @@ void SetStackColor(uint32_t * args, uint32_t argsLength){
   boardManager.SetColumnColors(V3D<uint32_t>{X_COORD, Y_COORD, BOARD_TYPES::PLANE_NORMAL::Z}, colors, numColors);
 }
 
+// command handling functions
+CommandHandler::CommandStatus BoardStateCommandHandler(uint32_t * /*args*/, uint32_t /*argsLength*/){
+  printBoardState();
+  return CommandHandler::CommandStatus::SUCCESS;
+}
+
+CommandHandler::CommandStatus PingCommandHandler(uint32_t * /*args*/, uint32_t /*argsLength*/){
+  GlobalPrint::Println("!" + String(Commands::PING) + ";");
+  return CommandHandler::CommandStatus::SUCCESS;
+}
+
+CommandHandler::CommandStatus SetColorCommandHandler(uint32_t * args, uint32_t argsLength){
+  GlobalPrint::Println("!2;");
+  animator.isEnabled = false;
+  V3D<uint32_t> black{};
+  boardManager.FillColor(black);
+  SetStackColor(reinterpret_cast<uint32_t *>(args), argsLength);
+  return CommandHandler::CommandStatus::SUCCESS;
+}
+
+CommandHandler::CommandStatus GoToIdleCommandHandler(uint32_t * /*args*/, uint32_t /*argsLength*/){
+  GlobalPrint::Println("!3;");
+  animator.isEnabled = true;
+  return CommandHandler::CommandStatus::SUCCESS;
+}
+
+
+
 // --------------------------------------------------
 // ----------------- FREERTOS TASKS -----------------
 // --------------------------------------------------
@@ -113,7 +141,7 @@ void UpdateCommunication(void * params){
     // DO serial processing
     serialMessage.Update();
     if(serialMessage.IsNewData()){
-      // TODO: Is it really a good idea to cast to unsigned here? do we ever use signed values? Find out.
+      // We reinterpret cast the args to a uint32_t pointer because we know that the args will always be positive
       commandHandler.ProcessCommand(reinterpret_cast<uint32_t *>(serialMessage.GetArgs()), serialMessage.GetPopulatedArgs());
       serialMessage.ClearNewData();
     }
@@ -181,36 +209,11 @@ void setup() {
   SetupBluetoothModule();
   Serial.begin(9600);
 
-  // TODO: We should define these as functions and not lambdas because I think these get 
-  // destroyed once the setup function exits and that will break everything
-  // register the commands with the command handler
-  commandHandler.RegisterCommand(Commands::BoardState, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
-    printBoardState();
-    return CommandHandler::CommandStatus::SUCCESS;
-  });
-
-  // create a lambda function to handle the ping command which calls GlobalPrint::Println("!" + String(Commands::PING) + ";");
-  commandHandler.RegisterCommand(Commands::PING, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
-    GlobalPrint::Println("!" + String(Commands::PING) + ";");
-    return CommandHandler::CommandStatus::SUCCESS;
-  });
-
-  commandHandler.RegisterCommand(Commands::SetStackColors, [](uint32_t * args, uint32_t argsLength){
-    GlobalPrint::Println("!2;");
-    animator.isEnabled = false;
-    V3D<uint32_t> black{};
-    boardManager.FillColor(black);
-    SetStackColor(reinterpret_cast<uint32_t *>(args), argsLength);
-    return CommandHandler::CommandStatus::SUCCESS;
-  });
-
-  commandHandler.RegisterCommand(Commands::GoToIdle, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
-    GlobalPrint::Println("!3;");
-    animator.isEnabled = true;
-    return CommandHandler::CommandStatus::SUCCESS;
-  });
-
-
+  // Register all of our commands with the command handler
+  commandHandler.RegisterCommand(Commands::BoardState, BoardStateCommandHandler);
+  commandHandler.RegisterCommand(Commands::PING, PingCommandHandler);
+  commandHandler.RegisterCommand(Commands::SetStackColors, SetColorCommandHandler);
+  commandHandler.RegisterCommand(Commands::GoToIdle, GoToIdleCommandHandler);
 
   Serial.println("Configuring communication methods");
   serialMessage.Init(9600);