From c3022bbe5f6c42382170b1edfa5937bf2087d7ca Mon Sep 17 00:00:00 2001
From: Lucien RENAUD <lucien.renaud@inria.fr>
Date: Thu, 6 Oct 2022 17:33:11 +0200
Subject: [PATCH] 3 motors working

---
 src/main.cpp | 61 ++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 54 insertions(+), 7 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index f82fb70..d1a1cfb 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,12 +4,16 @@
 #include <pinout.h>
 
 BLDCMotor motor1 = BLDCMotor(4);
-BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
-LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);
+BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3);
+LinearHallSensor linearSensor1 = LinearHallSensor(M1_Hall1, M1_Hall2);
 
 BLDCMotor motor2 = BLDCMotor(4);
-BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_PWM2, M1_PWM1, M1_PWM3);
-LinearHallSensor linearSensor2 = LinearHallSensor(M1_Hall1, M1_Hall2);
+BLDCDriver3PWM driver2 = BLDCDriver3PWM(M2_PWM2, M2_PWM1, M2_PWM3);
+LinearHallSensor linearSensor2 = LinearHallSensor(M2_Hall1, M2_Hall2);
+
+BLDCMotor motor3 = BLDCMotor(4);
+BLDCDriver3PWM driver3 = BLDCDriver3PWM(M3_PWM2, M3_PWM1, M3_PWM3);
+LinearHallSensor linearSensor3 = LinearHallSensor(M3_Hall1, M3_Hall2);
 
 void initSensor1()
 {
@@ -29,8 +33,18 @@ float callback2()
     return linearSensor2.readSensorCallback();
 }
 
+void initSensor3()
+{
+    linearSensor3.init(motor3);
+}
+float callback3()
+{
+    return linearSensor3.readSensorCallback();
+}
+
 GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
 GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
+GenericSensor sensor3 = GenericSensor(callback3, initSensor3);
 
 float targetX = 0.0;
 float targetY = 0.0;
@@ -73,7 +87,6 @@ void setup()
     Serial.println("\n\t\t### MOTOR 1 ###\n");
     driver1.voltage_power_supply = 7.0;
     Serial.println("Driver1 init: " + String(driver1.init()));
-    Serial.println("Driver2 init: " + String(driver2.init()));
     motor1.linkDriver(&driver1);
     motor1.useMonitoring(Serial);
     motor1.controller = MotionControlType::angle;
@@ -100,7 +113,6 @@ void setup()
     motor1.initFOC(2.98, CW);
     // motor1.initFOC();
     Serial.println("Motor 1 Done");
-    motor1.disable();
 
     delay(1000);
 
@@ -108,7 +120,7 @@ void setup()
     Serial.println("\n\t\t### MOTOR 2 ###\n");
     motor2.useMonitoring(Serial);
     driver2.voltage_power_supply = 7.0;
-    // Serial.println("Driver init: " + String(driver2.init()));
+    Serial.println("Driver init: " + String(driver2.init()));
     motor2.linkDriver(&driver2);
 
     motor2.controller = MotionControlType::angle;
@@ -135,6 +147,41 @@ void setup()
     // motor.initFOC(5.48, CCW);
     motor2.initFOC();
     Serial.println("Motor 2 Done");
+
+        delay(1000);
+
+    // ### MOTOR 3 ###
+    Serial.println("\n\t\t### MOTOR 3 ###\n");
+    motor3.useMonitoring(Serial);
+    driver3.voltage_power_supply = 7.0;
+    Serial.println("Driver init: " + String(driver3.init()));
+    motor3.linkDriver(&driver3);
+
+    motor3.controller = MotionControlType::angle;
+    motor3.foc_modulation = FOCModulationType::SinePWM;
+    motor3.voltage_limit = 1.0;
+    motor3.voltage_sensor_align = 1.0;
+    motor3.PID_velocity.P = 0.05f;
+    motor3.PID_velocity.I = 0.01;
+    motor3.PID_velocity.D = 0.0;
+    motor3.LPF_velocity.Tf = 0.01f;
+    motor3.P_angle.P = 150.0;
+    motor3.P_angle.I = 10.0;
+    motor3.velocity_limit = 25;
+
+    // Init sensor 2
+    motor3.init();
+    Serial.println("calibrating sensor 2 in open loop...");
+    sensor3.init();
+    Serial.println("Sensor 2 done");
+    delay(1000);
+
+    motor3.linkSensor(&sensor3);
+    motor3.init();
+    // motor.initFOC(5.48, CCW);
+    motor3.initFOC();
+    Serial.println("Motor 3 Done");
+    while(1);
 }
 
 void loop()