Feat can commander

examples/comms/can/can_custom_regs/can_custom_regs.ino

@@ -0,0 +1,73 @@
+#include "Arduino.h"   
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "SimpleCANio.h"
+#include "comms/can/CANCommander.h"
+
+// can pins
+#define CAN_RX NC // define the RX pin
+#define CAN_TX NC // define the TX pin
+#define CAN_SHDN NC // define the Shhutdown pin (inverse of enable) if needed
+#define CAN_ENABLE NC // define the ENABLE pin if needed
+#define CAN_ID 1
+
+// 3pwm pins
+#define PHA PA6
+#define PHB PA7
+#define PHC PA8
+
+BLDCMotor motor = BLDCMotor(7); // 7 pole pairs
+BLDCDriver3PWM driver = BLDCDriver3PWM(PHA, PHB, PHC);
+
+//CANio can(PIN_CAN0_RX, PIN_CAN0_TX); // Create CAN object
+CANio can(CAN_RX, CAN_TX, NC, CAN_ENABLE); // Create CAN object
+
+CANCommander commander(can, CAN_ID);
+void setup()
+{   
+    Serial.begin(115200);
+    SimpleFOCDebug::enable(&Serial);
+
+    commander.init();
+    commander.addMotor(&motor);
+
+    // add custom register to control built-in LED
+    pinMode(LED_BUILTIN, OUTPUT);
+    commander.addCustomRegister(
+        0xF0, // register id (should be > 0xE0) 
+        1, // size in bytes
+        [](RegisterIO& comms, FOCMotor* motor) -> bool {
+            // Read handler for custom register 0xF0
+            uint8_t customValue = digitalRead(LED_BUILTIN);
+            comms << customValue;
+            return true;
+        },
+        [](RegisterIO& comms, FOCMotor* motor) -> bool {
+            // Write handler for custom register 0xF0
+            uint8_t receivedValue;
+            comms >> receivedValue;
+            digitalWrite(LED_BUILTIN, receivedValue ? HIGH : LOW);
+            return true;
+        }
+    );
+
+    delay(5000);
+
+    motor.linkDriver(&driver);
+    driver.voltage_power_supply = 12;
+    driver.init();
+
+    motor.init();
+    motor.initFOC();
+
+    Serial.println("Setup complete!");
+    delay(10);
+}
+
+void loop()
+{
+    motor.loopFOC();
+    motor.move();
+
+    commander.run();
+}

examples/comms/can/can_example/can_example.ino

@@ -0,0 +1,52 @@
+#include "Arduino.h"   
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "SimpleCANio.h"
+#include "comms/can/CANCommander.h"
+
+// can pins
+#define CAN_RX NC // define the RX pin
+#define CAN_TX NC // define the TX pin
+#define CAN_SHDN NC // define the Shhutdown pin (inverse of enable) if needed
+#define CAN_ENABLE NC // define the ENABLE pin if needed
+#define CAN_ID 1
+
+// 3pwm pins
+#define PHA PA6
+#define PHB PA7
+#define PHC PA8
+
+BLDCMotor motor = BLDCMotor(7); // 7 pole pairs
+BLDCDriver3PWM driver = BLDCDriver3PWM(PHA, PHB, PHC);
+
+//CANio can(PIN_CAN0_RX, PIN_CAN0_TX); // Create CAN object
+CANio can(CAN_RX, CAN_TX, NC, CAN_ENABLE); // Create CAN object
+
+CANCommander commander(can, CAN_ID);
+void setup()
+{   
+    Serial.begin(115200);
+    SimpleFOCDebug::enable(&Serial);
+
+    commander.init();
+    commander.addMotor(&motor);
+    delay(5000);
+
+    motor.linkDriver(&driver);
+    driver.voltage_power_supply = 12;
+    driver.init();
+
+    motor.init();
+    motor.initFOC();
+
+    Serial.println("Setup complete!");
+    delay(10);
+}
+
+void loop()
+{
+    motor.loopFOC();
+    motor.move();
+
+    commander.run();
+}

src/comms/README.md

@@ -29,6 +29,9 @@ Implementations are available for either ASCII based protocol (TextIO) or binary
 
 [SimpleFOCRegisters.h](./SimpleFOCRegisters.h) contains a list of registers known to SimpleFOC. These registers can be read and/or written and code is provided to serialize/deserialize them.
 
