From ad389db90fb98ad7a78741e6ac23faf85747bff4 Mon Sep 17 00:00:00 2001
From: "Hudson C. Dalpra" <hudson@bduncanltd.com>
Date: Thu, 8 Feb 2024 15:56:23 +1300
Subject: [PATCH] feat(motor): added voltage_control

---
 src/main.cpp | 82 +++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 55 insertions(+), 27 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index b3ca477..c27fbc5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,4 +1,12 @@
-// Open loop motor control example
+/**
+ *
+ * Torque control example using voltage control loop.
+ *
+ * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
+ * you a way to control motor torque by setting the voltage to the motor instead of the current.
+ *
+ * This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
+ */
 #include <SimpleFOC.h>
 
 #define POLE_PAIRS 7
@@ -7,56 +15,76 @@
 BLDCMotor motor = BLDCMotor(POLE_PAIRS);
 BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
 
-//target variable
-float target_velocity = 0;
+// hall sensor instance
+HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3, POLE_PAIRS);
 
+// Interrupt routine initialisation
+// channel A and B callbacks
+void doA(){sensor.handleA();}
+void doB(){sensor.handleB();}
+void doC(){sensor.handleC();}
+
+// voltage set point variable
+float target_voltage = 2;
 // instantiate the commander
 Commander command = Commander(Serial);
-void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
-void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
+void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
 
 void setup() {
         Serial.begin(115200);
         delay(2000);
 
+        // initialize hall sensor hardware
+        sensor.init();
+        sensor.enableInterrupts(doA, doB, doC);
+        // link the motor to the sensor
+        motor.linkSensor(&sensor);
+
         // driver config
         // power supply voltage [V]
         driver.voltage_power_supply = 12;
-        // limit the maximal dc voltage the driver can set
-        // as a protection measure for the low-resistance motors
-        // this value is fixed on startup
-        driver.voltage_limit = 6;
         driver.init();
-        // link the motor and the driver
+        // link driver
         motor.linkDriver(&driver);
 
-        // limiting motor movements
-        // limit the voltage to be set to the motor
-        // start very low for high resistance motors
-        // current = voltage / resistance, so try to be well under 1Amp
-        motor.voltage_limit = 3;   // [V]
 
-        // open loop control config
-        motor.controller = MotionControlType::velocity_openloop;
+        // aligning voltage
+        motor.voltage_sensor_align = 3;
+
+        // choose FOC modulation (optional)
+        motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+        // set motion control loop to be used
+        motor.controller = MotionControlType::torque;
 
-        // init motor hardware
-        motor.init();
         // use monitoring with serial
         motor.useMonitoring(Serial);
 
+        // initialize motor
+        motor.init();
+        // align sensor and start FOC
+        motor.initFOC();
+
         // add target command T
-        command.add('T', doTarget, "target velocity");
-        command.add('L', doLimit, "voltage limit");
+        command.add('T', doTarget, "target voltage");
 
-        Serial.println("Motor ready!");
-        Serial.println("Set target velocity [rad/s]");
+        Serial.println(F("Motor ready."));
+        Serial.println(F("Set the target voltage using serial terminal:"));
+        delay(1000);
 }
 
 void loop() {
-        // open loop velocity movement
-        // using motor.voltage_limit and motor.velocity_limit
-        // to turn the motor "backwards", just set a negative target_velocity
-        motor.move(target_velocity);
+        // main FOC algorithm function
+        // the faster you run this function the better
+        // Arduino UNO loop  ~1kHz
+        // Bluepill loop ~10kHz
+        motor.loopFOC();
+
+        // Motion control function
+        // velocity, position or voltage (defined in motor.controller)
+        // this function can be run at much lower frequency than loopFOC() function
+        // You can also use motor.move() and set the motor.target in the code
+        motor.move(target_voltage);
 
         // user communication
         command.run();