Divide code

controller/fw/embed/include/config.h

@@ -0,0 +1,31 @@
+#pragma once
+#include "Arduino.h"
+#include <AS5045.h>
+#include <DRV8313.h>
+#include <SimpleFOC.h>
+#include <STM32_CAN.h>
+
+
+extern STM32_CAN Can;
+extern SPIClass spi;
+extern MagneticSensorAS5045 encoder;
+extern BLDCMotor motor;
+extern DRV8313Driver driver;
+extern LowsideCurrentSense current_sense;
+extern Commander command;
+
+struct MotorControlInputs {
+  float target_angle = 0.0;
+  float target_velocity = 0.0;
+  bool motor_enabled = false;
+  bool foc_state = false;
+};
+
+extern MotorControlInputs motor_control_inputs;
+
+void doMotor(char *cmd);
+void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
+               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
+               Commander *commander, CommandCallback callback);
+
+void foc_step(BLDCMotor *motor, Commander *commander);

controller/fw/embed/include/flash.h

@@ -66,6 +66,7 @@ static uint32_t write_ptr = SECTOR_6;
 //for bootloader    
 typedef void(*pFunction)(void); 
 
+
 // Function prototypes
 void flash_unlock(void);
 void flash_lock(void);

controller/fw/embed/include/process_can.h

@@ -0,0 +1,25 @@
+#pragma once 
+#include "config.h"
+#include "STM32_CAN.h"
+#include "flash.h"
+#include "reg_cah.h"
+
+extern FLASH_RECORD *flash_rec;
+extern volatile uint16_t msg_id;
+extern volatile uint16_t id_x;
+extern volatile uint8_t msg_ch;
+extern volatile uint8_t crc_h;
+extern volatile uint8_t crc_l;
+
+
+void send_velocity();
+void send_angle();
+void send_motor_enabled();
+void send_motor_enabled();
+void send_id();
+// void send_motor_torque();
+void send_pid(uint8_t param_pid);
+void setup_id(uint8_t my_id);
+void setup_angle(float target_angle);
+void setup_pid_angle(uint8_t param_pid, float data);
+void listen_can(const CAN_message_t &msg);

controller/fw/embed/src/config.cpp

@@ -0,0 +1,72 @@
+#include "config.h"
+
+void doMotor(char *cmd) {
+  command.motor(&motor, cmd);
+  digitalWrite(PC10, !digitalRead(PC10));
+  delayMicroseconds(2);
+}
+
+
+
+void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
+               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
+               Commander *commander, CommandCallback callback) {
+  encoder->init(&spi);
+
+  // Driver configuration
+  driver->pwm_frequency = 20000;
+  driver->voltage_power_supply = 24;
+  driver->voltage_limit = 24;
+  driver->init();
+
+  // Current sense initialization
+  current_sense->linkDriver(driver);
+  current_sense->init();
+
+  // Motor configuration
+  motor->linkSensor(encoder);
+  motor->linkDriver(driver);
+  motor->linkCurrentSense(current_sense);
+  motor->useMonitoring(Serial);
+  motor->monitor_downsample = 5000;  // Default monitoring interval
+  motor->controller = MotionControlType::angle;
+  motor->torque_controller = TorqueControlType::voltage;
+  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  // PID Configuration
+  motor->PID_velocity.P = 0.75;
+  motor->PID_velocity.I = 20;
+  motor->LPF_velocity.Tf = 0.005;
+  motor->P_angle.P = 0.5;
+  motor->LPF_angle.Tf = 0.001;
+
+  // Motor limits
+  motor->velocity_limit = 40;  // Speed limit in rad/s (382 rpm)
+  motor->voltage_limit = 24;
+  motor->current_limit = 0.5;
+
+  motor->sensor_direction = Direction::CCW;
+  motor->init();
+  motor->initFOC();
+}
+
+
+void foc_step(BLDCMotor *motor, Commander *commander) {
+  if (motor_control_inputs.target_velocity != 0 ||
+      motor->controller == MotionControlType::velocity) {
+    if (motor->controller != MotionControlType::velocity) {
+      motor->controller = MotionControlType::velocity;
+    }
+    motor->target = motor_control_inputs.target_velocity;
+  } else {
+    if (motor->controller != MotionControlType::angle) {
+      motor->controller = MotionControlType::angle;
+    }
+    motor->target = motor_control_inputs.target_angle;
+  }
+
+  motor->loopFOC();
+  motor->move();
+  motor->monitor();
+  commander->run();
+}

controller/fw/embed/src/main.cpp