+**📋 Complete register reference**: See [REGISTERS.md](./REGISTERS.md) for the full table with access types, data types, and sizes.
+
 The SimpleFOC packet based IO (PacketCommander, Telemetry), I2CCommander and SettingsStorage as well as our Python API [PySimpleFOC](https://github.com/simplefoc/pysimplefoc) are all based on this register abstraction, and therefore share the same register names/ids.
 
-If implementing your own communications protocol, we encourage you to base it on the Register abstraction if appropriate. This will provide you with ready to use code to access/write the register values, and will make your solution easier to use due to the shared common conventions.
+If implementing your own communications protocol, we encourage you to base it on the Register abstraction if appropriate. This will provide you with ready to use code to access/write the register values, and will make your solution easier to use due to the shared common conventions.
+

src/comms/REGISTERS.md

@@ -0,0 +1,119 @@
+# SimpleFOC CAN Registers Reference
+
+This document summarizes the registers defined in `SimpleFOCRegisters.h`, their access type (RO/RW/WO), data type, and size in bytes. Sizes reflect payload bytes in a single CAN frame.
+
+Note: Multi-value registers fit in one frame (≤ 8 bytes) only where indicated; larger compound values are still sent as single frames using compact encoding as implemented by `SimpleFOCRegisters`.
+
+## Legend
+- RO: Read-only
+- RW: Read/Write
+- WO: Write-only
+- u8: uint8_t (1 byte)
+- u32: uint32_t (4 bytes)
+- f32: float (4 bytes)
+- 3x f32: three floats (12 bytes, may be sent across multiple frames by higher-level tooling)
+
+## Registers
+
+| ID | Name | Access | Type | Size |
+|----|------|--------|------|------|
+| 0x00 | REG_STATUS | RO | u8×4 (motor, last_reg, last_err, status) | 4 |
+| 0x01 | REG_TARGET | RW | f32 | 4 |
+| 0x03 | REG_ENABLE_ALL | WO | u8 | 1 |
+| 0x04 | REG_ENABLE | RW | u8 | 1 |
+| 0x05 | REG_CONTROL_MODE | RW | u8 | 1 |
+| 0x06 | REG_TORQUE_MODE | RW | u8 | 1 |
+| 0x07 | REG_MODULATION_MODE | RW | u8 | 1 |
+| 0x09 | REG_ANGLE | RO | f32 | 4 |
+| 0x10 | REG_POSITION | RO | u32 + f32 (turns + rad) | 8 |
+| 0x11 | REG_VELOCITY | RO | f32 | 4 |
+| 0x12 | REG_SENSOR_ANGLE | RO | f32 | 4 |
+| 0x13 | REG_SENSOR_MECHANICAL_ANGLE | RO | f32 | 4 |
+| 0x14 | REG_SENSOR_VELOCITY | RO | f32 | 4 |
+| 0x15 | REG_SENSOR_TIMESTAMP | RO | u32 | 4 |
+| 0x16 | REG_PHASE_VOLTAGE | RW | 3x f32 | 12 |
+| 0x17 | REG_PHASE_STATE | RW | u8×3 | 3 |
+| 0x18 | REG_DRIVER_ENABLE | RW | u8 | 1 |
+| 0x1A | REG_TELEMETRY_REG | RW | u8 + n×(u8+u8) | variable |
+| 0x1B | REG_TELEMETRY_CTRL | RW | u8 | 1 |
+| 0x1C | REG_TELEMETRY_DOWNSAMPLE | RW | u32 | 4 |
+| 0x1D | REG_ITERATIONS_SEC | RO | u32 | 4 |
+| 0x20 | REG_VOLTAGE_Q | RO | f32 | 4 |
+| 0x21 | REG_VOLTAGE_D | RO | f32 | 4 |
+| 0x22 | REG_CURRENT_Q | RO | f32 | 4 |
+| 0x23 | REG_CURRENT_D | RO | f32 | 4 |
+| 0x24 | REG_CURRENT_A | RO | f32 | 4 |
+| 0x25 | REG_CURRENT_B | RO | f32 | 4 |
+| 0x26 | REG_CURRENT_C | RO | f32 | 4 |
+| 0x27 | REG_CURRENT_ABC | RO | 3x f32 | 12 |
+| 0x28 | REG_CURRENT_DC | RO | f32 | 4 |
+| 0x30 | REG_VEL_PID_P | RW | f32 | 4 |
+| 0x31 | REG_VEL_PID_I | RW | f32 | 4 |
+| 0x32 | REG_VEL_PID_D | RW | f32 | 4 |
+| 0x33 | REG_VEL_PID_LIM | RW | f32 | 4 |
+| 0x34 | REG_VEL_PID_RAMP | RW | f32 | 4 |
+| 0x35 | REG_VEL_LPF_T | RW | f32 | 4 |
+| 0x36 | REG_ANG_PID_P | RW | f32 | 4 |
+| 0x37 | REG_ANG_PID_I | RW | f32 | 4 |
+| 0x38 | REG_ANG_PID_D | RW | f32 | 4 |
+| 0x39 | REG_ANG_PID_LIM | RW | f32 | 4 |
+| 0x3A | REG_ANG_PID_RAMP | RW | f32 | 4 |
+| 0x3B | REG_ANG_LPF_T | RW | f32 | 4 |
+| 0x40 | REG_CURQ_PID_P | RW | f32 | 4 |
+| 0x41 | REG_CURQ_PID_I | RW | f32 | 4 |
+| 0x42 | REG_CURQ_PID_D | RW | f32 | 4 |
+| 0x43 | REG_CURQ_PID_LIM | RW | f32 | 4 |
+| 0x44 | REG_CURQ_PID_RAMP | RW | f32 | 4 |
+| 0x45 | REG_CURQ_LPF_T | RW | f32 | 4 |
+| 0x46 | REG_CURD_PID_P | RW | f32 | 4 |
+| 0x47 | REG_CURD_PID_I | RW | f32 | 4 |
+| 0x48 | REG_CURD_PID_D | RW | f32 | 4 |
+| 0x49 | REG_CURD_PID_LIM | RW | f32 | 4 |
+| 0x4A | REG_CURD_PID_RAMP | RW | f32 | 4 |
+| 0x4B | REG_CURD_LPF_T | RW | f32 | 4 |
+| 0x50 | REG_VOLTAGE_LIMIT | RW | f32 | 4 |
+| 0x51 | REG_CURRENT_LIMIT | RW | f32 | 4 |
+| 0x52 | REG_VELOCITY_LIMIT | RW | f32 | 4 |
+| 0x53 | REG_DRIVER_VOLTAGE_LIMIT | RW | f32 | 4 |
+| 0x54 | REG_PWM_FREQUENCY | RW | u32 | 4 |
+| 0x55 | REG_DRIVER_VOLTAGE_PSU | RW | f32 | 4 |
+| 0x56 | REG_VOLTAGE_SENSOR_ALIGN | RW | f32 | 4 |
+| 0x5F | REG_MOTION_DOWNSAMPLE | RW | u32 | 4 |
+| 0x60 | REG_ZERO_ELECTRIC_ANGLE | RO | f32 | 4 |
+| 0x61 | REG_SENSOR_DIRECTION | RO | u8 | 1 |
+| 0x62 | REG_ZERO_OFFSET | RW | f32 | 4 |
+| 0x63 | REG_POLE_PAIRS | RO | u32 | 4 |
+| 0x64 | REG_PHASE_RESISTANCE | RW | f32 | 4 |
+| 0x65 | REG_KV | RW | f32 | 4 |
+| 0x66 | REG_INDUCTANCE | RW | f32 | 4 |
+| 0x67 | REG_CURA_GAIN | RW | f32 | 4 |
+| 0x68 | REG_CURB_GAIN | RW | f32 | 4 |
+| 0x69 | REG_CURC_GAIN | RW | f32 | 4 |
+| 0x6A | REG_CURA_OFFSET | RW | f32 | 4 |
+| 0x6B | REG_CURB_OFFSET | RW | f32 | 4 |
+| 0x6C | REG_CURC_OFFSET | RW | f32 | 4 |
+| 0x70 | REG_NUM_MOTORS | RO | u8 | 1 |
+| 0x71 | REG_SYS_TIME | RO | u32 | 4 |
+| 0x7F | REG_MOTOR_ADDRESS | RW | f32 | 4 |
+
+## Custom Registers
+- Range starts at `REG_CUSTOM_START (0xE0)` and supports up to `MAX_CUSTOM_REGISTERS` (32).
+- Register handlers provide size and read/write callbacks:
+
+```cpp
+bool addCustomRegister(uint8_t reg, uint8_t size,
+    RegisterReadHandler readHandler,
+    RegisterWriteHandler writeHandler);
+```
+
+Example:
+```cpp
+bool myRead(RegisterIO& io, FOCMotor* m) { io << (float)m->shaft_velocity; return true; }
+bool myWrite(RegisterIO& io, FOCMotor* m) { float v; io >> v; m->target = v; return true; }
+commander.addCustomRegister(0xE0, 4, myRead, myWrite);
+```
+
+## Notes
+- Floats and integers are transmitted as raw little-endian binary.
+- Multi-component values (e.g., 3x floats) may exceed 8 bytes; higher layers should split or compress if needed.
+- The version constant `SIMPLEFOC_REGISTERS_VERSION` changes only on incompatible schema updates.

src/comms/SimpleFOCRegisters.cpp

@@ -1,6 +1,7 @@
 
 #include "./SimpleFOCRegisters.h"
 #include "BLDCMotor.h"
+#include "communication/SimpleFOCDebug.h"
 #include "./telemetry/Telemetry.h"
 
 
@@ -322,8 +323,14 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
         case SimpleFOCRegister::REG_NUM_MOTORS:
         case SimpleFOCRegister::REG_MOTOR_ADDRESS:
         case SimpleFOCRegister::REG_ENABLE_ALL:
-        default:
             return false;
+        default:
+            // check the custom registers
+            for (uint8_t i=0; i<customRegisterCount; i++) {
+                if (reg == customRegisters[i]->reg_id) {
+                    return customRegisters[i]->readHandler(comms, motor);
+                }
+            }
     }
     return true;
 };
