From f28a03ff051496cb580776866cb6a4325b3306f8 Mon Sep 17 00:00:00 2001 From: Lucien Renaud Date: Thu, 19 May 2022 23:32:31 +0200 Subject: [PATCH] Add linearHallSensor class --- platformio.ini | 2 +- src/linearHallSensor.cpp | 223 +++++++++++++++++++++++++++++++++++++++ src/linearHallSensor.h | 68 ++++++++++++ src/main.cpp | 100 ++++++++++-------- 4 files changed, 349 insertions(+), 44 deletions(-) create mode 100644 src/linearHallSensor.cpp create mode 100644 src/linearHallSensor.h diff --git a/platformio.ini b/platformio.ini index d55db0d..9f28ed2 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,4 +15,4 @@ framework = arduino monitor_port = COM3 monitor_speed = 115200 lib_archive = no -; lib_deps = askuric/Simple FOC@^2.2.2 +lib_deps = askuric/Simple FOC@^2.2.2 diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp new file mode 100644 index 0000000..560f0b3 --- /dev/null +++ b/src/linearHallSensor.cpp @@ -0,0 +1,223 @@ +#include "linearHallSensor.h" + +float norm(float x, float in_min, float in_max) +{ + return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0; +} + +LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2) +{ + _analogPin1 = ch1; + _analogPin2 = ch2; +} + +void LinearHallSensor::init(BLDCMotor motor) +{ + int senseA = analogRead(A0); + int senseB = analogRead(A1); + + int minA = senseA, maxA = senseA, minB = senseB, maxB = senseB; + + // Swipe motor 2 times + for (float i = -0.55; i <= 2.25; 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); + } + + delay(100); + + for (float i = 2.25; i >= -0.55; 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); + } + + _maxCh1 = maxA; + _maxCh2 = maxB; + _minCh1 = minA; + _minCh2 = minB; + + // 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; + + //measure offset angle + float normCh1 = norm(_prevCh1, _minCh1, _maxCh1); + float normCh2 = norm(_prevCh2, _minCh2, _maxCh2); + float pos = atan2(normCh1, normCh2); + + _offset = M_PI_2 - pos; + + + Serial.println("maxA: " + String(maxA) + "\tminA: " + String(minA) + "\tmaxB: " + String(maxB) + "\tminB: " + String(minB)); + Serial.println("Current Ch1:" + String(_prevCh1) + "\tCh2:" + String(_prevCh2)); +} + + +float LinearHallSensor::readSensorCallback() +{ + // Update variables + float currentCh1 = analogRead(_analogPin1); + float currentCh2 = analogRead(_analogPin2); + + // Normalise variable between -1 and 1 + float normCh1 = norm(currentCh1, _minCh1, _maxCh1); + float normCh2 = norm(currentCh2, _minCh2, _maxCh2); + + _currentPosition = atan2f(normCh2, normCh1); + Serial.print("Current angle: " + String(_currentPosition)); + + 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; +} diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h new file mode 100644 index 0000000..531fbb2 --- /dev/null +++ b/src/linearHallSensor.h @@ -0,0 +1,68 @@ +#ifndef LinearHallSensor_h +#define LinearHallSensor_h + +#include "Arduino.h" +#include "BLDCMotor.h" + +enum Slope : int8_t{ + INCREASING = 1, + DECREASING = -1, + EQUAL = 0 +}; + +/** + * @brief Linear Hall sensor class + * + */ +class LinearHallSensor +{ + public: + /** + Linear Hall Sensor class constructor + @param ch1 Analog pin for channel 1 + @param ch2 Analog pin for channel 2 + */ + LinearHallSensor(uint8_t ch1, uint8_t ch2); + + + /** + * @brief Initialize variable by measuring max and min value + * + */ + void init(BLDCMotor motor); + + + /** + * @brief Read the sensors and return the current angle in rad + * + * @return rad and angle + */ + float readSensorCallbackStateMachine(); + + float readSensorCallback(); + + private: + uint8_t _analogPin1; + uint8_t _analogPin2; + + uint16_t _maxCh1; + uint16_t _maxCh2; + uint16_t _minCh1; + uint16_t _minCh2; + + float _currentPosition; + float _offset; + + float _minPositionEndValue; + 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 ba60637..0911a69 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,8 @@ #include #include +#include + -MagneticSensorAnalog sensor = MagneticSensorAnalog(A0, 183, 512); BLDCMotor motorX = BLDCMotor(4); BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6); @@ -9,6 +10,21 @@ BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6); BLDCMotor motorY = BLDCMotor(4); BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9); +LinearHallSensor linearSensor = LinearHallSensor(A0, A1); + +void initSensor() +{ + linearSensor.init(motorY); +} + +float callback() +{ + return linearSensor.readSensorCallback(); +} + +GenericSensor sensor = GenericSensor(callback, initSensor); + + float targetX = 0.0; float targetY = 0.0; @@ -16,37 +32,35 @@ float target = 6.3; void setup() { - - sensor.init(); - // motorY.linkSensor(&sensor); - // 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.PID_velocity.P = 0.02f; - motorX.PID_velocity.I = 20; - motorX.PID_velocity.D = 0; - motorX.voltage_limit = 0.8; - motorX.LPF_velocity.Tf = 0.01f; - motorX.P_angle.P = 20; - motorX.velocity_limit = 20; + 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::SpaceVectorPWM; + motorY.foc_modulation = FOCModulationType::SinePWM; motorY.controller = MotionControlType::angle_openloop; - motorY.PID_velocity.P = 0.001f; - motorY.PID_velocity.I = 0; - motorY.PID_velocity.D = 0; motorY.voltage_limit = 0.8; - motorY.LPF_velocity.Tf = 0.01f; - motorY.P_angle.P = 0.1; - motorY.velocity_limit = 3; + // 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); @@ -58,10 +72,14 @@ void setup() motorY.init(); motorY.initFOC(); - Serial.println(F("Motor ready.")); - _delay(1000); - // Serial.println(sensor.getPreciseAngle()); + motorX.disable(); + Serial.println(F("Motor ready.")); + _delay(100); + Serial.println("calibrating..."); + sensor.init(); + Serial.println("Done\r\n"); + motorY.disable(); } void serialLoop() @@ -89,30 +107,26 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max void loop() { sensor.update(); + // serialLoop(); - // Serial.println(sensor.getPreciseAngle()); - // delay(5); + // int senseX = analogRead(A2); + // int senseY = analogRead(A3); - serialLoop(); + // targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2); + // targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2); - int senseX = analogRead(A2); - int senseY = analogRead(A3); + // 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); - targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2); - targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2); + // motorX.move(targetX); + // motorX.loopFOC(); - 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(); + // motorY.move(targetY); + // motorY.loopFOC(); } \ No newline at end of file