72 lines
2 KiB
C++
72 lines
2 KiB
C++
|
#include "config.h"
|
||
|
|
|
||
|
|
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,
|
||
|
|
Commander *commander, CommandCallback callback) {
|
||
|
|
encoder->init(&spi);
|
||
|
|
|
||
|
|
// 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
|
||
|
|
motor->PID_velocity.P = 0.75;
|
||
|
|
motor->PID_velocity.I = 20;
|
||
|
|
motor->LPF_velocity.Tf = 0.005;
|
||
|
|
motor->P_angle.P = 0.5;
|
||
|
|
motor->LPF_angle.Tf = 0.001;
|
||
|
|
|
||
|
|
// 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();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|