@@ -591,9 +598,17 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
         case SimpleFOCRegister::REG_NUM_MOTORS:
         case SimpleFOCRegister::REG_MOTOR_ADDRESS:
         case SimpleFOCRegister::REG_ENABLE_ALL:
-        default:
             return false;
+        default:
+            // check the custom registers
+            for (uint8_t i=0; i<customRegisterCount; i++) {
+                if (reg == customRegisters[i]->reg_id) {
+                    return customRegisters[i]->writeHandler(comms, motor);
+                }
+            }
     }
+    // if we reach here, the register is not found
+    SIMPLEFOC_DEBUG("Write to unknown register: ", reg);
     return false;
 };
 
@@ -691,10 +706,43 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
                 return 1;
         case SimpleFOCRegister::REG_DRIVER_ENABLE:
         case SimpleFOCRegister::REG_ENABLE_ALL: // write-only
-        default: // unknown register or write only register (no output) or can't handle in superclass
+            return 0;
+        default: 
+            // register not in the predefined regs, check the custom ones
+            for (uint8_t i=0; i<customRegisterCount; i++) {
+                if (customRegisters[i]->reg_id == reg) {
+                    return customRegisters[i]->size;
+                }
+            }
+            // if the code commes here the register is either
+            //  unknown register or write only register (no output) or can't handle in superclass
+            SIMPLEFOC_DEBUG("Size of unknown register requested: ", reg);
             return 0;
     }
 };
 
