From 241e8c04dbd4ee8ea3476b166d6e0684b1cd08e1 Mon Sep 17 00:00:00 2001
From: dekutree64 <54822734+dekutree64@users.noreply.github.com>
Date: Sat, 13 Jan 2024 20:25:13 -0600
Subject: [PATCH] LinearHall improvements and move to main repository

Changes compared to the original pull request in the drivers repository https://github.com/simplefoc/Arduino-FOC-drivers/pull/12
1. Added a version of init which turns the motor one revolution to find the center values of the sensors.
2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance.
3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update.
4. Changed to use the much faster _atan2 function that was added to foc_utils recently.
5. Added examples.
---
 .../angle_control/angle_control.ino           | 117 ++++++++++++++++++
 .../voltage_control/voltage_control.ino       |  96 ++++++++++++++
 .../velocity_control/velocity_control.ino     | 109 ++++++++++++++++
 src/SimpleFOC.h                               |   1 +
 src/sensors/LinearHall.cpp                    | 109 ++++++++++++++++
 src/sensors/LinearHall.h                      |  45 +++++++
 6 files changed, 477 insertions(+)
 create mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino
 create mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino
 create mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino
 create mode 100644 src/sensors/LinearHall.cpp
 create mode 100644 src/sensors/LinearHall.h

diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino
new file mode 100644
index 00000000..8179008d
--- /dev/null
+++ b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino
@@ -0,0 +1,117 @@
+/**
+ *
+ * Position/angle motion control example
+ * Steps:
+ * 1) Configure the motor and hall sensor
+ * 2) Run the code
+ * 3) Set the target angle (in radians) from serial terminal
+ */
+#include <SimpleFOC.h>
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+// Stepper motor & driver instance
+//StepperMotor motor = StepperMotor(50);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// hall sensor instance
+LinearHall sensor = LinearHall(A0, A1, 11);
+
+// angle set point variable
+float target_angle = 0;
+// instantiate the commander
+Commander command = Commander(Serial);
+void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
+
+void setup() {
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link the motor and the driver
+  motor.linkDriver(&driver);
+
+
+  // aligning voltage [V]
+  motor.voltage_sensor_align = 3;
+  // index search velocity [rad/s]
+  motor.velocity_index_search = 3;
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::angle;
+
+  // contoller configuration
+  // default parameters in defaults.h
+
+  // velocity PI controller parameters
+  motor.PID_velocity.P = 0.2f;
+  motor.PID_velocity.I = 2;
+  motor.PID_velocity.D = 0;
+  // default voltage_power_supply
+  motor.voltage_limit = 6;
+  // jerk control using voltage voltage ramp
+  // default value is 300 volts per sec  ~ 0.3V per millisecond
+  motor.PID_velocity.output_ramp = 1000;
+
+  // velocity low pass filtering time constant
+  motor.LPF_velocity.Tf = 0.01f;
+
+  // angle P controller
+  motor.P_angle.P = 20;
+  //  maximal velocity of the position control
+  motor.velocity_limit = 4;
+
+
+  // use monitoring with serial
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // initialize sensor hardware. This moves the motor to find the min/max sensor readings and 
+  // averages them to get the center values. The motor can't move until motor.init is called, and 
+  // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween.
+  // You can then take the values printed to the serial monitor and pass them to sensor.init to 
+  // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init 
+  // is called before or after motor.init.
+  sensor.init(&motor);
+  Serial.print("LinearHall centerA: ");
+  Serial.print(sensor.centerA);
+  Serial.print(", centerB: ");
+  Serial.println(sensor.centerB);
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+  // align sensor and start FOC
+  motor.initFOC();
+
+  // add target command T
+  command.add('T', doTarget, "target angle");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target angle using serial terminal:"));
+  _delay(1000);
+}
+
+void loop() {
+  // 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_angle);
+
+  // function intended to be used with serial plotter to monitor motor variables
+  // significantly slowing the execution down!!!!
+  // motor.monitor();
+
+  // user communication
+  command.run();
+}
diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino
new file mode 100644
index 00000000..e646849a
--- /dev/null
+++ b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino
@@ -0,0 +1,96 @@
+/**
+ * 
+ * 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>
+
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+// Stepper motor & driver instance
+//StepperMotor motor = StepperMotor(50);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// hall sensor instance
+LinearHall sensor = LinearHall(A0, A1, 11);
+
+
+// voltage set point variable
+float target_voltage = 2;
+// instantiate the commander
+Commander command = Commander(Serial);
+void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
+
+void setup() { 
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link driver
+  motor.linkDriver(&driver);
+
+  // 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;
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // initialize sensor hardware. This moves the motor to find the min/max sensor readings and 
+  // averages them to get the center values. The motor can't move until motor.init is called, and 
+  // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween.
+  // You can then take the values printed to the serial monitor and pass them to sensor.init to 
+  // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init 
+  // is called before or after motor.init.
+  sensor.init(&motor);
+  Serial.print("LinearHall centerA: ");
+  Serial.print(sensor.centerA);
+  Serial.print(", centerB: ");
+  Serial.println(sensor.centerB);
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+  // align sensor and start FOC
+  motor.initFOC();
+
+  // add target command T
+  command.add('T', doTarget, "target voltage");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target voltage using serial terminal:"));
+  _delay(1000);
+}
+
+
+void loop() {
+
+  // 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();
+}
\ No newline at end of file
diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino
new file mode 100644
index 00000000..870a0899
--- /dev/null
+++ b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino
@@ -0,0 +1,109 @@
+/**
+ *
+ * Velocity motion control example
+ * Steps:
+ * 1) Configure the motor and sensor
+ * 2) Run the code
+ * 3) Set the target velocity (in radians per second) from serial terminal
+ */
+#include <SimpleFOC.h>
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+// Stepper motor & driver instance
+//StepperMotor motor = StepperMotor(50);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// hall sensor instance
+LinearHall sensor = LinearHall(A0, A1, 11);
+
+// velocity set point variable
+float target_velocity = 0;
+// instantiate the commander
+Commander command = Commander(Serial);
+void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
+
+void setup() {
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link the motor and the driver
+  motor.linkDriver(&driver);
+
+  // aligning voltage [V]
+  motor.voltage_sensor_align = 3;
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::velocity;
+
+  // contoller configuration
+  // default parameters in defaults.h
+
+  // velocity PI controller parameters
+  motor.PID_velocity.P = 0.2f;
+  motor.PID_velocity.I = 2;
+  motor.PID_velocity.D = 0;
+  // default voltage_power_supply
+  motor.voltage_limit = 6;
+  // jerk control using voltage voltage ramp
+  // default value is 300 volts per sec  ~ 0.3V per millisecond
+  motor.PID_velocity.output_ramp = 1000;
+
+  // velocity low pass filtering time constant
+  motor.LPF_velocity.Tf = 0.01f;
+
+  // use monitoring with serial
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // initialize sensor hardware. This moves the motor to find the min/max sensor readings and 
+  // averages them to get the center values. The motor can't move until motor.init is called, and 
+  // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween.
+  // You can then take the values printed to the serial monitor and pass them to sensor.init to 
+  // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init 
+  // is called before or after motor.init.
+  sensor.init(&motor);
+  Serial.print("LinearHall centerA: ");
+  Serial.print(sensor.centerA);
+  Serial.print(", centerB: ");
+  Serial.println(sensor.centerB);
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+  // align sensor and start FOC
+  motor.initFOC();
+
+  // add target command T
+  command.add('T', doTarget, "target voltage");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target velocity using serial terminal:"));
+  _delay(1000);
+}
+
+
+void loop() {
+  // 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_velocity);
+
+  // function intended to be used with serial plotter to monitor motor variables
+  // significantly slowing the execution down!!!!
+  // motor.monitor();
+
+  // user communication
+  command.run();
+}
\ No newline at end of file
diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h
index 4e6815e5..9e1c80c6 100644
--- a/src/SimpleFOC.h
+++ b/src/SimpleFOC.h
@@ -104,6 +104,7 @@ void loop() {
 #include "sensors/MagneticSensorAnalog.h"
 #include "sensors/MagneticSensorPWM.h"
 #include "sensors/HallSensor.h"
+#include "sensors/LinearHall.h"
 #include "sensors/GenericSensor.h"
 #include "drivers/BLDCDriver3PWM.h"
 #include "drivers/BLDCDriver6PWM.h"
diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp
new file mode 100644
index 00000000..36303aaa
--- /dev/null
+++ b/src/sensors/LinearHall.cpp
@@ -0,0 +1,109 @@
+#include "LinearHall.h"
+
+// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
+__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b)
+{
+	*a = analogRead(hallA);
+	*b = analogRead(hallB);
+}
+
+LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
+  centerA = 512;
+  centerB = 512;
+  pinA = _hallA;
+  pinB = _hallB;
+  pp = _pp;
+}
+
+float LinearHall::getSensorAngle() {
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  //offset readings using center values, then compute angle
+  float reading = _atan2(lastA - centerA, lastB - centerB);
+
+  //handle rollover logic between each electrical revolution of the motor
+  if (reading > prev_reading) {
+    if (reading - prev_reading >= PI) {
+      if (electrical_rev - 1 < 0) {
+        electrical_rev = pp - 1;
+      } else {
+        electrical_rev = electrical_rev - 1;
+      }
+    }
+  } else if (reading < prev_reading) {
+    if (prev_reading - reading >= PI) {
+      if (electrical_rev + 1 >= pp) {
+        electrical_rev = 0;
+      } else {
+        electrical_rev = electrical_rev + 1;
+      }
+    }
+  }
+
+  //convert result from electrical angle and electrical revolution count to shaft angle in radians
+  float result = (reading + PI) / _2PI;
+  result = _2PI * (result + electrical_rev) / pp;
+
+  //update previous reading for rollover handling
+  prev_reading = reading;
+  return result;
+}
+
+void LinearHall::init(int _centerA, int _centerB) {
+  // Skip configuring the pins here because they normally default to input anyway, and
+  // this makes it possible to use ADC channel numbers instead of pin numbers when using
+  // custom implementation of ReadLinearHalls, to avoid having to remap them every update.
+  // If pins do need to be configured, it can be done by user code before calling init.
+  //pinMode(pinA, INPUT);
+  //pinMode(pinB, INPUT);
+
+  centerA = _centerA;
+  centerB = _centerB;
+
+  //establish initial reading for rollover handling
+  electrical_rev = 0;
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  prev_reading = _atan2(lastA - centerA, lastB - centerB);
+}
+
+void LinearHall::init(FOCMotor *motor) {
+  if (!motor->enabled) {
+    SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC.");
+    return;
+  }
+
+  //pinMode(pinA, INPUT);
+  //pinMode(pinB, INPUT);
+
+  int minA, maxA, minB, maxB;
+
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  minA = maxA = centerA = lastA;
+  minB = maxB = centerB = lastB;
+
+  // move one mechanical revolution forward
+  for (int i = 0; i <= 2000; i++)
+  {
+    float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
+    motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);
+
+    ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+
+    if (lastA < minA)
+      minA = lastA;
+    if (lastA > maxA)
+      maxA = lastA;
+    centerA = (minA + maxA) / 2;
+
+    if (lastB < minB)
+      minB = lastB;
+    if (lastB > maxB)
+      maxB = lastB;
+    centerB = (minB + maxB) / 2;
+
+    _delay(2);
+  }
+
+  //establish initial reading for rollover handling
+  electrical_rev = 0;
+  prev_reading = _atan2(lastA - centerA, lastB - centerB);
+}
diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h
new file mode 100644
index 00000000..f05b7b2b
--- /dev/null
+++ b/src/sensors/LinearHall.h
@@ -0,0 +1,45 @@
+#ifndef LINEAR_HALL_SENSOR_LIB_H
+#define LINEAR_HALL_SENSOR_LIB_H
+
+#include <SimpleFOC.h>
+
+class FOCMotor;
+void ReadLinearHalls(int hallA, int hallB, int *a, int *b);
+
+/**
+ * This sensor class is for two linear hall effect sensors such as 49E, which are
+ * positioned 90 electrical degrees apart (if one is centered on a rotor magnet,
+ * the other is half way between rotor magnets).
+ * It can also be used for a single magnet mounted to the motor shaft (set pp to 1).
+ *
+ * For more information, see this forum thread and PDF
+ * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959
+ * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1
+ */
+class LinearHall: public Sensor{
+  public:
+    LinearHall(int hallA, int hallB, int pp);
+
+    void init(int centerA, int centerB); // Initialize without moving motor
+    void init(FOCMotor *motor); // Move motor to find center values
+
+    // Get current shaft angle from the sensor hardware, and
+    // return it as a float in radians, in the range 0 to 2PI.
+    //  - This method is pure virtual and must be implemented in subclasses.
+    //    Calling this method directly does not update the base-class internal fields.
+    //    Use update() when calling from outside code.
+    float getSensorAngle() override;
+
+    int centerA;
+    int centerB;
+    int lastA, lastB;
+
+  private:
+    int pinA;
+    int pinB;
+    int pp;
+    int electrical_rev;
+    float prev_reading;
+};
+
+#endif