From 526ffacd4f334ef6f926d67d0c0e1e33b245afaa Mon Sep 17 00:00:00 2001 From: Lucien Renaud Date: Tue, 30 Aug 2022 11:21:03 +0200 Subject: [PATCH] add endstop detection --- src/linearHallSensor.cpp | 149 +++++++++++++++++++++++++++------------ src/linearHallSensor.h | 4 +- src/main.cpp | 97 ++++++++++--------------- 3 files changed, 141 insertions(+), 109 deletions(-) diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp index 81135fc..21f0799 100644 --- a/src/linearHallSensor.cpp +++ b/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 - for (float i = _minPositionEndValue; i <= _maxPositionEndValue; 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; - } - 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; - } + // Swipe motor to search hard end and find max analog values of sensors + bool endFound = false; + const float step = 0.0025; + uint8_t currentCheck = 0; // current check number + const uint8_t nCheck = 15; // number of times to check if its the same value + const uint8_t epsilon = 3; + float currentPosition = 0.0; - // Min offset with 50 values average - float sum = 0.0; - for (uint8_t i = 0; i <= 49; i++) + Serial.println("\tFinding end stops"); + while (!endFound) { - _offset = 0.0; - sum += readSensorCallback(); + Serial.print("Position =" + String(currentPosition) + '\t'); + 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 - for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01) + // Read max angle + 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); diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h index 28663b7..5af1225 100644 --- a/src/linearHallSensor.h +++ b/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); /** diff --git a/src/main.cpp b/src/main.cpp index f060e1a..6ab9c20 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,22 +3,20 @@ #include #include -BLDCMotor motor = BLDCMotor(4); -BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3); +BLDCMotor motor1 = BLDCMotor(4); +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 initSensor() +void initSensor1() { - linearSensor.init(motor); + linearSensor1.init(motor1); +} +float callback1() +{ + return linearSensor1.readSensorCallback(); } -float callback() -{ - return linearSensor.readSensorCallback(); -} - -GenericSensor sensor = GenericSensor(callback, initSensor); +GenericSensor sensor1 = GenericSensor(callback1, initSensor1); 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(M1_PWM2, OUTPUT); - // pinMode(M1_PWM3, OUTPUT); - // digitalWrite(M1_PWM1, LOW); - // digitalWrite(M1_PWM2, LOW); - // digitalWrite(M1_PWM3, LOW); + pinMode(LED_BUILTIN, OUTPUT); // Lightup LED + digitalWrite(LED_BUILTIN, LOW); - // pinMode(M2_PWM1, OUTPUT); - // pinMode(M2_PWM2, OUTPUT); - // pinMode(M2_PWM3, OUTPUT); - // digitalWrite(M2_PWM1, LOW); - // digitalWrite(M2_PWM2, LOW); - // digitalWrite(M2_PWM3, LOW); - - // pinMode(M3_PWM1, OUTPUT); - // pinMode(M3_PWM2, OUTPUT); - // pinMode(M3_PWM3, OUTPUT); - // digitalWrite(M3_PWM1, LOW); - // digitalWrite(M3_PWM2, LOW); - // digitalWrite(M3_PWM3, LOW); - - pinMode(LED_BUILTIN, OUTPUT); // Lightup LED - 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); + driver1.voltage_power_supply = 7.1; + driver1.init(); + motor1.linkDriver(&driver1); + motor1.useMonitoring(Serial); + motor1.controller = MotionControlType::angle; + motor1.foc_modulation = FOCModulationType::SinePWM; + motor1.voltage_limit = 1.0; + motor1.voltage_sensor_align = 1.0; + motor1.PID_velocity.P = 0.05f; + motor1.PID_velocity.I = 0.01; + motor1.PID_velocity.D = 0.0; + motor1.LPF_velocity.Tf = 0.01f; + motor1.P_angle.P = 150.0; + motor1.P_angle.I = 10.0; + motor1.velocity_limit = 25; // 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); - motor.init(); + motor1.linkSensor(&sensor1); + motor1.init(); // motor.initFOC(5.48, CCW); - motor.initFOC(); + motor1.initFOC(); } void loop() { serialLoop(); - motor.move(target); - motor.loopFOC(); - motor.monitor(); + motor1.move(target); + motor1.loopFOC(); + motor1.monitor(); } \ No newline at end of file