From 1702a2c7347a0da56ebd92d90e909babb7b0bb11 Mon Sep 17 00:00:00 2001 From: Aditya Pulipaka Date: Sun, 16 Nov 2025 10:31:46 -0600 Subject: [PATCH] commit for the morning before implementing 4 yoke --- DualYokeControl (Previous Version).ino | 50 +++++++++----------------- IndSensorMap.cpp | 13 +++++++ IndSensorMap.hpp | 22 ++++++++++++ 3 files changed, 51 insertions(+), 34 deletions(-) create mode 100644 IndSensorMap.cpp create mode 100644 IndSensorMap.hpp diff --git a/DualYokeControl (Previous Version).ino b/DualYokeControl (Previous Version).ino index 26befd8..7db7b39 100644 --- a/DualYokeControl (Previous Version).ino +++ b/DualYokeControl (Previous Version).ino @@ -1,4 +1,5 @@ #include +#include "IndSensorMap.hpp" // PIN MAPPING @@ -9,18 +10,14 @@ const int dir2Pin = 4; const int pwm2Pin = 3; const int dist2Pin = A2; - const int range2Pin = A4; const int rangePin = A3; // variables -int dist_raw, oor, t0, tcurr, tprior, telapsed, pwm, pwm2, dist2_raw, oor2; +int dist_raw, tcurr, tprior, telapsed, pwm, pwm2, oor, oor2, dist2_raw; float dist,ecurr, eprior, derror, ecum, ff,dist2,ecurr2, eprior2, derror2, ecum2, ff2; -const float sensor_gain = 10.0 / (712-200.0); -const float offset = 8; - const int CAP = 200; @@ -32,6 +29,9 @@ float MAX_INTEGRAL_TERM = 1e4; const float ref = 21.0; //14.9; const float ref2 = 22.0; //14.9; +const float refL = (ref + ref2) / 2; +const float refR = refL; + const float K_current = 1; const float ki_current = 0; @@ -79,8 +79,7 @@ void setup() { pinMode(distPin, INPUT); pinMode(rangePin, INPUT); - t0 = micros(); - tprior = t0; + tprior = micros(); ecum = 0; ecum2 = 0; @@ -118,32 +117,30 @@ void loop() { if (telapsed >= dt_micros){ // put your main code here, to run repeatedly: dist_raw = analogRead(distPin); - dist = float_map(dist_raw, 189, 950, 16, 26); // 189->950, 16->26 + if (dist_raw > 900) oor = true; + dist = ind2mm(ind0Map, dist_raw); // 189->950, 16->26 Serial.print(dist); Serial.print(", "); dist2_raw = analogRead(dist2Pin); - dist2 = float_map(dist2_raw, 189, 950, 16, 26); + if (dist2_raw > 900) oor2 = true; + dist2 = ind2mm(ind1Map, dist2_raw); Serial.print(dist2); Serial.print(", "); - oor = (dist_raw <= 195); // naive oor - oor2 = (dist2_raw <= 195); // naive oor - -// ecurr = ref - dist; - //ecurr = dist - ref; derror = ecurr - eprior; ecurr2 = ref2 - dist2; - //ecurr2 = dist2 - ref2; derror2 = ecurr2 - eprior2; if (ON) { - int pwm1 = levitate(ecurr, derror, ecum, oor); - int pwm2 = levitate2(ecurr2, derror2, ecum2, oor2); - Serial.print(pwm1); + int collective1 = levitate(ecurr, derror, ecum, oor); + int collective2 = levitate2(ecurr2, derror2, ecum2, oor2); + send_pwm1(pwm); + send_pwm2(pwm2); + Serial.print(pwm); Serial.print(", "); Serial.print(pwm2); Serial.print(", "); @@ -157,13 +154,8 @@ void loop() { Serial.print(", "); } - Serial.print(ON); - -// -// -// tprior = micros(); eprior = ecurr; eprior2 = ecurr2; @@ -195,11 +187,10 @@ int levitate(float e, float de, float ecum, int oor){ pwm = constrain(K*(e + ki*ecum + kd*de), -CAP,CAP); } - send_pwm1(pwm); return (int)pwm; } -int levitate2(float e, float de, float ecum, int oor){ +int levitate2(float e, float de, float ecunm, int oor){ if (oor){ pwm2 = 0; } @@ -218,7 +209,6 @@ int levitate2(float e, float de, float ecum, int oor){ pwm2 = constrain(K*(e + ki*ecum + kd*de), -CAP,CAP); } - send_pwm2(pwm2); return (int)pwm2; } @@ -246,11 +236,3 @@ void send_pwm2(int val){ analogWrite(pwm2Pin,abs(val)); } - -float float_map(int x, int in_min, int in_max, int out_min, int out_max){ - float top = (float)((x-in_min) * (out_max-out_min)); - return top / (float)(in_max - in_min) + (float)out_min; - - - //return dist_raw * sensor_gain + offset; -} diff --git a/IndSensorMap.cpp b/IndSensorMap.cpp new file mode 100644 index 0000000..2f492a1 --- /dev/null +++ b/IndSensorMap.cpp @@ -0,0 +1,13 @@ +#include "IndSensorMap.hpp" +#include + +// Sensor calibration data +IndSensorMap ind0Map = {-8.976076325826309, 913.5463710698101, 0.29767471011439534, 5.6686184386250025, 0.3627635461289861}; +IndSensorMap ind1Map = {-4.831976283950702, 885.9877001844566, 0.2793284618109283, 3.8852507844119217, 0.2389935455347361}; +IndSensorMap ind2Map = {-9.824360913609562, 871.4744633266955, 0.2909366235093304, 4.3307594408159495, 0.2822807132259202}; +IndSensorMap ind3Map = {-13.891292062248292, 990.6819962477331, 0.16376045588859353, -0.074904004740735, 0.17727132893449118}; + +// Convert raw analog reading to millimeters using sensor calibration +float ind2mm(IndSensorMap ind, unsigned int raw) { + return ind.C - (1.0 / ind.B) * log(pow((ind.K - ind.A) / ((float)raw - ind.A), ind.v) - 1.0); +} diff --git a/IndSensorMap.hpp b/IndSensorMap.hpp new file mode 100644 index 0000000..f38fc9b --- /dev/null +++ b/IndSensorMap.hpp @@ -0,0 +1,22 @@ +#ifndef IND_SENSOR_MAP_HPP +#define IND_SENSOR_MAP_HPP + +// Inductive Sensor Mapping Struct +typedef struct IndSensorMap { + double A; + double K; + double B; + double C; + double v; +} IndSensorMap; + +// Predefined sensor calibrations +extern IndSensorMap ind0Map; +extern IndSensorMap ind1Map; +extern IndSensorMap ind2Map; +extern IndSensorMap ind3Map; + +// Convert raw analog reading to millimeters using sensor calibration +float ind2mm(IndSensorMap ind, unsigned int raw); + +#endif // IND_SENSOR_MAP_HPP