diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp index 21f0799..fff700d 100644 --- a/src/linearHallSensor.cpp +++ b/src/linearHallSensor.cpp @@ -1,5 +1,7 @@ #include "linearHallSensor.h" +#include + 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);