robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

ADD: SimpleFOC naive + stable motor work

Browse source
This commit is contained in:
vanyabeat 2024-03-11 20:55:56 +03:00
parent 798b54aa91
commit 00d55a0b35
3 changed files with 82 additions and 70 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitlab-ci.yml Normal file
View file

@ -0,0 +1,2 @@
include:
- local: '/firmware/.gitlab-ci.yml'

18
firmware/.gitlab-ci.yml Normal file
View file

@ -0,0 +1,18 @@
image: python:3.9
stages:
- build
before_script:
- pip install -U platformio
- pio update
build:
stage: build
script:
- pio run
artifacts:
paths:
- .pio/build/robotroller/firmware.elf
- .pio/build/robotroller/firmware.bin
expire_in: 1 week

132
firmware/src/main.cpp
View file

@ -16,28 +16,40 @@
#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(12);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
InlineCurrentSense current_sense = InlineCurrentSense(0.51, 50, PB1, PB0, PC5);
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 onMotor(char *cmd) { command.motor(&motor, cmd); }
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);
@ -45,86 +57,66 @@ void setup() {
digitalWrite(PA12, HIGH);
digitalWrite(PC6, HIGH);
// arduino pin initialization INA180A current sensor
pinMode(CURRENT_SENSOR_1, OUTPUT);
pinMode(CURRENT_SENSOR_2, OUTPUT);
pinMode(CURRENT_SENSOR_3, OUTPUT);
analogReadResolution(12);
sensor.init();
motor.linkSensor(&sensor);
driver.pwm_frequency = 20000;
driver.voltage_power_supply = 12;
driver.voltage_limit = 12;
driver.voltage_power_supply = 20;
driver.voltage_limit = 20;
driver.init();
motor.linkDriver(&driver);
current_sense.linkDriver(&driver);
// init current sense
current_sense.init();
// link the motor to current sense
motor.linkCurrentSense(&current_sense);
// control loop type and torque mode
motor.controller = MotionControlType::angle;
motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::torque;
motor.motion_downsample = 0;
// velocity loop PID
motor.PID_velocity.P = 0;
motor.PID_velocity.I = 0;
motor.PID_velocity.D = 0;
motor.PID_velocity.output_ramp = 0;
motor.PID_velocity.limit = 0;
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0;
// angle loop PID
motor.P_angle.P = 0;
motor.P_angle.I = 0;
motor.P_angle.D = 0;
motor.P_angle.output_ramp = 0;
motor.P_angle.limit = 0;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0;
// current q loop PID
motor.PID_current_q.P = 0;
motor.PID_current_q.I = 0;
motor.PID_current_q.D = 0;
motor.PID_current_q.output_ramp = 0;
motor.PID_current_q.limit = 0;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 0;
// current d loop PID
motor.PID_current_d.P = 0;
motor.PID_current_d.I = 0;
motor.PID_current_d.D = 0;
motor.PID_current_d.output_ramp = 0;
motor.PID_current_d.limit = 0;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 0;
// Limits
motor.velocity_limit = 0;
motor.voltage_limit = 0;
motor.current_limit = 0;
// sensor zero offset - home position
motor.sensor_offset = 0;
// general settings
// motor phase resistance
motor.phase_resistance = 0;
// pwm modulation settings
motor.foc_modulation = FOCModulationType::SinePWM;
motor.modulation_centered = 1;
command.add('A', onMotor, "motor");
// tell the motor to use the monitoring
motor.voltage_limit = 20;
motor.velocity_limit = 20;
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // disable monitor at first - optional
motor.init();
motor.initFOC();
current_sense.init();
motor.linkCurrentSense(&current_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.monitor();
motor.move();
motor.monitor();
command.run();
}