Start with CAN_SIMPLE protocol

Firmware/Tupfile.lua

@@ -157,6 +157,7 @@ build{
         'MotorControl/sensorless_estimator.cpp',
         'MotorControl/trapTraj.cpp',
         'MotorControl/main.cpp',
+        'communication/can_simple.cpp',
         'communication/communication.cpp',
         'communication/ascii_protocol.cpp',
         'communication/interface_uart.cpp',

Firmware/communication/can_simple.cpp

@@ -0,0 +1,26 @@
+#include "can_simple.hpp"
+#include "odrive_main.h"
+
+void CANSimple::handle_can_message(CAN_message_t& msg) {
+    // This functional way of handling the messages is neat and is much cleaner from
+    // a data security point of view, but it will require some tweaking to fix the syntax.
+    //
+    // auto func = callback_map.find(msg.id);
+    // if(func != callback_map.end()){
+    //     func->second(msg);
+    // }
+
+    //     Frame
+    // nodeID | CMD
+    // 4 bits | 7 bits
+    auto nodeID = (msg.id >> 7 & 0x15);
+    for(int i = 0; i < AXIS_COUNT; i++){
+        if(axes[i]->config_.nodeID)
+    }
+    switch (msg.id & 0x7F) {
+        case 0x010: move_to_pos_callback(); break;
+    }
+}
+
+void move_to_pos_callback(Axis& axis, uint32_t pos){
+}

Firmware/communication/can_simple.hpp

@@ -0,0 +1,22 @@
+#ifndef __CAN_SIMPLE_HPP_
+#define __CAN_SIMPLE_HPP_
+
+#include "interface_can.hpp"
+
+class CANSimple {
+   public:
+    static void handle_can_message(CAN_message_t& msg);
+
+   private:
+    // Controller
+    static void move_to_pos_callback(Axis& axis, uint32_t pos);
+
+    // This functional way of handling the messages is neat and is much cleaner from
+    // a data security point of view, but it will require some tweaking
+    //
+    // const std::map<uint32_t, std::function<void(CAN_message_t&)>> callback_map = {
+    //     {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
+    // };
+};
+
+#endif

Firmware/communication/interface_can.cpp

@@ -32,7 +32,7 @@
 */
 
 #include "interface_can.hpp"
-#include <unordered_map>
+
 #include "fibre/crc.hpp"
 #include "freertos_vars.h"
 #include "utils.h"
@@ -41,15 +41,20 @@
 #include <cmsis_os.h>
 #include <stm32f4xx_hal.h>
 
+// Specific CAN Protocols
+#include "can_simple.hpp"
+
 #include <odrive_main.h>
 
-std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;
+// Safer context handling via maps instead of arrays
+// #include <unordered_map>
+// std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;
 
 // Constructor is called by communication.cpp and the handle is assigned appropriately
 ODriveCAN::ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config)
     : handle_{handle},
       config_{config} {
-    ctxMap[handle_] = this;
+    // ctxMap[handle_] = this;
 }
 
 void ODriveCAN::can_server_thread() {
@@ -60,10 +65,12 @@ void ODriveCAN::can_server_thread() {
     for (;;) {
         CAN_message_t rxmsg;
 
-        osSemaphoreWait(sem_can, 10);
+        osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status
         while (available()) {
             read(rxmsg);
-            write(rxmsg);
+            switch(config_.protocol) {
+                case CAN_PROTOCOL_SIMPLE: CANSimple::handle_can_message(rxmsg); break;
+            }
         }
 
         // Handle heartbeat message

Firmware/communication/interface_can.hpp

@@ -16,20 +16,25 @@ typedef struct {
 } CAN_message_t;
 
 // Anonymous enum for defining the most common CAN baud rates
-   
-    enum {
-        CAN_BAUD_125K = 125000,
-        CAN_BAUD_250K = 250000,
-        CAN_BAUD_500K = 500000,
-        CAN_BAUD_1000K = 1000000,
-        CAN_BAUD_1M = 1000000
-    };
+
+enum {
+    CAN_BAUD_125K = 125000,
+    CAN_BAUD_250K = 250000,
+    CAN_BAUD_500K = 500000,
+    CAN_BAUD_1000K = 1000000,
+    CAN_BAUD_1M = 1000000
+};
+
+enum CAN_Protocol_t {
+    CAN_PROTOCOL_SIMPLE
+};
 
 class ODriveCAN {
    public:
     struct Config_t {
         uint8_t node_id = 0;
         uint32_t baud = CAN_BAUD_250K;
+        CAN_Protocol_t protocol = CAN_PROTOCOL_SIMPLE;
     };
 
     ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config);
@@ -39,7 +44,7 @@ class ODriveCAN {
     volatile bool thread_id_valid_ = false;
     bool start_can_server();
     void can_server_thread();
-    
+
     // I/O Functions
     uint32_t available();
     uint32_t write(CAN_message_t &txmsg);
@@ -51,6 +56,7 @@ class ODriveCAN {
             make_protocol_object("config",
                                  make_protocol_ro_property("node_id", &config_.node_id),
                                  make_protocol_ro_property("baud_rate", &config_.baud)),
+                                 make_protocol_property("can_protocol", &config_.protocol),
             make_protocol_function("set_node_id", *this, &ODriveCAN::set_node_id, "nodeID"),
             make_protocol_function("set_baud_rate", *this, &ODriveCAN::set_baud_rate, "baudRate"));
     }
@@ -61,7 +67,6 @@ class ODriveCAN {
 
     void set_node_id(uint8_t nodeID);
     void set_baud_rate(uint32_t baudRate);
-    
 };
 
 #endif  // __INTERFACE_CAN_HPP