122 lines
No EOL
3.4 KiB
C++
122 lines
No EOL
3.4 KiB
C++
#include <AS5040.h>
|
|
#include <Arduino.h>
|
|
#include <SPI.h>
|
|
#include <SimpleFOC.h>
|
|
|
|
#pragma region "Motor and sensor setup"
|
|
#define LED1 PC10
|
|
#define LED2 PC11
|
|
#define HARDWARE_SERIAL_RX_PIN PB7
|
|
#define HARDWARE_SERIAL_TX_PIN PB6
|
|
#define AS5040_CS PB15
|
|
#define AS5040_MISO PB14
|
|
#define AS5040_MOSI PC1
|
|
#define AS5040_SCLK PB10
|
|
#define CURRENT_SENSOR_1 PB1 // INA180A
|
|
#define CURRENT_SENSOR_2 PB0
|
|
#define CURRENT_SENSOR_3 PC5
|
|
#pragma endregion
|
|
// simple foc debug mode
|
|
#define DEBUG_SIMPLE_FOC
|
|
|
|
// Init AS5040
|
|
AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
|
|
// Init iPower 6208
|
|
BLDCMotor motor = BLDCMotor(16);
|
|
// Init 3PWM driver DRV8313
|
|
BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10, PC6, PA11, PA12);
|
|
|
|
// Init current sense
|
|
InlineCurrentSense current_sense = InlineCurrentSense(
|
|
0.0263, 100, CURRENT_SENSOR_2, CURRENT_SENSOR_1, CURRENT_SENSOR_3);
|
|
// Init UART Commander
|
|
Commander command = Commander(Serial);
|
|
void doMotor(char *cmd) { command.motor(&motor, cmd); }
|
|
|
|
void setup() {
|
|
SimpleFOCDebug::enable(&Serial);
|
|
// Setup hardware serial
|
|
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
|
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
|
Serial.begin(115200);
|
|
// Init pins
|
|
pinMode(PC8, OUTPUT);
|
|
pinMode(PC9, OUTPUT);
|
|
pinMode(PA11, OUTPUT);
|
|
pinMode(PA12, OUTPUT);
|
|
pinMode(PA10, OUTPUT);
|
|
pinMode(PC6, OUTPUT);
|
|
pinMode(PA8, OUTPUT);
|
|
pinMode(PA9, OUTPUT);
|
|
pinMode(PA10, OUTPUT);
|
|
// end init pins
|
|
digitalWrite(PC8, HIGH);
|
|
digitalWrite(PC9, HIGH);
|
|
delay(1);
|
|
digitalWrite(PA11, HIGH);
|
|
digitalWrite(PA12, HIGH);
|
|
digitalWrite(PC6, HIGH);
|
|
|
|
sensor.init();
|
|
motor.linkSensor(&sensor);
|
|
|
|
driver.pwm_frequency = 20000;
|
|
driver.voltage_power_supply = 20;
|
|
driver.voltage_limit = 20;
|
|
driver.init();
|
|
motor.linkDriver(&driver);
|
|
current_sense.linkDriver(&driver);
|
|
motor.controller = MotionControlType::angle;
|
|
motor.torque_controller = TorqueControlType::foc_current;
|
|
motor.foc_modulation = FOCModulationType::SinePWM;
|
|
motor.voltage_limit = 20;
|
|
motor.velocity_limit = 20;
|
|
motor.useMonitoring(Serial);
|
|
motor.init();
|
|
current_sense.init();
|
|
motor.linkCurrentSense(¤t_sense);
|
|
motor.torque_controller = TorqueControlType::foc_current;
|
|
motor.controller = MotionControlType::angle;
|
|
motor.motion_downsample = 0.0;
|
|
motor.PID_velocity.P = 0.5;
|
|
motor.PID_velocity.I = 10.0;
|
|
motor.PID_velocity.D = 0.0;
|
|
motor.PID_velocity.output_ramp = 1000.0;
|
|
motor.PID_velocity.limit = 2.0;
|
|
motor.LPF_velocity.Tf = 0.005;
|
|
motor.P_angle.P = 0.5;
|
|
motor.P_angle.I = 10;
|
|
motor.P_angle.D = 1;
|
|
motor.P_angle.output_ramp = 0.0;
|
|
motor.P_angle.limit = 20.0;
|
|
motor.LPF_angle.Tf = 0.0;
|
|
motor.PID_current_q.P = 3.0;
|
|
motor.PID_current_q.I = 300.0;
|
|
motor.PID_current_q.D = 0.0;
|
|
motor.PID_current_q.output_ramp = 0.0;
|
|
motor.PID_current_q.limit = 12.0;
|
|
motor.LPF_current_q.Tf = 0.005;
|
|
motor.PID_current_d.P = 3.0;
|
|
motor.PID_current_d.I = 300.0;
|
|
motor.PID_current_d.D = 0.0;
|
|
motor.PID_current_d.output_ramp = 0.0;
|
|
motor.PID_current_d.limit = 12.0;
|
|
motor.LPF_current_d.Tf = 0.005;
|
|
motor.velocity_limit = 20.0;
|
|
motor.voltage_limit = 20.0;
|
|
motor.current_limit = 2.0;
|
|
motor.sensor_offset = 0.0;
|
|
motor.phase_resistance = 3.608;
|
|
motor.foc_modulation = FOCModulationType::SinePWM;
|
|
motor.modulation_centered = 1.0;
|
|
command.add('M', doMotor, "motor");
|
|
motor.useMonitoring(Serial);
|
|
_delay(1000);
|
|
}
|
|
|
|
void loop() {
|
|
motor.loopFOC();
|
|
motor.move();
|
|
motor.monitor();
|
|
command.run();
|
|
} |