From 4f42094b0ea9fdbc12ed29f4585cf67e4995e353 Mon Sep 17 00:00:00 2001 From: Valentin Dabstep Date: Fri, 16 May 2025 21:06:31 +0300 Subject: [PATCH] Divide code --- controller/fw/embed/include/config.h | 31 ++ controller/fw/embed/include/flash.h | 1 + controller/fw/embed/include/process_can.h | 25 ++ controller/fw/embed/src/config.cpp | 72 +++++ controller/fw/embed/src/main.cpp | 341 +--------------------- controller/fw/embed/src/process_can.cpp | 234 +++++++++++++++ 6 files changed, 373 insertions(+), 331 deletions(-) create mode 100644 controller/fw/embed/include/config.h create mode 100644 controller/fw/embed/include/process_can.h create mode 100644 controller/fw/embed/src/config.cpp create mode 100644 controller/fw/embed/src/process_can.cpp diff --git a/controller/fw/embed/include/config.h b/controller/fw/embed/include/config.h new file mode 100644 index 0000000..26ac082 --- /dev/null +++ b/controller/fw/embed/include/config.h @@ -0,0 +1,31 @@ +#pragma once +#include "Arduino.h" +#include +#include +#include +#include + + +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); diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index c550a9a..0b7f427 100644 --- a/controller/fw/embed/include/flash.h +++ b/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); diff --git a/controller/fw/embed/include/process_can.h b/controller/fw/embed/include/process_can.h new file mode 100644 index 0000000..316e07e --- /dev/null +++ b/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); \ No newline at end of file diff --git a/controller/fw/embed/src/config.cpp b/controller/fw/embed/src/config.cpp new file mode 100644 index 0000000..c10cb4a --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 4674d2d..c4bbd40 100644 --- a/controller/fw/embed/src/main.cpp +++ b/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; -static CAN_message_t CAN_TX_msg; -static CAN_message_t CAN_inMsg; +FLASH_RECORD* flash_rec; + 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) { - command.motor(&motor, cmd); - digitalWrite(PC10, !digitalRead(PC10)); - delayMicroseconds(2); -} + 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; - -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 -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',¤t_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, ¤t_sense, &command, doMotor); diff --git a/controller/fw/embed/src/process_can.cpp b/controller/fw/embed/src/process_can.cpp new file mode 100644 index 0000000..aa469be --- /dev/null +++ b/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 +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',¤t_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; + } + } + } +} \ No newline at end of file