234 lines
6.0 KiB
C++
234 lines
6.0 KiB
C++
#include "linearHallSensor.h"
|
|
|
|
#include <math.h>
|
|
|
|
float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_max = 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)
|
|
{
|
|
_analogPin1 = ch1;
|
|
_analogPin2 = ch2;
|
|
}
|
|
|
|
inline uint32_t min(uint32_t arr[], int n)
|
|
{
|
|
uint32_t m = 0;
|
|
for (int i = 0; i < n; i++)
|
|
m = arr[i] < m ? arr[i] : m;
|
|
return m;
|
|
}
|
|
|
|
inline uint32_t max(uint32_t *arr, int n)
|
|
{
|
|
uint32_t m = 0;
|
|
for (int i = 0; i < n; i++)
|
|
m = arr[i] > m ? arr[i] : m;
|
|
return m;
|
|
}
|
|
|
|
inline uint32_t mean(uint32_t *arr, int n)
|
|
{
|
|
uint32_t m = 0;
|
|
for (int i = 0; i < n; i++)
|
|
m += arr[i];
|
|
return m / n;
|
|
}
|
|
|
|
inline uint32_t std_var(uint32_t *arr, int n)
|
|
{
|
|
uint32_t s = 0;
|
|
uint32_t m = mean(arr, n);
|
|
for (int i = 0; i < n; i++)
|
|
s += (arr[i] - m) * (arr[i] - m);
|
|
s /= (uint32_t)n;
|
|
s = sqrt(s);
|
|
return s;
|
|
}
|
|
|
|
void init_arr(uint32_t *arr, int n, uint32_t val)
|
|
{
|
|
for (int i = 0; i < n; i++)
|
|
arr[i] = val;
|
|
}
|
|
|
|
void print_arr(uint32_t *arr, int n)
|
|
{
|
|
Serial.printf("[");
|
|
for (int i = 0; i < n; i++)
|
|
Serial.printf("%lu,", arr[i]);
|
|
Serial.printf("]");
|
|
}
|
|
|
|
void LinearHallSensor::init(BLDCMotor motor)
|
|
{
|
|
MotionControlType prevController = motor.controller;
|
|
motor.controller = MotionControlType::angle_openloop;
|
|
float prevVoltageLimit = motor.voltage_limit;
|
|
motor.voltage_limit = 2.0;
|
|
|
|
// Swipe motor to search hard end and find max analog values of sensors
|
|
bool endFound = false;
|
|
const float step = 0.005;
|
|
const uint8_t epsilon = 3;
|
|
float currentPosition = 0.0;
|
|
|
|
const uint8_t N = 20;
|
|
uint32_t senseA[N];
|
|
uint32_t senseB[N];
|
|
|
|
int ptr = 0;
|
|
|
|
init_arr(senseA, N, 0);
|
|
init_arr(senseB, N, 0);
|
|
|
|
senseA[ptr] = analogRead(_analogPin1);
|
|
senseB[ptr] = analogRead(_analogPin2);
|
|
|
|
_minCh1 = senseA[ptr];
|
|
_minCh2 = senseB[ptr];
|
|
_maxCh1 = _minCh1;
|
|
_maxCh2 = _minCh2;
|
|
|
|
Serial.println("\t- Finding end stops");
|
|
while (!endFound)
|
|
{
|
|
senseA[ptr] = analogRead(_analogPin1);
|
|
senseB[ptr] = analogRead(_analogPin2);
|
|
|
|
// Check if new extremes sensor values
|
|
if (senseA[ptr] > _maxCh1)
|
|
_maxCh1 = senseA[ptr];
|
|
else if (senseA[ptr] < _minCh1)
|
|
_minCh1 = senseA[ptr];
|
|
|
|
if (senseB[ptr] > _maxCh2)
|
|
_maxCh2 = senseB[ptr];
|
|
else if (senseB[ptr] < _minCh2)
|
|
_minCh2 = senseB[ptr];
|
|
|
|
if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
|
|
endFound = true;
|
|
|
|
ptr = (ptr + 1) % N;
|
|
|
|
// Move to new position
|
|
currentPosition += step;
|
|
motor.move(currentPosition);
|
|
delay(3);
|
|
}
|
|
_maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
|
|
currentPosition = _maxPositionEndValue - M_PI / 16;
|
|
delay(100);
|
|
motor.move(currentPosition);
|
|
Serial.println("\t- 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;
|
|
|
|
init_arr(senseA, N, 0);
|
|
init_arr(senseB, N, 0);
|
|
ptr = 0;
|
|
|
|
while (!endFound)
|
|
{
|
|
senseA[ptr] = analogRead(_analogPin1);
|
|
senseB[ptr] = analogRead(_analogPin2);
|
|
|
|
// Check if new extremes sensor values
|
|
if (senseA[ptr] > _maxCh1)
|
|
_maxCh1 = senseA[ptr];
|
|
else if (senseA[ptr] < _minCh1)
|
|
_minCh1 = senseA[ptr];
|
|
|
|
if (senseB[ptr] > _maxCh2)
|
|
_maxCh2 = senseB[ptr];
|
|
else if (senseB[ptr] < _minCh2)
|
|
_minCh2 = senseB[ptr];
|
|
|
|
if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
|
|
endFound = true;
|
|
|
|
ptr = (ptr + 1) % N;
|
|
|
|
// Move to new position
|
|
currentPosition -= step;
|
|
motor.move(currentPosition);
|
|
delay(3);
|
|
}
|
|
_minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
|
|
Serial.println("\t- 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("\t- maxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
|
|
Serial.println("\t- Offset: " + String(_offset));
|
|
|
|
// Read max angle
|
|
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.0025)
|
|
{
|
|
motor.move(i);
|
|
delay(1);
|
|
_maxSensor = readSensorCallback();
|
|
}
|
|
delay(250);
|
|
|
|
// Set max 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("\t- Max angle: " + String(_maxSensor));
|
|
|
|
// Recovering previous controller type and go to 0
|
|
for (float i = _maxPositionEndValue; i >= (0); i = i - 0.005)
|
|
{
|
|
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
|
|
uint32_t currentCh1 = analogRead(_analogPin1);
|
|
uint32_t currentCh2 = analogRead(_analogPin2);
|
|
|
|
// Normalise variable between -1 and 1
|
|
float normCh1 = norm((float)currentCh1, _minCh1, _maxCh1);
|
|
float normCh2 = norm((float)currentCh2, _minCh2, _maxCh2);
|
|
|
|
_currentPosition = atan2f(normCh2, normCh1);
|
|
_estimatePosition += dist_angle(_currentPosition, _prevPosition);
|
|
|
|
_prevPosition = _currentPosition;
|
|
return (_estimatePosition / 4.0 - _offset);
|
|
}
|
|
|
|
float LinearHallSensor::getMaxAngle()
|
|
{
|
|
return _maxSensor;
|
|
} |