add endstop detection

src/linearHallSensor.cpp

@@ -5,12 +5,10 @@ float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_
     return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
 }
 
-LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos)
+LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
 {
     _analogPin1 = ch1;
     _analogPin2 = ch2;
-    _minPositionEndValue = minPos;
-    _maxPositionEndValue = maxPos;
 }
 
 void LinearHallSensor::init(BLDCMotor motor)
@@ -22,68 +20,129 @@ void LinearHallSensor::init(BLDCMotor motor)
 
     uint32_t senseA = analogRead(_analogPin1);
     uint32_t senseB = analogRead(_analogPin2);
+    uint32_t oldSenseA = 0;
+    uint32_t oldSenseB = 0;
 
     _minCh1 = senseA;
     _maxCh1 = senseA;
     _minCh2 = senseB;
     _maxCh2 = senseB;
 
-    // Swipe motor
+    // Swipe motor to search hard end and find max analog values of sensors
-    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
+    bool endFound = false;
-    {
+    const float step = 0.0025;
-        motor.move(i);
+    uint8_t currentCheck = 0; // current check number
-        delay(1);
+    const uint8_t nCheck = 15; // number of times to check if its the same value
-        senseA = analogRead(_analogPin1);
+    const uint8_t epsilon = 3;
-        senseB = analogRead(_analogPin2);
+    float currentPosition = 0.0;
-        if (senseA > _maxCh1)
-            _maxCh1 = senseA;
-        else if (senseA < _minCh1)
-            _minCh1 = senseA;
-        if (senseB > _maxCh1)
-            _maxCh1 = senseB;
-        else if (senseB < _minCh2)
-            _minCh2 = senseB;
-    }
-    delay(100);
-    for (float i = _maxPositionEndValue; i >= _minPositionEndValue; i = i - 0.005)
-    {
-        motor.move(i);
-        delay(1);
-        senseA = analogRead(_analogPin1);
-        senseB = analogRead(_analogPin2);
-        if (senseA > _maxCh1)
-            _maxCh1 = senseA;
-        else if (senseA < _minCh1)
-            _minCh1 = senseA;
-        if (senseB > _maxCh1)
-            _maxCh1 = senseB;
-        else if (senseB < _minCh2)
-            _minCh2 = senseB;
-    }
 
-    // Min offset with 50 values average
+    Serial.println("\tFinding end stops");
-    float sum = 0.0;
+    while (!endFound)
-    for (uint8_t i = 0; i <= 49; i++)
     {
-        _offset = 0.0;
+        Serial.print("Position =" + String(currentPosition) + '\t');
-        sum += readSensorCallback();
+        Serial.print("SenseA =" + String(senseA) + "\tSenseB =" + String(senseB));
+        Serial.print("\tOldseA =" + String(oldSenseA) + "\tOldseB =" + String(oldSenseB) + "\tchecked =" + String(currentCheck) + "\r");
+        senseA = analogRead(_analogPin1);
+        senseB = analogRead(_analogPin2);
+
+        // Check if new extremes sensor values
+        if (senseA > _maxCh1)
+            _maxCh1 = senseA;
+        else if (senseA < _minCh1)
+            _minCh1 = senseA;
+        if (senseB > _maxCh1)
+            _maxCh1 = senseB;
+        else if (senseB < _minCh2)
+            _minCh2 = senseB;
+
+        // Compare with previous position
+        if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon))
+        {
+            if (currentCheck == nCheck)
+                endFound = true;
+            else
+                currentCheck++;
+        }
+        else
+            currentCheck = 0;
+
+        // Replace previous values
+        oldSenseA = senseA;
+        oldSenseB = senseB;
+
+        // Move to new position
+        currentPosition += step;
+        motor.move(currentPosition);
+        delay(2);
     }
+    _maxPositionEndValue = currentPosition - (step * nCheck);
+    currentPosition = _maxPositionEndValue;
+    motor.move(currentPosition);
+    Serial.println("Found first end stop : Max position = " + String(_maxPositionEndValue));
+    delay(100);
+
+    // Swipe motor to search other hard end and find eventually new max analog values of sensors
+    endFound = false;
+    currentCheck = 0;
+    while (!endFound)
+    {
+        senseA = analogRead(_analogPin1);
+        senseB = analogRead(_analogPin2);
+
+        // Check if new extremes sensor values
+        if (senseA > _maxCh1)
+            _maxCh1 = senseA;
+        else if (senseA < _minCh1)
+            _minCh1 = senseA;
+        if (senseB > _maxCh1)
+            _maxCh1 = senseB;
+        else if (senseB < _minCh2)
+            _minCh2 = senseB;
+
+        // Compare with previous position
+        if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon))
+        {
+            if (currentCheck == nCheck)
+                endFound = true;
+            else
+                currentCheck++;
+        }
+        else
+            currentCheck = 0;
+
+        // Replace previous values
+        oldSenseA = senseA;
+        oldSenseB = senseB;
+
+        // Move to new position
+        currentPosition -= step;
+        motor.move(currentPosition);
+        delay(2);
+    }
+    _minPositionEndValue = currentPosition + (step * nCheck);
+    Serial.println("Found second end stop : Min position = " + String(_minPositionEndValue));
+    delay(100);
+
+    // Set min offset with 50 values average
+    float sum = 0.0;
+    _offset = 0.0;
+    for (uint8_t i = 0; i <= 49; i++)
+        sum += readSensorCallback();
     _offset = -sum / 50.0;
 
     Serial.println("\n\rmaxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
     Serial.println("Offset: " + String(_offset));
 
-    // Detect max angle
+    // Read max angle
-    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01)
+    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
     {
         motor.move(i);
         delay(1);
         _maxSensor = readSensorCallback();
     }
