ADD: SimpleFOC naive + stable motor work

.gitlab-ci.yml

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

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

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);
+BLDCMotor motor = BLDCMotor(16);
-BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
+// Init 3PWM driver DRV8313
-InlineCurrentSense current_sense = InlineCurrentSense(0.51, 50, PB1, PB0, PC5);
+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_power_supply = 20;
-  driver.voltage_limit = 12;
+  driver.voltage_limit = 20;
   driver.init();
   motor.linkDriver(&driver);
-
   current_sense.linkDriver(&driver);
-  // init current sense
+  motor.controller = MotionControlType::angle;
-  current_sense.init();
-  // link the motor to current sense
-  motor.linkCurrentSense(&current_sense);
-  // control loop type and torque mode
   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;
+  motor.voltage_limit = 20;
-  command.add('A', onMotor, "motor");
+  motor.velocity_limit = 20;
-  // tell the motor to use the monitoring
   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();
 }