Endstop detection OK, thx Yassine ;)
This commit is contained in:
Lucien RENAUD
parent
526ffacd4f
commit
1e34b1992e
|
|
@ -1,5 +1,7 @@
|
|||
#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;
|
||||
|
|
@ -11,6 +13,55 @@ LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
|
|||
_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;
|
||||
|
|
@ -18,108 +69,108 @@ void LinearHallSensor::init(BLDCMotor motor)
|
|||
float prevVoltageLimit = motor.voltage_limit;
|
||||
motor.voltage_limit = 0.8;
|
||||
|
||||
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 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
|
||||
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;
|
||||
const uint8_t epsilon = 2;
|
||||
float currentPosition = 0.0;
|
||||
|
||||
uint32_t senseA[30];
|
||||
uint32_t senseB[30];
|
||||
int N = 30;
|
||||
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("\tFinding end stops");
|
||||
while (!endFound)
|
||||
{
|
||||
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);
|
||||
senseA[ptr] = analogRead(_analogPin1);
|
||||
senseB[ptr] = 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;
|
||||
if (senseA[ptr] > _maxCh1)
|
||||
_maxCh1 = senseA[ptr];
|
||||
else if (senseA[ptr] < _minCh1)
|
||||
_minCh1 = senseA[ptr];
|
||||
if (senseB[ptr] > _maxCh1)
|
||||
_maxCh1 = senseB[ptr];
|
||||
else if (senseB[ptr] < _minCh2)
|
||||
_minCh2 = senseB[ptr];
|
||||
|
||||
// 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;
|
||||
// Serial.printf("senseA: ");
|
||||
// print_arr(senseA, N);
|
||||
// Serial.printf(" std_var: %lu\n", std_var(senseA, N));
|
||||
// Serial.printf("senseB: ");
|
||||
// print_arr(senseB, N);
|
||||
// Serial.printf(" std_var: %lu\n", std_var(senseB, N));
|
||||
|
||||
// Replace previous values
|
||||
oldSenseA = senseA;
|
||||
oldSenseB = senseB;
|
||||
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(2);
|
||||
}
|
||||
_maxPositionEndValue = currentPosition - (step * nCheck);
|
||||
currentPosition = _maxPositionEndValue;
|
||||
// _maxPositionEndValue = currentPosition - (step * nCheck);
|
||||
_maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
|
||||
currentPosition = _maxPositionEndValue - M_PI / 8;
|
||||
delay(100);
|
||||
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
|
||||
// Swipe motor to search other hard end, and find eventually new max analog values of sensors
|
||||
endFound = false;
|
||||
currentCheck = 0;
|
||||
|
||||
init_arr(senseA, N, 0);
|
||||
init_arr(senseB, N, 0);
|
||||
ptr = 0;
|
||||
|
||||
while (!endFound)
|
||||
{
|
||||
senseA = analogRead(_analogPin1);
|
||||
senseB = analogRead(_analogPin2);
|
||||
senseA[ptr] = analogRead(_analogPin1);
|
||||
senseB[ptr] = 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;
|
||||
if (senseA[ptr] > _maxCh1)
|
||||
_maxCh1 = senseA[ptr];
|
||||
else if (senseA[ptr] < _minCh1)
|
||||
_minCh1 = senseA[ptr];
|
||||
if (senseB[ptr] > _maxCh1)
|
||||
_maxCh1 = senseB[ptr];
|
||||
else if (senseB[ptr] < _minCh2)
|
||||
_minCh2 = senseB[ptr];
|
||||
|
||||
// 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;
|
||||
// Serial.printf("senseA: "); print_arr(senseA, N); Serial.printf(" std_var: %lu\n", std_var(senseA, N));
|
||||
// Serial.printf("senseB: "); print_arr(senseB, N); Serial.printf(" std_var: %lu\n", std_var(senseB, N));
|
||||
|
||||
// Replace previous values
|
||||
oldSenseA = senseA;
|
||||
oldSenseB = senseB;
|
||||
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(2);
|
||||
}
|
||||
_minPositionEndValue = currentPosition + (step * nCheck);
|
||||
_minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
|
||||
Serial.println("Found second end stop : Min position = " + String(_minPositionEndValue));
|
||||
delay(100);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue