PID CAN setup fix
This commit is contained in:
parent
013768ad1c
commit
5fa3db6e6c
4 changed files with 37 additions and 23 deletions
|
@ -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);
|
||||||
|
|
|
@ -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();
|
||||||
}
|
}
|
||||||
|
|
|
@ -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, ¤t_sense, &command, doMotor);
|
setup_foc(&encoder, &motor, &driver, ¤t_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
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue