Endstop detection OK, thx Yassine ;)

This commit is contained in:
Lucien RENAUD 2022-09-01 17:39:51 +02:00
parent 526ffacd4f
commit 1e34b1992e

View File

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