lurenaud/DJI-Gimbal-FOC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add linearHallSensor class

Browse Source
This commit is contained in:
Lucien Renaud 2022-05-19 23:32:31 +02:00
parent 095bf9e1b3
commit f28a03ff05
4 changed files with 349 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
platformio.ini
View File

@ -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

223
src/linearHallSensor.cpp Normal file
View File

@ -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;
}

68
src/linearHallSensor.h Normal file
View File

@ -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

100
src/main.cpp
View File

@ -1,7 +1,8 @@
#include <Arduino.h>
#include <SimpleFOC.h>
#include <linearHallSensor.h>
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();
}