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

Endstop detection OK, thx Yassine ;)

Browse Source
This commit is contained in:
Lucien RENAUD 2022-09-01 17:39:51 +02:00
parent 526ffacd4f
commit 1e34b1992e
1 changed files with 116 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
src/linearHallSensor.cpp
View File

@ -1,5 +1,7 @@
#include "linearHallSensor.h" #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) 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; 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; _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) void LinearHallSensor::init(BLDCMotor motor)
{ {
MotionControlType prevController = motor.controller; MotionControlType prevController = motor.controller;
@ -18,108 +69,108 @@ void LinearHallSensor::init(BLDCMotor motor)
float prevVoltageLimit = motor.voltage_limit; float prevVoltageLimit = motor.voltage_limit;
motor.voltage_limit = 0.8; 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 // Swipe motor to search hard end and find max analog values of sensors
bool endFound = false; bool endFound = false;
const float step = 0.0025; 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 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; 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"); Serial.println("\tFinding end stops");
while (!endFound) while (!endFound)
{ {
Serial.print("Position =" + String(currentPosition) + '\t'); senseA[ptr] = analogRead(_analogPin1);
Serial.print("SenseA =" + String(senseA) + "\tSenseB =" + String(senseB)); senseB[ptr] = analogRead(_analogPin2);
Serial.print("\tOldseA =" + String(oldSenseA) + "\tOldseB =" + String(oldSenseB) + "\tchecked =" + String(currentCheck) + "\r");
senseA = analogRead(_analogPin1);
senseB = analogRead(_analogPin2);
// Check if new extremes sensor values // Check if new extremes sensor values
if (senseA > _maxCh1) if (senseA[ptr] > _maxCh1)
_maxCh1 = senseA; _maxCh1 = senseA[ptr];
else if (senseA < _minCh1) else if (senseA[ptr] < _minCh1)
_minCh1 = senseA; _minCh1 = senseA[ptr];
if (senseB > _maxCh1) if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB; _maxCh1 = senseB[ptr];
else if (senseB < _minCh2) else if (senseB[ptr] < _minCh2)
_minCh2 = senseB; _minCh2 = senseB[ptr];
// Compare with previous position // Serial.printf("senseA: ");
if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon)) // print_arr(senseA, N);
{ // Serial.printf(" std_var: %lu\n", std_var(senseA, N));
if (currentCheck == nCheck) // Serial.printf("senseB: ");
// print_arr(senseB, N);
// Serial.printf(" std_var: %lu\n", std_var(senseB, N));
if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
endFound = true; endFound = true;
else
currentCheck++;
}
else
currentCheck = 0;
// Replace previous values ptr = (ptr + 1) % N;
oldSenseA = senseA;
oldSenseB = senseB;
// Move to new position // Move to new position
currentPosition += step; currentPosition += step;
motor.move(currentPosition); motor.move(currentPosition);
delay(2); delay(2);
} }
_maxPositionEndValue = currentPosition - (step * nCheck); // _maxPositionEndValue = currentPosition - (step * nCheck);
currentPosition = _maxPositionEndValue; _maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
currentPosition = _maxPositionEndValue - M_PI / 8;
delay(100);
motor.move(currentPosition); motor.move(currentPosition);
Serial.println("Found first end stop : Max position = " + String(_maxPositionEndValue)); Serial.println("Found first end stop : Max position = " + String(_maxPositionEndValue));
delay(100); 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; endFound = false;
currentCheck = 0; currentCheck = 0;
init_arr(senseA, N, 0);
init_arr(senseB, N, 0);
ptr = 0;
while (!endFound) while (!endFound)
{ {
senseA = analogRead(_analogPin1); senseA[ptr] = analogRead(_analogPin1);
senseB = analogRead(_analogPin2); senseB[ptr] = analogRead(_analogPin2);
// Check if new extremes sensor values // Check if new extremes sensor values
if (senseA > _maxCh1) if (senseA[ptr] > _maxCh1)
_maxCh1 = senseA; _maxCh1 = senseA[ptr];
else if (senseA < _minCh1) else if (senseA[ptr] < _minCh1)
_minCh1 = senseA; _minCh1 = senseA[ptr];
if (senseB > _maxCh1) if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB; _maxCh1 = senseB[ptr];
else if (senseB < _minCh2) else if (senseB[ptr] < _minCh2)
_minCh2 = senseB; _minCh2 = senseB[ptr];
// Compare with previous position // Serial.printf("senseA: "); print_arr(senseA, N); Serial.printf(" std_var: %lu\n", std_var(senseA, N));
if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon)) // Serial.printf("senseB: "); print_arr(senseB, N); Serial.printf(" std_var: %lu\n", std_var(senseB, N));
{
if (currentCheck == nCheck) if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
endFound = true; endFound = true;
else
currentCheck++;
}
else
currentCheck = 0;
// Replace previous values ptr = (ptr + 1) % N;
oldSenseA = senseA;
oldSenseB = senseB;
// Move to new position // Move to new position
currentPosition -= step; currentPosition -= step;
motor.move(currentPosition); motor.move(currentPosition);
delay(2); delay(2);
} }
_minPositionEndValue = currentPosition + (step * nCheck); _minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
Serial.println("Found second end stop : Min position = " + String(_minPositionEndValue)); Serial.println("Found second end stop : Min position = " + String(_minPositionEndValue));
delay(100); delay(100);