2025-05-16 21:06:31 +03:00
|
|
|
#include "config.h"
|
|
|
|
|
2025-05-27 16:33:02 +03:00
|
|
|
|
2025-05-16 21:06:31 +03:00
|
|
|
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,
|
2025-05-27 16:33:02 +03:00
|
|
|
Commander *commander, CommandCallback callback,FLASH_RECORD* pid_data) {
|
2025-05-16 21:06:31 +03:00
|
|
|
encoder->init(&spi);
|
2025-05-27 16:33:02 +03:00
|
|
|
|
|
|
|
/* 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;
|
2025-05-16 21:06:31 +03:00
|
|
|
|
|
|
|
// 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
|
2025-05-22 18:03:43 +03:00
|
|
|
motor->PID_velocity.P = 0.75f;
|
|
|
|
motor->PID_velocity.I = 20.0f;
|
|
|
|
motor->LPF_velocity.Tf = 0.005f;
|
2025-05-27 16:33:02 +03:00
|
|
|
motor->P_angle.P = p;
|
|
|
|
motor->P_angle.I = i;
|
|
|
|
motor->P_angle.D = d;
|
2025-05-22 18:03:43 +03:00
|
|
|
motor->LPF_angle.Tf = 0.001f;
|
2025-05-16 21:06:31 +03:00
|
|
|
|
|
|
|
// Motor limits
|
|
|
|
motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm)
|
|
|
|
motor->voltage_limit = 24;
|
|
|
|
motor->current_limit = 0.5;
|
2025-05-27 16:33:02 +03:00
|
|
|
|
2025-05-16 21:06:31 +03:00
|
|
|
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();
|
2025-05-27 16:33:02 +03:00
|
|
|
}
|