Add two linear halls sensor module.

examples/motion_control/torque_control/linear_hall_sensor/voltage_control/voltage_control.ino

@@ -0,0 +1,95 @@
+/**
+ * 
+ * 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>
+
+int16_t HALL_A_MIN = 98;
+int16_t HALL_A_MAX = 577;
+
+int16_t HALL_B_MIN = 80;
+int16_t HALL_B_MAX = 591;
+
+// BLDC motor & driver instance
+//BLDCMotor motor = BLDCMotor(11);
+//BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+BLDCMotor motor = BLDCMotor(4);
+BLDCDriver3PWM driver = BLDCDriver3PWM(5, 6, 10, 8);
+
+// Stepper motor & driver instance
+//StepperMotor motor = StepperMotor(50);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// Linear hall sensor instance
+LinearHallSensor sensor = LinearHallSensor(A4, A5);
+
+// 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() { 
+
+  /* use monitoring with serial */
+  Serial.begin(115200);
+  /* comment out if not needed */
+  motor.useMonitoring(Serial);
+
+  // initialize encoder sensor hardware
+  sensor.init(/* HALL_A_MIN, HALL_A_MAX, HALL_B_MIN, HALL_B_MAX*/);
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+
+  // 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;
+
+  // initialize motor
+  motor.init();
+  // 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();
+}

src/SimpleFOC.h

