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 <SimpleFOC.h>
#include <STM32_CAN.h>
#include "flash.h"
extern STM32_CAN Can;
extern SPIClass spi;
@ -26,6 +26,6 @@ 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);
Commander *commander, CommandCallback callback,FLASH_RECORD* pid_data);
void foc_step(BLDCMotor *motor, Commander *commander);

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

@ -1,5 +1,6 @@
#include "config.h"
void doMotor(char *cmd) {
command.motor(&motor, cmd);
digitalWrite(PC10, !digitalRead(PC10));
@ -10,8 +11,18 @@ void doMotor(char *cmd) {
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
Commander *commander, CommandCallback callback) {
Commander *commander, CommandCallback callback,FLASH_RECORD* pid_data) {
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->pwm_frequency = 20000;
@ -37,14 +48,16 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
motor->PID_velocity.P = 0.75f;
motor->PID_velocity.I = 20.0f;
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 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();
@ -69,4 +82,4 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
motor->move();
motor->monitor();
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();
// 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
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);
}
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;
}
// void setup_pid_angle(uint8_t param_pid, uint32_t data){
// conv_float_to_int.f = data;
// switch (param_pid) {
// case pid_p:
// motor.P_angle.P = conv_float_to_int.f;
// break;
// case pid_i:
// motor.P_angle.I = conv_float_to_int.f;
// break;
// case pid_d:
// motor.P_angle.D = conv_float_to_int.f;
// break;
// default:
// break;
// }
write_param(param_pid,data);
}
// write_param(param_pid,data);
// }
void listen_can(const CAN_message_t &msg) {