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 <AS5045.h>
|
||||
#include <DRV8313.h>
|
||||
#include <cstring>
|
||||
#include <iterator>
|
||||
#include "common/base_classes/FOCMotor.h"
|
||||
#include "hal_conf_extra.h"
|
||||
#include "wiring_analog.h"
|
||||
#include "wiring_constants.h"
|
||||
// clang-format on
|
||||
|
||||
STM32_CAN Can(CAN2, DEF);
|
||||
|
|
@ -13,29 +18,37 @@ static CAN_message_t CAN_TX_msg;
|
|||
static CAN_message_t CAN_inMsg;
|
||||
|
||||
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);
|
||||
|
||||
DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
|
||||
EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
|
||||
RESET_DRIVER, FAULT_DRIVER);
|
||||
LowsideCurrentSense current_sense(0.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2,
|
||||
CURRENT_SENSOR_3);
|
||||
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
|
||||
CURRENT_SENSOR_2, CURRENT_SENSOR_3);
|
||||
|
||||
Commander command(Serial);
|
||||
|
||||
struct MotorControlInputs {
|
||||
float target_angle;
|
||||
float target_velocity;
|
||||
bool motor_enabled;
|
||||
};
|
||||
|
||||
MotorControlInputs motor_control_inputs;
|
||||
|
||||
void doMotor(char *cmd) {
|
||||
command.motor(&motor, cmd);
|
||||
digitalWrite(PC10, !digitalRead(PC10));
|
||||
delayMicroseconds(2);
|
||||
// digitalWrite(PC10, !digitalRead(PC10));
|
||||
// delayMicroseconds(2);
|
||||
}
|
||||
|
||||
void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver,
|
||||
LowsideCurrentSense *current_sense, Commander *commander,
|
||||
CommandCallback callback) {
|
||||
sensor->init(&spi);
|
||||
motor->linkSensor(sensor);
|
||||
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
|
||||
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
|
||||
Commander *commander, CommandCallback callback) {
|
||||
encoder->init(&spi);
|
||||
motor->linkSensor(encoder);
|
||||
driver->pwm_frequency = 20000;
|
||||
driver->voltage_power_supply = 20;
|
||||
driver->voltage_limit = 40;
|
||||
|
|
@ -49,7 +62,9 @@ void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *dr
|
|||
motor->useMonitoring(Serial);
|
||||
motor->monitor_start_char = 'M';
|
||||
motor->monitor_end_char = 'M';
|
||||
commander->verbose = VerboseMode::machine_readable;
|
||||
// motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
|
||||
motor->monitor_downsample = 10; // default 10
|
||||
commander->verbose = VerboseMode::user_friendly;
|
||||
motor->controller = MotionControlType::angle;
|
||||
motor->torque_controller = TorqueControlType::voltage;
|
||||
motor->foc_modulation = FOCModulationType::SinePWM;
|
||||
|
|
@ -62,30 +77,86 @@ void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *dr
|
|||
motor->P_angle.P = 15;
|
||||
motor->velocity_limit = 40;
|
||||
motor->voltage_limit = 40;
|
||||
motor->current_limit = 0.5;
|
||||
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_data() {
|
||||
// TODO(vanyabeat) add can functionability for ROS packets
|
||||
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() {
|
||||
// TODO(vanyabeat) add can functionability for ROS packets
|
||||
send_velocity();
|
||||
send_angle();
|
||||
send_motor_enabled();
|
||||
// read_temperature();
|
||||
digitalWrite(PC11, !digitalRead(PC11));
|
||||
}
|
||||
|
||||
void read_can_message() {
|
||||
float angle;
|
||||
memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle));
|
||||
motor.target = angle;
|
||||
void read_can_step() {
|
||||
char flag = CAN_inMsg.buf[0];
|
||||
if (flag == 'V') {
|
||||
memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1],
|
||||
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) {
|
||||
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();
|
||||
|
|
@ -99,6 +170,8 @@ void setup() {
|
|||
// setup led pin
|
||||
pinMode(PC11, OUTPUT);
|
||||
pinMode(PC10, OUTPUT);
|
||||
// Setup thermal sensor pin
|
||||
// pinMode(TH1, INPUT_ANALOG);
|
||||
Can.begin();
|
||||
Can.setBaudRate(1000000);
|
||||
TIM_TypeDef *Instance = TIM2;
|
||||
|
|
@ -106,16 +179,16 @@ void setup() {
|
|||
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
||||
SendTimer->attachInterrupt(send_data);
|
||||
SendTimer->resume();
|
||||
setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor);
|
||||
setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
float current_angle = 0;
|
||||
float current_angle = 0.0;
|
||||
void loop() {
|
||||
run_foc(&motor, &command);
|
||||
current_angle = sensor.getAngle();
|
||||
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||
foc_step(&motor, &command);
|
||||
// current_angle = encoder.getAngle();
|
||||
// memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||
while (Can.read(CAN_inMsg)) {
|
||||
read_can_message();
|
||||
read_can_step();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue