122 lines
3.6 KiB
C++
122 lines
3.6 KiB
C++
// clang-format off
|
|
#include <Arduino.h>
|
|
#include <SimpleFOC.h>
|
|
#include <STM32_CAN.h>
|
|
#include <AS5045.h>
|
|
#include <DRV8313.h>
|
|
#include "hal_conf_extra.h"
|
|
// clang-format on
|
|
|
|
STM32_CAN Can(CAN2, DEF);
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
Commander command(Serial);
|
|
|
|
void doMotor(char *cmd) {
|
|
command.motor(&motor, cmd);
|
|
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);
|
|
driver->pwm_frequency = 20000;
|
|
driver->voltage_power_supply = 24;
|
|
driver->voltage_limit = 24;
|
|
driver->init();
|
|
motor->linkDriver(driver);
|
|
current_sense->linkDriver(driver);
|
|
current_sense->init();
|
|
motor->linkCurrentSense(current_sense);
|
|
SimpleFOCDebug::enable(nullptr);
|
|
commander->add('M', callback, "motor");
|
|
motor->useMonitoring(Serial);
|
|
motor->monitor_start_char = 'M';
|
|
motor->monitor_end_char = 'M';
|
|
commander->verbose = VerboseMode::machine_readable;
|
|
motor->monitor_downsample = 5000;
|
|
motor->controller = MotionControlType::angle;
|
|
motor->torque_controller = TorqueControlType::voltage;
|
|
motor->foc_modulation = FOCModulationType::SinePWM;
|
|
motor->PID_velocity.P = 0.3;
|
|
motor->PID_velocity.I = 15;
|
|
motor->PID_velocity.D = 0.001;
|
|
motor->PID_velocity.output_ramp = 1000;
|
|
motor->LPF_velocity.Tf = 0.005;
|
|
motor->LPF_angle.Tf = 0.005;
|
|
motor->P_angle.P = 15;
|
|
motor->velocity_limit = 40;
|
|
motor->voltage_limit = 40;
|
|
motor->sensor_direction = Direction::CCW;
|
|
current_sense->skip_align = true;
|
|
motor->init();
|
|
motor->initFOC();
|
|
}
|
|
|
|
|
|
void send_data() {
|
|
// TODO(vanyabeat) add can functionability for ROS packets
|
|
CAN_TX_msg.id = 1;
|
|
CAN_TX_msg.buf[0] = 'A';
|
|
CAN_TX_msg.len = 5;
|
|
Can.write(CAN_TX_msg);
|
|
digitalWrite(PC11, !digitalRead(PC11));
|
|
}
|
|
|
|
void read_can_message() {
|
|
float angle;
|
|
memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle));
|
|
motor.target = angle;
|
|
digitalWrite(PC10, !digitalRead(PC10));
|
|
}
|
|
|
|
void run_foc(BLDCMotor *motor, Commander *commander) {
|
|
motor->loopFOC();
|
|
motor->move();
|
|
motor->monitor();
|
|
commander->run();
|
|
}
|
|
|
|
void setup() {
|
|
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
|
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
|
Serial.begin(115200);
|
|
// setup led pin
|
|
pinMode(PC11, OUTPUT);
|
|
pinMode(PC10, OUTPUT);
|
|
Can.begin();
|
|
Can.setBaudRate(1000000);
|
|
TIM_TypeDef *Instance = TIM2;
|
|
HardwareTimer *SendTimer = new HardwareTimer(Instance);
|
|
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
|
SendTimer->attachInterrupt(send_data);
|
|
SendTimer->resume();
|
|
setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor);
|
|
_delay(1000);
|
|
}
|
|
|
|
float current_angle = 0;
|
|
void loop() {
|
|
run_foc(&motor, &command);
|
|
current_angle = sensor.getAngle();
|
|
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
|
while (Can.read(CAN_inMsg)) {
|
|
read_can_message();
|
|
}
|
|
}
|