-    Serial.println("current motor angle: " + String(motor.electricalAngle()));
     delay(250);
 
-    // take average value of 50 values
+    // Set max take average value of 50 values
     sum = 0.0;
     for (uint8_t i = 0; i <= 49; i++)
         sum += readSensorCallback();
@@ -92,7 +151,7 @@ void LinearHallSensor::init(BLDCMotor motor)
     Serial.println("Max sensor: " + String(_maxSensor));
 
     // Recovering previous controller type and go in the middle
-    for (float i = _maxPositionEndValue; i >= _minPositionEndValue / 2.0; i = i - 0.01)
+    for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue) / 2.0; i = i - 0.01)
     {
         motor.move(i);
         delay(1);

src/linearHallSensor.h

@@ -15,10 +15,8 @@ class LinearHallSensor
          Linear Hall Sensor class constructor
         @param ch1 Analog pin for channel 1
         @param ch2 Analog pin for channel 2
-        @param minPos Minimum position in Openloop
-        @param maxPos Maximum position in Openloop
         */ 
-        LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos);
+        LinearHallSensor(uint8_t ch1, uint8_t ch2);
 
 
         /**

src/main.cpp

@@ -3,22 +3,20 @@
 #include <linearHallSensor.h>
 #include <pinout.h>
 
-BLDCMotor motor = BLDCMotor(4);
+BLDCMotor motor1 = BLDCMotor(4);
-BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3);
+BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
+LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);
 
-LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35);
+void initSensor1()
-
-void initSensor()
 {
-    linearSensor.init(motor);
+    linearSensor1.init(motor1);
+}
+float callback1()
+{
+    return linearSensor1.readSensorCallback();
 }
 
-float callback()
+GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
-{
-    return linearSensor.readSensorCallback();
-}
-
-GenericSensor sensor = GenericSensor(callback, initSensor);
 
 float targetX = 0.0;
 float targetY = 0.0;
@@ -51,68 +49,45 @@ void setup()
 {
 
     Serial.begin(115200);
-    delay(2000);
+    delay(5000);
     Serial.println("INIT");
 
-    // pinMode(M1_PWM1, OUTPUT);
+    pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
-    // pinMode(M1_PWM2, OUTPUT);
+    digitalWrite(LED_BUILTIN, LOW);
-    // pinMode(M1_PWM3, OUTPUT);
-    // digitalWrite(M1_PWM1, LOW);
-    // digitalWrite(M1_PWM2, LOW);
-    // digitalWrite(M1_PWM3, LOW);
 
-    // pinMode(M2_PWM1, OUTPUT);
+    driver1.voltage_power_supply = 7.1;
-    // pinMode(M2_PWM2, OUTPUT);
+    driver1.init();
-    // pinMode(M2_PWM3, OUTPUT);
+    motor1.linkDriver(&driver1);
-    // digitalWrite(M2_PWM1, LOW);
+    motor1.useMonitoring(Serial);
-    // digitalWrite(M2_PWM2, LOW);
+    motor1.controller = MotionControlType::angle;
-    // digitalWrite(M2_PWM3, LOW);
+    motor1.foc_modulation = FOCModulationType::SinePWM;
-
+    motor1.voltage_limit = 1.0;
-    // pinMode(M3_PWM1, OUTPUT);
+    motor1.voltage_sensor_align = 1.0;
-    // pinMode(M3_PWM2, OUTPUT);
+    motor1.PID_velocity.P = 0.05f;
-    // pinMode(M3_PWM3, OUTPUT);
+    motor1.PID_velocity.I = 0.01;
-    // digitalWrite(M3_PWM1, LOW);
+    motor1.PID_velocity.D = 0.0;
-    // digitalWrite(M3_PWM2, LOW);
+    motor1.LPF_velocity.Tf = 0.01f;
-    // digitalWrite(M3_PWM3, LOW);
+    motor1.P_angle.P = 150.0;
-
+    motor1.P_angle.I = 10.0;
-    pinMode(LED_BUILTIN, OUTPUT);       // Lightup LED
+    motor1.velocity_limit = 25;
-    digitalWrite(LED_BUILTIN, HIGH);
-
-    driver.voltage_power_supply = 5.0;
-    driver.init();
-    motor.linkDriver(&driver);
-    motor.useMonitoring(Serial);
-    motor.controller = MotionControlType::angle;
-    motor.foc_modulation = FOCModulationType::SinePWM;
-    motor.voltage_limit = 1.5;
-    motor.voltage_sensor_align = 0.8;
-    motor.PID_velocity.P = 0.075f;
-    motor.PID_velocity.I = 0.01;
-    motor.PID_velocity.D = 0.0;
-    motor.LPF_velocity.Tf = 0.01f;
-    motor.P_angle.P = 120.0;
-    motor.P_angle.I = 10.0;
-    motor.velocity_limit = 50;
-
-    motor.useMonitoring(Serial);
 
     // Init sensor
-    motor.init();
+    motor1.init();
     Serial.println("calibrating sensor in open loop...");
-    sensor.init();
+    sensor1.init();
     Serial.println("Done");
     delay(1000);
 
-    motor.linkSensor(&sensor);
+    motor1.linkSensor(&sensor1);
-    motor.init();
+    motor1.init();
     // motor.initFOC(5.48, CCW);
-    motor.initFOC();
+    motor1.initFOC();
 }
 
 void loop()
 {
     serialLoop();
-    motor.move(target);
+    motor1.move(target);
-    motor.loopFOC();
+    motor1.loopFOC();
-    motor.monitor();
+    motor1.monitor();
 }