From 00d55a0b354901b5602d5f24935cc89995eba540 Mon Sep 17 00:00:00 2001 From: vanyabeat Date: Mon, 11 Mar 2024 20:55:56 +0300 Subject: [PATCH] ADD: SimpleFOC naive + stable motor work --- .gitlab-ci.yml | 2 + firmware/.gitlab-ci.yml | 18 ++++++ firmware/src/main.cpp | 132 +++++++++++++++++++--------------------- 3 files changed, 82 insertions(+), 70 deletions(-) create mode 100644 .gitlab-ci.yml create mode 100644 firmware/.gitlab-ci.yml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..0f0867b --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,2 @@ +include: + - local: '/firmware/.gitlab-ci.yml' \ No newline at end of file diff --git a/firmware/.gitlab-ci.yml b/firmware/.gitlab-ci.yml new file mode 100644 index 0000000..35d3b2b --- /dev/null +++ b/firmware/.gitlab-ci.yml @@ -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 diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index e1969f8..f8544d4 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -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(¤t_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(¤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.monitor(); motor.move(); + motor.monitor(); command.run(); } \ No newline at end of file