@@ -16,24 +16,15 @@
 
 #include "reg_cah.h"
 #include "flash.h"
-
+#include "config.h"
+#include "process_can.h"
 void SysTick_Handler(void) {
   HAL_IncTick();
 }
 
 STM32_CAN Can(CAN2, DEF);
-/*      FLASH Related Definitions      */
-uint32_t flash_flag;
-uint8_t flag_can = 0;
-uint32_t flash_error;
-FLASH_EraseInitTypeDef pEraseInit;
-uint32_t SectorError;
 
-volatile uint16_t msg_id;
+
-volatile uint16_t id_x;
-volatile uint8_t msg_ch;
-volatile uint8_t crc_h;
-volatile uint8_t crc_l;
 
 /* bool for test CAN    */
 volatile bool CAN_GET = false;
@@ -41,9 +32,8 @@ volatile bool CAN_GET = false;
 
 volatile float kt = 0.1;  // Torque calculation constant
 
-static FLASH_RECORD* flash_rec;
+FLASH_RECORD* flash_rec;
-static CAN_message_t CAN_TX_msg;
+
-static CAN_message_t CAN_inMsg;
 
 SPIClass spi;
 MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
@@ -58,322 +48,17 @@ LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
 
 Commander command(Serial);
 
-struct MotorControlInputs {
-  float target_angle = 0.0;
-  float target_velocity = 0.0;
-  bool motor_enabled = false;
-  bool foc_state = false;
-};
-
 MotorControlInputs motor_control_inputs;
 
