From 1bcd56e5f5a7d719af4d115c0b06cac992a35818 Mon Sep 17 00:00:00 2001 From: Lucien Renaud Date: Tue, 24 May 2022 10:32:15 +0200 Subject: [PATCH] Close-loop position OK :D --- src/linearHallSensor.cpp | 269 +++++++++++++-------------------------- src/linearHallSensor.h | 38 +++--- src/main.cpp | 138 ++++++++------------ 3 files changed, 154 insertions(+), 291 deletions(-) diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp index 560f0b3..637e6ea 100644 --- a/src/linearHallSensor.cpp +++ b/src/linearHallSensor.cpp @@ -1,223 +1,128 @@ #include "linearHallSensor.h" -float norm(float x, float in_min, float in_max) +float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_max = 1.0) { - return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0; + return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min; } -LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2) +LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos) { _analogPin1 = ch1; _analogPin2 = ch2; + _minPositionEndValue = minPos; + _maxPositionEndValue = maxPos; } void LinearHallSensor::init(BLDCMotor motor) { - int senseA = analogRead(A0); - int senseB = analogRead(A1); + MotionControlType prevController = motor.controller; + motor.controller = MotionControlType::angle_openloop; + float prevVoltageLimit = motor.voltage_limit; + motor.voltage_limit = 0.8; - int minA = senseA, maxA = senseA, minB = senseB, maxB = senseB; + uint32_t senseA = analogRead(_analogPin1); + uint32_t senseB = analogRead(_analogPin2); - // Swipe motor 2 times - for (float i = -0.55; i <= 2.25; i = i + 0.005) + _minCh1 = senseA, _maxCh1 = senseA; + _minCh2 = senseB, _maxCh2 = senseB; + + // Swipe motor + for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005) { motor.move(i); - senseA = analogRead(A0); - senseB = analogRead(A1); - if (senseA > maxA) - maxA = senseA; - else if (senseA < minA) - minA = senseA; - if (senseB > maxB) - maxB = senseB; - else if (senseB < minB) - minB = senseB; 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 = 2.25; i >= -0.55; i = i - 0.005) + for (float i = _maxPositionEndValue; i >= _minPositionEndValue; i = i - 0.005) { motor.move(i); - senseA = analogRead(A0); - senseB = analogRead(A1); - if (senseA > maxA) - maxA = senseA; - else if (senseA < minA) - minA = senseA; - if (senseB > maxB) - maxB = senseB; - else if (senseB < minB) - minB = senseB; 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; } - _maxCh1 = maxA; - _maxCh2 = maxB; - _minCh1 = minA; - _minCh2 = minB; + // Min offset with 50 values average + float sum = 0.0; + for (uint8_t i = 0; i <= 49; i++) + { + _offset = 0.0; + sum += readSensorCallback(); + } + _offset = -sum / 50.0; - // Intialise the state machine variables - _state = 0; // Set the state machine to 0 - _prevCh1 = analogRead(_analogPin1); - _prevCh2 = analogRead(_analogPin2); - _directionCh1 = Direction::UNKNOWN; - _directionCh2 = Direction::UNKNOWN; + Serial.println("\n\rmaxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2)); + Serial.println("Offset: " + String(_offset)); - //measure offset angle - float normCh1 = norm(_prevCh1, _minCh1, _maxCh1); - float normCh2 = norm(_prevCh2, _minCh2, _maxCh2); - float pos = atan2(normCh1, normCh2); + // Detect max angle + for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01) + { + motor.move(i); + delay(1); + _maxSensor = readSensorCallback(); + } + Serial.println("current motor angle: " + String(motor.electricalAngle())); + delay(250); - _offset = M_PI_2 - pos; + // take average value of 50 values + sum = 0.0; + for (uint8_t i = 0; i <= 49; i++) + sum += readSensorCallback(); + _maxSensor = sum / 50.0; + Serial.println("Max sensor: " + String(_maxSensor)); - Serial.println("maxA: " + String(maxA) + "\tminA: " + String(minA) + "\tmaxB: " + String(maxB) + "\tminB: " + String(minB)); - Serial.println("Current Ch1:" + String(_prevCh1) + "\tCh2:" + String(_prevCh2)); + // Recovering previous controller type and go in the middle + for (float i = _maxPositionEndValue; i >= _minPositionEndValue/2.0; i = i - 0.01) + { + motor.move(i); + delay(1); + readSensorCallback(); + } + motor.controller = prevController; + motor.voltage_limit = prevVoltageLimit; } +float dist_angle(float newAngle, float prevAngle) +{ + float diff = newAngle - prevAngle; + while (diff < (-M_PI)) + diff += 2 * M_PI; + while (diff > M_PI) + diff -= 2 * M_PI; + return diff; +} float LinearHallSensor::readSensorCallback() { // Update variables - float currentCh1 = analogRead(_analogPin1); - float currentCh2 = analogRead(_analogPin2); + uint32_t currentCh1 = analogRead(_analogPin1); + uint32_t currentCh2 = analogRead(_analogPin2); // Normalise variable between -1 and 1 - float normCh1 = norm(currentCh1, _minCh1, _maxCh1); - float normCh2 = norm(currentCh2, _minCh2, _maxCh2); + float normCh1 = norm((float)currentCh1, _minCh1, _maxCh1); + float normCh2 = norm((float)currentCh2, _minCh2, _maxCh2); _currentPosition = atan2f(normCh2, normCh1); - Serial.print("Current angle: " + String(_currentPosition)); + _estimatePosition += dist_angle(_currentPosition, _prevPosition); - float temp1 = normCh1 + sin(_offset); - normCh1 = temp1 * floorf(temp1); - - float temp2 = normCh2 + cos(_offset); - normCh2 = temp2 * floorf(temp2); - - _currentPosition = atan2f(normCh2, normCh1); - Serial.print("\tNew angle: " + String(_currentPosition)+ '\r'); - - return _currentPosition; -} - -float LinearHallSensor::readSensorCallbackStateMachine() -{ - // Update variables - float currentCh1 = analogRead(_analogPin1); - float currentCh2 = analogRead(_analogPin2); - - if (currentCh1 > _prevCh1) - _directionCh1 = Slope::INCREASING; - else if (currentCh1 < _prevCh1) - _directionCh1 = Slope::DECREASING; - else - _directionCh1 = Slope::EQUAL; - - if (currentCh2 > _prevCh2) - _directionCh2 = Slope::INCREASING; - else if (currentCh2 < _prevCh2) - _directionCh2 = Slope::DECREASING; - else - _directionCh2 = Slope::EQUAL; - - // Normalise variable between -1 and 1 - float normCh1 = norm(currentCh1, _minCh1, _maxCh1); - float normCh2 = norm(currentCh2, _minCh2, _maxCh2); - - // Update previous var - _prevCh1 = currentCh1; - _prevCh2 = currentCh2; - - // State machine - switch (_state) - { - case 0: - _currentPosition = 0.0; - if (_directionCh1 == Slope::INCREASING) - _state = 1; - break; - case 1: - if(_directionCh1 != Slope::EQUAL) - _currentPosition++; - - if (_directionCh1 == Slope::DECREASING) - _state = 2; - else if (normCh1 >= 0.8 && _directionCh1 == Slope::INCREASING) - _state = 3; - break; - case 2: - if(_directionCh1 != Slope::EQUAL) - _currentPosition--; - - if (_directionCh1 == Slope::INCREASING) - _state = 1; - else if (_directionCh1 == Slope::DECREASING && normCh2 <= 0.2) - _state = 8; - break; - case 3: - if(_directionCh2 != Slope::EQUAL) - _currentPosition++; - - if (_directionCh2 == Slope::INCREASING) - _state = 4; - else if (_directionCh2 == Slope::DECREASING && normCh2 <= 0.2) - _state = 5; - break; - case 4: - if(_directionCh2 != Slope::EQUAL) - _currentPosition--; - - if (_directionCh2 == Slope::DECREASING) - _state = 3; - else if (_directionCh2 == Slope::INCREASING && normCh1 <= 0.8) - _state = 2; - break; - case 5: - if(_directionCh1 != Slope::EQUAL) - _currentPosition++; - if (_directionCh1 == Slope::INCREASING) - _state = 6; - else if (_directionCh1 == Slope::DECREASING && normCh1 <= 0.2) - _state = 7; - break; - case 6: - if(_directionCh1 != Slope::EQUAL) - _currentPosition--; - - if (_directionCh1 == Slope::DECREASING) - _state = 5; - else if (_directionCh1 == Slope::INCREASING && normCh2 >= 0.2) - _state = 4; - break; - case 7: - if(_directionCh2 != Slope::EQUAL) - _currentPosition++; - - if (_directionCh2 == Slope::DECREASING) - _state = 8; - else if (_directionCh2 == Slope::INCREASING && normCh2 >= 0.8) - _state = 1; - break; - case 8: - if(_directionCh2 != Slope::EQUAL) - _currentPosition--; - - if (_directionCh2 == Slope::INCREASING) - _state = 7; - else if (_directionCh2 == Slope::DECREASING && normCh1 >= 0.2) - _state = 6; - break; - - default: - break; - } - - Serial.print("\rState: " + String(_state) + - "\tSlope Ch1: " + String(_directionCh1) + "\tnormCh1: " + String(normCh1)+ - "\tSlope Ch2: " + String(_directionCh2) + "\tnormCh2: " + String(normCh2)+ - "\tPosition: " + String(_currentPosition)); - return _currentPosition; + _prevPosition = _currentPosition; + return (_estimatePosition + _offset)/4.0; } diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h index 531fbb2..28663b7 100644 --- a/src/linearHallSensor.h +++ b/src/linearHallSensor.h @@ -4,12 +4,6 @@ #include "Arduino.h" #include "BLDCMotor.h" -enum Slope : int8_t{ - INCREASING = 1, - DECREASING = -1, - EQUAL = 0 -}; - /** * @brief Linear Hall sensor class * @@ -21,8 +15,10 @@ 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); + LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos); /** @@ -42,27 +38,23 @@ class LinearHallSensor float readSensorCallback(); private: - uint8_t _analogPin1; - uint8_t _analogPin2; + uint32_t _analogPin1; + uint32_t _analogPin2; - uint16_t _maxCh1; - uint16_t _maxCh2; - uint16_t _minCh1; - uint16_t _minCh2; + // max values read from the ADC on channels during init + uint32_t _maxCh1; + uint32_t _maxCh2; + uint32_t _minCh1; + uint32_t _minCh2; + float _prevPosition; float _currentPosition; - float _offset; + float _estimatePosition; // current position + offset + float _offset; // offset angle measured at init + float _maxSensor; // Max angle value measured (so command from 0 to that value) - float _minPositionEndValue; + float _minPositionEndValue; // min pos in open loop mode float _maxPositionEndValue; - - uint16_t _prevCh1; - uint16_t _prevCh2; - - int8_t _directionCh1; - int8_t _directionCh2; - - uint8_t _state; }; #endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 0911a69..beab5d7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,19 +2,14 @@ #include #include +BLDCMotor motor = BLDCMotor(4); +BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9); - -BLDCMotor motorX = BLDCMotor(4); -BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6); - -BLDCMotor motorY = BLDCMotor(4); -BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9); - -LinearHallSensor linearSensor = LinearHallSensor(A0, A1); +LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35); void initSensor() { - linearSensor.init(motorY); + linearSensor.init(motor); } float callback() @@ -24,63 +19,10 @@ float callback() GenericSensor sensor = GenericSensor(callback, initSensor); - float targetX = 0.0; float targetY = 0.0; -float target = 6.3; - -void setup() -{ - // X MOTR STUFF - driverX.voltage_power_supply = 5.0; - driverX.init(); - motorX.linkDriver(&driverX); - motorX.foc_modulation = FOCModulationType::SpaceVectorPWM; - motorX.controller = MotionControlType::angle_openloop; - motorX.voltage_limit = 0.6; - // motorX.PID_velocity.P = 0.001f; - // motorX.PID_velocity.I = 20; - // motorX.PID_velocity.D = 0; - // motorX.LPF_velocity.Tf = 0.01f; - // motorX.P_angle.P = 20; - motorX.velocity_limit = 10; - - // *************** Y MOTOR STUFF ****************** - driverY.voltage_power_supply = 5.0; - driverY.init(); - motorY.linkDriver(&driverY); - motorY.foc_modulation = FOCModulationType::SinePWM; - motorY.controller = MotionControlType::angle_openloop; - motorY.voltage_limit = 0.8; - // motorY.PID_velocity.P = 0.001f; - // motorY.PID_velocity.I = 0; - // motorY.PID_velocity.D = 0; - // motorY.LPF_velocity.Tf = 0.01f; - // motorY.P_angle.P = 0.1; - motorY.velocity_limit = 10; - - - - Serial.begin(115200); - _delay(100); - Serial.println("INIT"); - - // initialize motor - motorX.init(); - motorX.initFOC(); - motorY.init(); - motorY.initFOC(); - - motorX.disable(); - - Serial.println(F("Motor ready.")); - _delay(100); - Serial.println("calibrating..."); - sensor.init(); - Serial.println("Done\r\n"); - motorY.disable(); -} +float target = 0.0; void serialLoop() { @@ -104,29 +46,53 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min; } +void setup() +{ + Serial.begin(115200); + _delay(100); + Serial.println("INIT"); + pinMode(3, OUTPUT); + pinMode(5, OUTPUT); + pinMode(6, OUTPUT); + digitalWrite(3, LOW); + digitalWrite(5, LOW); + digitalWrite(6, LOW); + + driver.voltage_power_supply = 6.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 = 200.0; + motor.P_angle.I = 50.0; + motor.velocity_limit = 50; + + motor.useMonitoring(Serial); + + + // Init sensor + motor.init(); + Serial.println("calibrating sensor in open loop..."); + sensor.init(); + Serial.println("Done"); + delay(1000); + + motor.linkSensor(&sensor); + motor.init(); + motor.initFOC(); +} + void loop() { - sensor.update(); - // serialLoop(); - - // int senseX = analogRead(A2); - // int senseY = analogRead(A3); - - // targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2); - // targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2); - - // Serial.print("senseX="); - // Serial.print(senseX); - // Serial.print("\t mappedX="); - // Serial.print(targetX); - // Serial.print("\t senseY="); - // Serial.print(senseY); - // Serial.print("\t mappedY="); - // Serial.println(targetY); - - // motorX.move(targetX); - // motorX.loopFOC(); - - // motorY.move(targetY); - // motorY.loopFOC(); + serialLoop(); + motor.move(target); + motor.loopFOC(); + motor.monitor(); } \ No newline at end of file