Refactor and enhance motor control implementation
- Introduced `MotorControlInputs` struct to centralize motor control state. - Renamed `sensor` to `encoder` for clarity in naming conventions. - Added `send_velocity`, `send_angle`, and `send_motor_enabled` functions for modular CAN message handling. - Updated `setup_foc` to enhance motor initialization with configurable parameters. - Replaced `run_foc` with `foc_step` for improved motor control logic: - Dynamically switches between angle and velocity control based on inputs. - Enhanced CAN message handling with `read_can_step` to process specific control commands. - Adjusted current sensing and other configuration values for precision. - Added placeholders for temperature sensor setup and handling.
This commit is contained in:
parent
0199c1e38f
commit
1534b854fc
1 changed files with 152 additions and 79 deletions
|
|
@ -4,7 +4,12 @@
|
||||||
#include <STM32_CAN.h>
|
#include <STM32_CAN.h>
|
||||||
#include <AS5045.h>
|
#include <AS5045.h>
|
||||||
#include <DRV8313.h>
|
#include <DRV8313.h>
|
||||||
|
#include <cstring>
|
||||||
|
#include <iterator>
|
||||||
|
#include "common/base_classes/FOCMotor.h"
|
||||||
#include "hal_conf_extra.h"
|
#include "hal_conf_extra.h"
|
||||||
|
#include "wiring_analog.h"
|
||||||
|
#include "wiring_constants.h"
|
||||||
// clang-format on
|
// clang-format on
|
||||||
|
|
||||||
STM32_CAN Can(CAN2, DEF);
|
STM32_CAN Can(CAN2, DEF);
|
||||||
|
|
@ -13,109 +18,177 @@ static CAN_message_t CAN_TX_msg;
|
||||||
static CAN_message_t CAN_inMsg;
|
static CAN_message_t CAN_inMsg;
|
||||||
|
|
||||||
SPIClass spi;
|
SPIClass spi;
|
||||||
MagneticSensorAS5045 sensor(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
|
MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
|
||||||
|
|
||||||
BLDCMotor motor(POLE_PAIRS);
|
BLDCMotor motor(POLE_PAIRS);
|
||||||
|
|
||||||
DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
|
DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
|
||||||
EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
|
EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
|
||||||
RESET_DRIVER, FAULT_DRIVER);
|
RESET_DRIVER, FAULT_DRIVER);
|
||||||
LowsideCurrentSense current_sense(0.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2,
|
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
|
||||||
CURRENT_SENSOR_3);
|
CURRENT_SENSOR_2, CURRENT_SENSOR_3);
|
||||||
|
|
||||||
Commander command(Serial);
|
Commander command(Serial);
|
||||||
|
|
||||||
|
struct MotorControlInputs {
|
||||||
|
float target_angle;
|
||||||
|
float target_velocity;
|
||||||
|
bool motor_enabled;
|
||||||
|
};
|
||||||
|
|
||||||
|
MotorControlInputs motor_control_inputs;
|
||||||
|
|
||||||
void doMotor(char *cmd) {
|
void doMotor(char *cmd) {
|
||||||
command.motor(&motor, cmd);
|
command.motor(&motor, cmd);
|
||||||
digitalWrite(PC10, !digitalRead(PC10));
|
// digitalWrite(PC10, !digitalRead(PC10));
|
||||||
delayMicroseconds(2);
|
// delayMicroseconds(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver,
|
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
|
||||||
LowsideCurrentSense *current_sense, Commander *commander,
|
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
|
||||||
CommandCallback callback) {
|
Commander *commander, CommandCallback callback) {
|
||||||
sensor->init(&spi);
|
encoder->init(&spi);
|
||||||
motor->linkSensor(sensor);
|
motor->linkSensor(encoder);
|
||||||
driver->pwm_frequency = 20000;
|
driver->pwm_frequency = 20000;
|
||||||
driver->voltage_power_supply = 20;
|
driver->voltage_power_supply = 20;
|
||||||
driver->voltage_limit = 40;
|
driver->voltage_limit = 40;
|
||||||
driver->init();
|
driver->init();
|
||||||
motor->linkDriver(driver);
|
motor->linkDriver(driver);
|
||||||
current_sense->linkDriver(driver);
|
current_sense->linkDriver(driver);
|
||||||
current_sense->init();
|
current_sense->init();
|
||||||
motor->linkCurrentSense(current_sense);
|
motor->linkCurrentSense(current_sense);
|
||||||
SimpleFOCDebug::enable(nullptr);
|
SimpleFOCDebug::enable(nullptr);
|
||||||
commander->add('M', callback, "motor");
|
commander->add('M', callback, "motor");
|
||||||
motor->useMonitoring(Serial);
|
motor->useMonitoring(Serial);
|
||||||
motor->monitor_start_char = 'M';
|
motor->monitor_start_char = 'M';
|
||||||
motor->monitor_end_char = 'M';
|
motor->monitor_end_char = 'M';
|
||||||
commander->verbose = VerboseMode::machine_readable;
|
// motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
|
||||||
motor->controller = MotionControlType::angle;
|
motor->monitor_downsample = 10; // default 10
|
||||||
motor->torque_controller = TorqueControlType::voltage;
|
commander->verbose = VerboseMode::user_friendly;
|
||||||
motor->foc_modulation = FOCModulationType::SinePWM;
|
motor->controller = MotionControlType::angle;
|
||||||
motor->PID_velocity.P = 0.3;
|
motor->torque_controller = TorqueControlType::voltage;
|
||||||
motor->PID_velocity.I = 15;
|
motor->foc_modulation = FOCModulationType::SinePWM;
|
||||||
motor->PID_velocity.D = 0.001;
|
motor->PID_velocity.P = 0.3;
|
||||||
motor->PID_velocity.output_ramp = 1000;
|
motor->PID_velocity.I = 15;
|
||||||
motor->LPF_velocity.Tf = 0.005;
|
motor->PID_velocity.D = 0.001;
|
||||||
motor->LPF_angle.Tf = 0.005;
|
motor->PID_velocity.output_ramp = 1000;
|
||||||
motor->P_angle.P = 15;
|
motor->LPF_velocity.Tf = 0.005;
|
||||||
motor->velocity_limit = 40;
|
motor->LPF_angle.Tf = 0.005;
|
||||||
motor->voltage_limit = 40;
|
motor->P_angle.P = 15;
|
||||||
motor->sensor_direction = Direction::CCW;
|
motor->velocity_limit = 40;
|
||||||
current_sense->skip_align = true;
|
motor->voltage_limit = 40;
|
||||||
motor->init();
|
motor->current_limit = 0.5;
|
||||||
motor->initFOC();
|
motor->sensor_direction = Direction::CCW;
|
||||||
|
current_sense->skip_align = true;
|
||||||
|
motor->init();
|
||||||
|
motor->initFOC();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void send_velocity() {
|
||||||
|
float current_velocity = motor.shaftVelocity();
|
||||||
|
CAN_TX_msg.id = 1;
|
||||||
|
CAN_TX_msg.buf[0] = 'V';
|
||||||
|
CAN_TX_msg.len = 5;
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_angle() {
|
||||||
|
float current_angle = motor.shaftAngle();
|
||||||
|
CAN_TX_msg.id = 1;
|
||||||
|
CAN_TX_msg.buf[0] = 'A';
|
||||||
|
CAN_TX_msg.len = 5;
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_motor_enabled() {
|
||||||
|
CAN_TX_msg.id = 1;
|
||||||
|
CAN_TX_msg.buf[0] = 'E';
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
|
||||||
|
sizeof(motor_control_inputs.motor_enabled));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
void send_data() {
|
void send_data() {
|
||||||
// TODO(vanyabeat) add can functionability for ROS packets
|
// TODO(vanyabeat) add can functionability for ROS packets
|
||||||
CAN_TX_msg.id = 1;
|
send_velocity();
|
||||||
CAN_TX_msg.buf[0] = 'A';
|
send_angle();
|
||||||
CAN_TX_msg.len = 5;
|
send_motor_enabled();
|
||||||
Can.write(CAN_TX_msg);
|
// read_temperature();
|
||||||
digitalWrite(PC11, !digitalRead(PC11));
|
digitalWrite(PC11, !digitalRead(PC11));
|
||||||
}
|
}
|
||||||
|
|
||||||
void read_can_message() {
|
void read_can_step() {
|
||||||
float angle;
|
char flag = CAN_inMsg.buf[0];
|
||||||
memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle));
|
if (flag == 'V') {
|
||||||
motor.target = angle;
|
memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1],
|
||||||
digitalWrite(PC10, !digitalRead(PC10));
|
sizeof(motor_control_inputs.target_velocity));
|
||||||
|
} else if (flag == 'A') {
|
||||||
|
memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
|
||||||
|
sizeof(motor_control_inputs.target_angle));
|
||||||
|
} else if (flag == 'E') {
|
||||||
|
bool enable_flag = CAN_inMsg.buf[1];
|
||||||
|
if (enable_flag == 1) {
|
||||||
|
memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1],
|
||||||
|
sizeof(motor_control_inputs.motor_enabled));
|
||||||
|
motor.enable();
|
||||||
|
} else if (enable_flag == 0) {
|
||||||
|
memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1],
|
||||||
|
sizeof(motor_control_inputs.motor_enabled));
|
||||||
|
motor.disable();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// motor.target = angle;
|
||||||
|
digitalWrite(PC10, !digitalRead(PC10));
|
||||||
}
|
}
|
||||||
|
|
||||||
void run_foc(BLDCMotor *motor, Commander *commander) {
|
void foc_step(BLDCMotor *motor, Commander *commander) {
|
||||||
motor->loopFOC();
|
if (motor_control_inputs.target_velocity != 0 ||
|
||||||
motor->move();
|
motor->controller == MotionControlType::velocity) {
|
||||||
motor->monitor();
|
if (motor->controller != MotionControlType::velocity) {
|
||||||
commander->run();
|
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();
|
||||||
}
|
}
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
||||||
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
// setup led pin
|
// setup led pin
|
||||||
pinMode(PC11, OUTPUT);
|
pinMode(PC11, OUTPUT);
|
||||||
pinMode(PC10, OUTPUT);
|
pinMode(PC10, OUTPUT);
|
||||||
Can.begin();
|
// Setup thermal sensor pin
|
||||||
Can.setBaudRate(1000000);
|
// pinMode(TH1, INPUT_ANALOG);
|
||||||
TIM_TypeDef *Instance = TIM2;
|
Can.begin();
|
||||||
HardwareTimer *SendTimer = new HardwareTimer(Instance);
|
Can.setBaudRate(1000000);
|
||||||
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
TIM_TypeDef *Instance = TIM2;
|
||||||
SendTimer->attachInterrupt(send_data);
|
HardwareTimer *SendTimer = new HardwareTimer(Instance);
|
||||||
SendTimer->resume();
|
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
||||||
setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor);
|
SendTimer->attachInterrupt(send_data);
|
||||||
_delay(1000);
|
SendTimer->resume();
|
||||||
|
setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);
|
||||||
|
_delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
float current_angle = 0;
|
float current_angle = 0.0;
|
||||||
void loop() {
|
void loop() {
|
||||||
run_foc(&motor, &command);
|
foc_step(&motor, &command);
|
||||||
current_angle = sensor.getAngle();
|
// current_angle = encoder.getAngle();
|
||||||
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
// memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||||
while (Can.read(CAN_inMsg)) {
|
while (Can.read(CAN_inMsg)) {
|
||||||
read_can_message();
|
read_can_step();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue