robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

PID CAN setup fix

Browse source
This commit is contained in:
Valentin Dabstep 2025-05-27 16:33:02 +03:00
parent 013768ad1c
commit 5fa3db6e6c
4 changed files with 37 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

4
controller/fw/embed/include/config.h
View file

@ -4,7 +4,7 @@
#include <DRV8313.h> #include <DRV8313.h>
#include <SimpleFOC.h> #include <SimpleFOC.h>
#include <STM32_CAN.h> #include <STM32_CAN.h>
#include "flash.h"
extern STM32_CAN Can; extern STM32_CAN Can;
extern SPIClass spi; extern SPIClass spi;
@ -26,6 +26,6 @@ extern MotorControlInputs motor_control_inputs;
void doMotor(char *cmd); void doMotor(char *cmd);
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense, DRV8313Driver *driver, LowsideCurrentSense *current_sense,
Commander *commander, CommandCallback callback); Commander *commander, CommandCallback callback,FLASH_RECORD* pid_data);
void foc_step(BLDCMotor *motor, Commander *commander); void foc_step(BLDCMotor *motor, Commander *commander);

21
controller/fw/embed/src/config.cpp
View file

@ -1,5 +1,6 @@
#include "config.h" #include "config.h"
void doMotor(char *cmd) { void doMotor(char *cmd) {
command.motor(&motor, cmd); command.motor(&motor, cmd);
digitalWrite(PC10, !digitalRead(PC10)); digitalWrite(PC10, !digitalRead(PC10));
@ -10,8 +11,18 @@ void doMotor(char *cmd) {
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense, DRV8313Driver *driver, LowsideCurrentSense *current_sense,
Commander *commander, CommandCallback callback) { Commander *commander, CommandCallback callback,FLASH_RECORD* pid_data) {
encoder->init(&spi); encoder->init(&spi);
/* convert data from flash int value to float*/
conv_float_to_int.i = pid_data[pid_p].value;
float p = conv_float_to_int.f;
conv_float_to_int.i = pid_data[pid_i].value;
float i = conv_float_to_int.f;
conv_float_to_int.i = pid_data[pid_d].value;
float d = conv_float_to_int.f;
// Driver configuration // Driver configuration
driver->pwm_frequency = 20000; driver->pwm_frequency = 20000;
@ -37,14 +48,16 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
motor->PID_velocity.P = 0.75f; motor->PID_velocity.P = 0.75f;
motor->PID_velocity.I = 20.0f; motor->PID_velocity.I = 20.0f;
motor->LPF_velocity.Tf = 0.005f; motor->LPF_velocity.Tf = 0.005f;
motor->P_angle.P = 0.5f; motor->P_angle.P = p;
motor->P_angle.I = i;
motor->P_angle.D = d;
motor->LPF_angle.Tf = 0.001f; motor->LPF_angle.Tf = 0.001f;
// Motor limits // Motor limits
motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm) motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm)
motor->voltage_limit = 24; motor->voltage_limit = 24;
motor->current_limit = 0.5; motor->current_limit = 0.5;
motor->sensor_direction = Direction::CCW; motor->sensor_direction = Direction::CCW;
motor->init(); motor->init();
motor->initFOC(); motor->initFOC();
@ -69,4 +82,4 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
motor->move(); motor->move();
motor->monitor(); motor->monitor();
commander->run(); commander->run();
} }

2
controller/fw/embed/src/main.cpp
View file

@ -83,7 +83,7 @@ volatile uint32_t ipsr_value = 0;
// flash_rec = load_params(); // flash_rec = load_params();
// Initialize FOC system // Initialize FOC system
setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor); setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor,flash_rec);
CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0 CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0
CAN_IER_FFIE0 | // FIFO0 full CAN_IER_FFIE0 | // FIFO0 full

33
controller/fw/embed/src/process_can.cpp
View file

@ -129,23 +129,24 @@ void setup_angle(float target_angle) {
motor.move(target_angle); motor.move(target_angle);
} }
void setup_pid_angle(uint8_t param_pid, float data){ // void setup_pid_angle(uint8_t param_pid, uint32_t data){
switch (param_pid) { // conv_float_to_int.f = data;
case pid_p: // switch (param_pid) {
motor.P_angle.P = data; // case pid_p:
break; // motor.P_angle.P = conv_float_to_int.f;
case pid_i: // break;
motor.P_angle.I = data; // case pid_i:
break; // motor.P_angle.I = conv_float_to_int.f;
case pid_d: // break;
motor.P_angle.D = data; // case pid_d:
break; // motor.P_angle.D = conv_float_to_int.f;
default: // break;
break; // default:
} // break;
// }
write_param(param_pid,data); // write_param(param_pid,data);
} // }
void listen_can(const CAN_message_t &msg) { void listen_can(const CAN_message_t &msg) {