@@ -104,6 +104,7 @@ void loop() {
 #include "sensors/MagneticSensorAnalog.h"
 #include "sensors/MagneticSensorPWM.h"
 #include "sensors/HallSensor.h"
+#include "sensors/LinearHallSensor.h"
 #include "sensors/GenericSensor.h"
 #include "drivers/BLDCDriver3PWM.h"
 #include "drivers/BLDCDriver6PWM.h"

src/sensors/LinearHallSensor.cpp

@@ -0,0 +1,194 @@
+#include "LinearHallSensor.h"
+#include "./communication/SimpleFOCDebug.h"
+
+/** LinearHallSensor(uint8_t _pinAnalog, int _min, int _max)
+ * @param _pinAnalog  the pin that is reading the pwm from magnetic sensor
+ * @param _min_raw_count  the smallest expected reading.  Whilst you might expect it to be 0 it is often ~15.  Getting this wrong results in a small click once per revolution
+ * @param _max_raw_count  the largest value read.  whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096)
+ */
+LinearHallSensor::LinearHallSensor(uint8_t _pinAnalog_a, uint8_t _pinAnalog_b)
+{
+  pinAnalog_a = _pinAnalog_a;
+  pinAnalog_b = _pinAnalog_b;
+
+  if (pullup == Pullup::USE_INTERN)
+  {
+    pinMode(pinAnalog_a, INPUT_PULLUP);
+    pinMode(pinAnalog_b, INPUT_PULLUP);
+  }
+  else
+  {
+    pinMode(pinAnalog_a, INPUT);
+    pinMode(pinAnalog_b, INPUT);
+  }
+}
+
+/**
+ * @brief 线性传感器初始化
+ *
+ */
+void LinearHallSensor::init(int _min_raw_count_a, int _max_raw_count_a,
+                            int _min_raw_count_b, int _max_raw_count_b)
+{
+  raw_count_a = getRawCountA();
+  raw_count_b = getRawCountB();
+
+  if (_isset(_min_raw_count_a) && _isset(_max_raw_count_a) && _isset(_min_raw_count_b) && _isset(_max_raw_count_b))
+  {
+    /** LinearHall A */
+    min_raw_count_a = _min_raw_count_a;
+    max_raw_count_a = _max_raw_count_a;
+    /** LinearHall B */
+    min_raw_count_b = _min_raw_count_b;
+    max_raw_count_b = _max_raw_count_b;
+  }
+  else
+  {
+    /** 开始旋转电机一周校准线性霍尔的最大最小值 */
+    int minA, maxA, minB, maxB, centerA, centerB;
+
+    minA = raw_count_a;
+    maxA = minA;
+    centerA = (minA + maxA) / 2;
+    minB = raw_count_b;
+    maxB = minB;
+    centerB = (minB + maxB) / 2;
+
+    SIMPLEFOC_DEBUG("Sensor start init");
+
+    // move one electrical revolution forward
+    for (int i = 0; i <= 5000; i++)
+    {
+      // float angle = _3PI_2 + _2PI * i / 500.0f;
+      // BLDCMotor::setPhaseVoltage(FOCMotor::voltage_sensor_align, 0, angle);
+
+      //手动旋转电机一周来检测霍尔最大最小值
+      int tempA = getRawCountA();
+      if (tempA < minA)
+        minA = tempA;
+      if (tempA > maxA)
+        maxA = tempA;
+      centerA = (minA + maxA) / 2;
+
+      int tempB = getRawCountB();
+      if (tempB < minB)
+        minB = tempB;
+      if (tempB > maxB)
+        maxB = tempB;
+      centerB = (minB + maxB) / 2;
+
+      if (i % 500 == 0)
+      {
+        static int num = 10;
+        SIMPLEFOC_DEBUG("Count down: ", num);
+        SIMPLEFOC_DEBUG("A: ", centerA);
+        SIMPLEFOC_DEBUG("B: ", centerB);
+        SIMPLEFOC_DEBUG("min A: ", minA);
+        SIMPLEFOC_DEBUG("max A: ", maxA);
+        SIMPLEFOC_DEBUG("min B: ", minB);
+        SIMPLEFOC_DEBUG("max B: ", maxB);
+
+        num--;
+      }
+
+      _delay(2);
+    }
+
+    min_raw_count_a = minA;
+    max_raw_count_a = maxA;
+
+    min_raw_count_b = minB;
+    max_raw_count_b = maxB;
+
+    SIMPLEFOC_DEBUG("Sensor init ok:");
+    SIMPLEFOC_DEBUG("A: ", centerA);
+    SIMPLEFOC_DEBUG("B: ", centerB);
+    SIMPLEFOC_DEBUG("min A: ", minA);
+    SIMPLEFOC_DEBUG("max A: ", maxA);
+    SIMPLEFOC_DEBUG("min B: ", minB);
+    SIMPLEFOC_DEBUG("max B: ", maxB);
+  }
+
+  cpr = max_raw_count_a - min_raw_count_a;
+
+  this->Sensor::init(); // call base class init
+}
+
+/**
+ * @brief Shaft angle calculation
+ *
+ * @return angle is in radians [rad] float [0,2PI]
+ */
+float LinearHallSensor::getSensorAngle()
+{
+  /* 1.获取双霍尔原始模拟值 */
+  raw_count_a = getRawCountA();
+  raw_count_b = getRawCountB();
+
+  /* 2.双霍尔模拟值滤波 */
+
+  /* 3.双霍尔滤波后值映射到[-1,1] */
+  this->hall_filtered_a = map_float(raw_count_a, min_raw_count_a, max_raw_count_a, -1, 1);
+  this->hall_filtered_b = map_float(raw_count_b, min_raw_count_b, max_raw_count_b, -1, 1);
+
+  /* 4.反正切函数计算轴角度[-180,180] */
+  float angle = atan2(hall_filtered_a, hall_filtered_b) * 180 / PI;
+
+  /* 5.轴角转为弧度制[0,2PI] */
+  return (angle + 180) * PI / 180.0f;
+}
+
+/**
+ * @brief Reading the raw counter of the linear hall sensor A
+ *
+ * @return int
+ */
+int LinearHallSensor::getRawCountA()
+{
+  return analogRead(pinAnalog_a);
+}
+
+/**
+ * @brief Reading the raw counter of the linear hall sensor B
+ *
+ * @return int
+ */
+int LinearHallSensor::getRawCountB()
+{
+  return analogRead(pinAnalog_b);
+}
+
+/**
+ * @brief 数据映射 float
+ *
+ * @param in_x    输入值
+ * @param in_min  输入值的最小幅值
+ * @param in_max  输入值的最大幅值
+ * @param out_min 输出值的最小幅值
+ * @param out_max 输出值的最大幅值
+ * @return float  映射后的输出值
+ */
+float LinearHallSensor::map_float(float in_x, float in_min, float in_max, float out_min, float out_max)
+{
+  return (in_x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+/**
+ * @brief 获取线性霍尔A滤波后的值
+ *
+ * @return float [-1,1]
+ */
+float LinearHallSensor::getHallFiledA()
+{
+  return this->hall_filtered_a;
+}
+
+/**
+ * @brief 获取线性霍尔B滤波后的值
+ *
+ * @return float [-1,1]
+ */
+float LinearHallSensor::getHallFiledB()
+{
+  return this->hall_filtered_b;
+}

src/sensors/LinearHallSensor.h

@@ -0,0 +1,69 @@
+#ifndef LINEARHALLSENSOR_LIB_H
+#define LINEARHALLSENSOR_LIB_H
+
+#include "Arduino.h"
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+/**
+ * This sensor has LinearHallSensor.
+ * This approach is very simple.
+ */
+class LinearHallSensor : public Sensor
+{
+public:
+  /**
+   * LinearHallSensor class constructor
+   * @param _pinAnalog_a  the pin to read the HALL A signal
+   * @param _pinAnalog_b  the pin to read the HALL B signal
+   */
+  LinearHallSensor(uint8_t _pinAnalog_a, uint8_t _pinAnalog_b);
+
+  /** sensor initialise pins */
+  void init(int _min_raw_count_a = NOT_SET, int _max_raw_count_a = NOT_SET,
+            int _min_raw_count_b = NOT_SET, int _max_raw_count_b = NOT_SET);
+
+  uint8_t pinAnalog_a; //!< Linear Hall pin A
+  uint8_t pinAnalog_b; //!< Linear Hall pin B
+
+  /** Encoder configuration */
+  Pullup pullup;
+
+  /** implementation of abstract functions of the Sensor class */
+  /** get current angle (rad) */
+  float getSensorAngle() override;
+  /** raw count (typically in range of 0-1023), useful for debugging resolution issues */
+  int raw_count_a;
+  int raw_count_b;
+
+  /** 获取霍尔滤波后的值[-1,1] */
+  float getHallFiledA();
+  float getHallFiledB();
+
+  /** 数据映射 */
+  float map_float(float in_x, float in_min, float in_max, float out_min, float out_max);
+
+private:
+  /** LinearHall 实际输出模拟值范围 */
+  int min_raw_count_a;
+  int max_raw_count_a;
+  int min_raw_count_b;
+  int max_raw_count_b;
+
+  /** LinearHall 滤波后值映射到[-1,1] */
+  float hall_filtered_a;
+  float hall_filtered_b;
+
+  int cpr;
+  int read();
+
+  /**
+   * Function getting current angle register value
+   * it uses angle_register variable
+   */
+  int getRawCountA();
+  int getRawCountB();
+};
+
+#endif