servo/controller/fw/embed/src/config.cpp

86 lines
2.3 KiB
C++
Raw Normal View History

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
}