+bool SimpleFOCRegisters::addCustomRegister(uint8_t reg, uint8_t size, RegisterReadHandler readHandler, RegisterWriteHandler writeHandler) {
+    // check if we have space
+    if (customRegisterCount >= MAX_CUSTOM_REGISTERS) {
+        SIMPLEFOC_DEBUG("Custom register count exceeded maximum (32): ", reg);
+        return false;
+    }
+    // check if the register has valid id
+    if (reg < REG_CUSTOM_START) {
+        SIMPLEFOC_DEBUG("Custom register ID invalid (should be >= 0xE0): ", reg);
+        return false;
+    }
+    // check if the register is already registered
+    for (uint8_t i=0; i<customRegisterCount; i++) {
+        if (customRegisters[i]->reg_id == reg) {
+            SIMPLEFOC_DEBUG("Custom register ID already registered: ", reg);
+            return false;
+        }
+    }
+    // add the custom register
+    customRegisters[customRegisterCount++] = new CustomRegisterHandler{reg, size, readHandler, writeHandler};
+    return true;
+}
+
 
 SimpleFOCRegisters* SimpleFOCRegisters::regs = new SimpleFOCRegisters();

src/comms/SimpleFOCRegisters.h

@@ -10,7 +10,8 @@
 // does not change the version, but removing or changing the meaning of existing registers does, or changing the number of an existing register.
 #define SIMPLEFOC_REGISTERS_VERSION 0x01
 
-
+#define MAX_CUSTOM_REGISTERS 32
+#define REG_CUSTOM_START 0xE0
 
 typedef enum : uint8_t  {
     REG_STATUS = 0x00,          // RO - 1 byte (motor status)
@@ -104,6 +105,18 @@ typedef enum : uint8_t  {
 } SimpleFOCRegister;
 
 
+// Custom register handlers
+typedef bool (*RegisterReadHandler)(RegisterIO& comms, FOCMotor* motor);
+typedef bool (*RegisterWriteHandler)(RegisterIO& comms, FOCMotor* motor);
+// Custom register handler structure
+struct CustomRegisterHandler {
+    uint8_t reg_id;
+    uint8_t size;
+    RegisterReadHandler readHandler;
+    RegisterWriteHandler writeHandler;
+};
+
+
 class SimpleFOCRegisters {
 public:
     SimpleFOCRegisters();
@@ -113,4 +126,10 @@ class SimpleFOCRegisters {
     virtual bool commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor);
 
     static SimpleFOCRegisters* regs;
+
+    // Method to add custom register handlers
+    bool addCustomRegister(uint8_t reg, uint8_t size, RegisterReadHandler readHandler, RegisterWriteHandler writeHandler);
+    protected:
+        CustomRegisterHandler* customRegisters[MAX_CUSTOM_REGISTERS] = {nullptr};
+        uint8_t customRegisterCount = 0;
 };