-void doMotor(char *cmd) {
+ volatile uint16_t msg_id;
-  command.motor(&motor, cmd);
+ volatile uint16_t id_x;
-  digitalWrite(PC10, !digitalRead(PC10));
+ volatile uint8_t msg_ch;
-  delayMicroseconds(2);
+ volatile uint8_t crc_h;
-}
+ volatile uint8_t crc_l;
 
 
-
-void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
-               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
-               Commander *commander, CommandCallback callback) {
-  encoder->init(&spi);
-
-  // Driver configuration
-  driver->pwm_frequency = 20000;
-  driver->voltage_power_supply = 24;
-  driver->voltage_limit = 24;
-  driver->init();
-
-  // Current sense initialization
-  current_sense->linkDriver(driver);
-  current_sense->init();
-
-  // Motor configuration
-  motor->linkSensor(encoder);
-  motor->linkDriver(driver);
-  motor->linkCurrentSense(current_sense);
-  motor->useMonitoring(Serial);
-  motor->monitor_downsample = 5000;  // Default monitoring interval
-  motor->controller = MotionControlType::angle;
-  motor->torque_controller = TorqueControlType::voltage;
-  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
-
-  // PID Configuration
-  motor->PID_velocity.P = 0.75;
-  motor->PID_velocity.I = 20;
-  motor->LPF_velocity.Tf = 0.005;
-  motor->P_angle.P = 0.5;
-  motor->LPF_angle.Tf = 0.001;
-
-  // Motor limits
-  motor->velocity_limit = 40;  // Speed limit in rad/s (382 rpm)
-  motor->voltage_limit = 24;
-  motor->current_limit = 0.5;
-
-  motor->sensor_direction = Direction::CCW;
-  motor->init();
-  motor->initFOC();
-}
-
-template <typename T>
-void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
-  // Create CAN message
-  CAN_message_t msg_l;
-  msg_l.id = id;
-  // msg_l.len = 8;  // Protocol-defined message length
-  memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
-  memcpy(&msg_l.buf[1], data, sizeof(T));
-
-  // Prepare CRC calculation buffer (ID + data)
-  uint8_t crc_data[CAN_MSG_MAX_LEN];
-
-  // Copy message ID 
-  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
-  // Copy all data bytes
-  memcpy(crc_data + 1, msg_l.buf, 6);
-
-  // Calculate CRC  
-  uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
- 
-  // Insert CRC into buffer
-  msg_l.buf[6] = crc_value & 0xFF;
-  msg_l.buf[7] = (crc_value >> 8) & 0xFF;
-
-  // Send message
-  Can.write(msg_l);
-}
-  
-void send_velocity() {  
-  float current_velocity = motor.shaftVelocity();  
-  if (flash_rec == nullptr) {  // Null check  
-      // Error handling: logging, alerts, etc.  
-      return;
-  }
-  float value = flash_rec[vel].value;
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'V',&value);
-}
-
-void send_angle() {
-  float current_angle = motor.shaftAngle();
-  if (flash_rec == nullptr) {  // Null check
-      // Error handling: logging, alerts, etc.
-      return;
-  }
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'A',&current_angle);
-}
-
-void send_motor_enabled() {
-  int motor = 1;
-  if (flash_rec == nullptr) {  // Null check
-      // Error handling: logging, alerts, etc.
-      return;
-  }
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'M',&motor);
-}
-
-void send_foc_state() {
-  /* Firmware data reading */
-  if (flash_rec == nullptr) {  // Null check
-      // Error handling: logging, alerts, etc.
-      return;
-  }
-  uint8_t value = flash_rec[foc_id].value;
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'F',&value);
-}
-
-void send_id() {
-  /* Firmware data reading */
-  if (flash_rec == nullptr) {  // Null check
-      // Error handling: logging, alerts, etc.
-      return;
-  }
-
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'I',&id);
-  __NOP();
-}
-
-// void send_motor_torque() {
-//   float i_q = motor.current.q;  // Q-axis current (A)
-//   float torque = kt * i_q;      // Torque calculation
-//   torque *= 100;
-//   CAN_TX_msg.id = flash_rec->value;
-//   CAN_TX_msg.buf[0] = 'T';
-//   CAN_TX_msg.len = 5;
-//   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
-//   Can.write(CAN_TX_msg);
-// }
-
-void send_pid(uint8_t param_pid){
-  if (flash_rec == nullptr) {  // Null check
-    return;
-  }
-  uint8_t id = flash_rec[addr_id].value;
-  conv_float_to_int.i = flash_rec[param_pid].value;
-  uint32_t data = conv_float_to_int.i;
-  switch(param_pid){
-    case pid_p:
-      param_pid = REG_MOTOR_POSPID_Kp;
-      break;
-
-    case pid_i:
-      param_pid = REG_MOTOR_POSPID_Ki;
-      break;
-
-    case pid_d:
-      param_pid = REG_MOTOR_POSPID_Kd;
-      break;
-  }
-
-  send_can_with_id_crc(id,param_pid,&data);
-}
-
-void setup_id(uint8_t my_id) {
-  write_param(addr_id,my_id);
-}
-
-void setup_angle(float target_angle) {
-  motor.enable();  // Enable motor if disabled
-  motor.controller = MotionControlType::angle;
-  motor.move(target_angle);
-}
-
-void setup_pid_angle(uint8_t param_pid, float data){
-  switch (param_pid) {
-    case pid_p:
-      motor.P_angle.P = data;
-      break;
-    case pid_i:
-      motor.P_angle.I = data;
-      break;
-    case pid_d:
-      motor.P_angle.D = data;
-      break;
-    default:
-      break;
-  }
-
-  write_param(param_pid,data);
-}
-
-void listen_can(const CAN_message_t &msg) {
-  msg_id = msg.id;
-  msg_ch = msg_id & 0xF;        // Extract message channel
-  id_x = (msg_id >> 4) & 0x7FF; // Extract device address
-
-  /* CRC Calculation */
-  uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
-  uint8_t data[10] = {0}; // Message buffer for CRC verification
-
-  // Copy message ID (2 bytes)
-  memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
-  // Copy message data (excluding CRC bytes)
-  memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
-  
-  // Calculate CRC
-  uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
-  
-  // Verify CRC match
-  if (calculated_crc != received_crc) {
-    return; // Ignore message on CRC mismatch
-  }
-  flash_rec = load_params();
-  /* Message Structure: 0x691
-     69 - Device address
-     1 - Action code */
-  if(id_x == flash_rec[addr_id].value){
-    if(msg_ch == REG_WRITE){
-      switch(msg.buf[0]) {
-        case REG_ID:
-          setup_id(msg.buf[1]); 
-          break;
-        case REG_LED_BLINK:
-          for (int i = 0; i < 10; i++) {
-            GPIOC->ODR ^= GPIO_ODR_OD10;
-            delay(100);
-          }
-          break;
-          
-        case MOTOR_ANGLE:
-          memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
-                sizeof(motor_control_inputs.target_angle));
-          setup_angle(motor_control_inputs.target_angle);
-          break;
-
-        case REG_MOTOR_POSPID_Kp:
-          memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
-          conv_float_to_int.f = motor.P_angle.P;
-          write_param(pid_p,conv_float_to_int.i);
-          break;
-
-        case REG_MOTOR_POSPID_Ki:
-          memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
-          conv_float_to_int.f = motor.P_angle.I;
-          write_param(pid_i,conv_float_to_int.i);
-          break;
-
-        case REG_MOTOR_POSPID_Kd:
-          memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
-          conv_float_to_int.f = motor.P_angle.D;
-          write_param(pid_d,conv_float_to_int.i);
-          break;
-
-        case MOTOR_ENABLED:
-          if (msg.buf[1] == 1) {
-            motor.enable();
-            motor_control_inputs.motor_enabled = 1;
-          } else {
-            motor.disable();
-            motor_control_inputs.motor_enabled = 0;
-          }
-        default:
-          break;
-      }
-    }
-    else if (msg_ch == REG_READ) {
-      switch (msg.buf[0]) {
-        case REG_ID: send_id(); break;
-        case MOTOR_VELOCITY: send_velocity(); break;
-        case MOTOR_ANGLE: send_angle(); break;
-        case MOTOR_ENABLED: send_motor_enabled(); break;
-        // case MOTOR_TORQUE: send_motor_torque(); break;
-        case FOC_STATE: send_foc_state(); break;
-        case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break;
-        case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break;
-        case REG_MOTOR_POSPID_Kd: send_pid(pid_d); break;
-        default: break;
-      }
-    }
-  }
-}
-
 volatile uint32_t ipsr_value = 0;
 
-void foc_step(BLDCMotor *motor, Commander *commander) {
-  if (motor_control_inputs.target_velocity != 0 ||
-      motor->controller == MotionControlType::velocity) {
-    if (motor->controller != MotionControlType::velocity) {
-      motor->controller = MotionControlType::velocity;
-    }
-    motor->target = motor_control_inputs.target_velocity;
-  } else {
-    if (motor->controller != MotionControlType::angle) {
-      motor->controller = MotionControlType::angle;
-    }
-    motor->target = motor_control_inputs.target_angle;
-  }
-
-  motor->loopFOC();
-  motor->move();
-  motor->monitor();
-  commander->run();
-}
 
 
   void setup(){
@@ -395,12 +80,6 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
   CAN2->IER |= CAN_IER_FMPIE0;
 
   flash_rec = load_params();
-  /*  For test          */
-  // float value = 3.46;  //addr
-  // conv_float_to_int.f = value;
-  // write_param(pid_p,conv_float_to_int.i); //for update address in firmware
-  // // Load parameters from flash
-  // flash_rec = load_params();
   
   // Initialize FOC system
   setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);

controller/fw/embed/src/process_can.cpp

@@ -0,0 +1,234 @@
+#include "process_can.h"
+
+
+static CAN_message_t CAN_TX_msg;
+static CAN_message_t CAN_inMsg;
+
+
+template <typename T>
+void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
+  // Create CAN message
+  CAN_message_t msg_l;
+  msg_l.id = id;
+  // msg_l.len = 8;  // Protocol-defined message length
+  memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
+  memcpy(&msg_l.buf[1], data, sizeof(T));
+
+  // Prepare CRC calculation buffer (ID + data)
+  uint8_t crc_data[CAN_MSG_MAX_LEN];
+
+  // Copy message ID 
+  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
+  // Copy all data bytes
+  memcpy(crc_data + 1, msg_l.buf, 6);
+
+  // Calculate CRC  
+  uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
+ 
+  // Insert CRC into buffer
+//   memcpy(&msg_l.buf[6], &crc_value, sizeof(uint16_t));
+    msg_l.buf[6] = crc_value & 0xFF;
+    msg_l.buf[7] = (crc_value >> 8) & 0xFF;
+
+  // Send message
+  Can.write(msg_l);
+}
+
+void send_velocity() {  
+  float current_velocity = motor.shaftVelocity();  
+  if (flash_rec == nullptr) {  // Null check  
+      // Error handling: logging, alerts, etc.  
+      return;
+  }
+  float value = flash_rec[vel].value;
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'V',&value);
+}
+
+void send_angle() {
+  float current_angle = motor.shaftAngle();
+  if (flash_rec == nullptr) {  // Null check
+      // Error handling: logging, alerts, etc.
+      return;
+  }
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'A',&current_angle);
+}
+
+
+void send_motor_enabled() {
+  /* Firmware data reading */
+  if (flash_rec == nullptr) {  // Null check
+      // Error handling: logging, alerts, etc.
+      return;
+  }
+  uint8_t value = flash_rec[foc_id].value;
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'F',&value);
+}
+
+void send_id() {
+  /* Firmware data reading */
+  if (flash_rec == nullptr) {  // Null check
+      // Error handling: logging, alerts, etc.
+      return;
+  }
+
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'I',&id);
+  __NOP();
+}
+
+// void send_motor_torque() {
+//   float i_q = motor.current.q;  // Q-axis current (A)
+//   float torque = kt * i_q;      // Torque calculation
+//   torque *= 100;
+//   CAN_TX_msg.id = flash_rec->value;
+//   CAN_TX_msg.buf[0] = 'T';
+//   CAN_TX_msg.len = 5;
+//   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
+//   Can.write(CAN_TX_msg);
+// }
+
+void send_pid(uint8_t param_pid){
+  if (flash_rec == nullptr) {  // Null check
+    return;
+  }
+  uint8_t id = flash_rec[addr_id].value;
+  conv_float_to_int.i = flash_rec[param_pid].value;
+  uint32_t data = conv_float_to_int.i;
+  switch(param_pid){
+    case pid_p:
+      param_pid = REG_MOTOR_POSPID_Kp;
+      break;
+
+    case pid_i:
+      param_pid = REG_MOTOR_POSPID_Ki;
+      break;
+
+    case pid_d:
+      param_pid = REG_MOTOR_POSPID_Kd;
+      break;
+  }
+
+  send_can_with_id_crc(id,param_pid,&data);
+}
+
+void setup_id(uint8_t my_id) {
+  write_param(addr_id,my_id);
+}
+
+void setup_angle(float target_angle) {
+  motor.enable();  // Enable motor if disabled
+  motor.controller = MotionControlType::angle;
+  motor.move(target_angle);
+}
+
+void setup_pid_angle(uint8_t param_pid, float data){
+  switch (param_pid) {
+    case pid_p:
+      motor.P_angle.P = data;
+      break;
+    case pid_i:
+      motor.P_angle.I = data;
+      break;
+    case pid_d:
+      motor.P_angle.D = data;
+      break;
+    default:
+      break;
+  }
+
+  write_param(param_pid,data);
+}
+
+void listen_can(const CAN_message_t &msg) {
+  msg_id = msg.id;
+  msg_ch = msg_id & 0xF;        // Extract message channel
+  uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
+
+  /* CRC Calculation */
+  uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
+  uint8_t data[10] = {0}; // Message buffer for CRC verification
+
+  // Copy message ID (2 bytes)
+  memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
+  // Copy message data (excluding CRC bytes)
+  memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
+  
+  // Calculate CRC
+  uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
+  
+  // Verify CRC match
+  if (calculated_crc != received_crc) {
+    return; // Ignore message on CRC mismatch
+  }
+  flash_rec = load_params();
+  /* Message Structure: 0x691
+     69 - Device address
+     1 - Action code */
+  if(id_x == flash_rec[addr_id].value){
+    if(msg_ch == REG_WRITE){
+      switch(msg.buf[0]) {
+        case REG_ID:
+          setup_id(msg.buf[1]); 
+          break;
+        case REG_LED_BLINK:
+          for (int i = 0; i < 10; i++) {
+            GPIOC->ODR ^= GPIO_ODR_OD10;
+            delay(100);
+          }
+          break;
+          
+        case MOTOR_ANGLE:
+          memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
+                sizeof(motor_control_inputs.target_angle));
+          setup_angle(motor_control_inputs.target_angle);
+          break;
+
+        case REG_MOTOR_POSPID_Kp:
+          memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
+          conv_float_to_int.f = motor.P_angle.P;
+          write_param(pid_p,conv_float_to_int.i);
+          break;
+
+        case REG_MOTOR_POSPID_Ki:
+          memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
+          conv_float_to_int.f = motor.P_angle.I;
+          write_param(pid_i,conv_float_to_int.i);
+          break;
+
+        case REG_MOTOR_POSPID_Kd:
+          memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
+          conv_float_to_int.f = motor.P_angle.D;
+          write_param(pid_d,conv_float_to_int.i);
+          break;
+
+        case MOTOR_ENABLED:
+          if (msg.buf[1] == 1) {
+            motor.enable();
+            motor_control_inputs.motor_enabled = 1;
+          } else {
+            motor.disable();
+            motor_control_inputs.motor_enabled = 0;
+          }
+        default:
+          break;
+      }
+    }
+    else if (msg_ch == REG_READ) {
+      switch (msg.buf[0]) {
+        case REG_ID: send_id(); break;
+        case MOTOR_VELOCITY: send_velocity(); break;
+        case MOTOR_ANGLE: send_angle(); break;
+        case MOTOR_ENABLED: send_motor_enabled(); break;
+        // case MOTOR_TORQUE: send_motor_torque(); break;
+        // case FOC_STATE: send_foc_state(); break;
+        case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break;
+        case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break;
+        case REG_MOTOR_POSPID_Kd: send_pid(pid_d); break;
+        default: break;
+      }
+    }
+  }
+}