diff --git a/DualYokeControl (Previous Version).ino b/DualYokeControl (Previous Version).ino index cb97e69..5827d53 100644 --- a/DualYokeControl (Previous Version).ino +++ b/DualYokeControl (Previous Version).ino @@ -3,9 +3,6 @@ // PIN MAPPING -#define indL A0 -#define indR A1 - #define dirFR 2 #define pwmFR 3 #define dirBR 4 @@ -17,26 +14,16 @@ // variables -int dist_raw, tprior, telapsed, pwm, pwm2, oor, oor2, dist2_raw; +int dist_raw, tprior, telapsed, pwm, pwm2, dist2_raw; +bool oor, oor2; float dist,ecurr, eprior, derror, ecum, ff,dist2,ecurr2, eprior2, derror2, ecum2, ff2; #define CAP 200 // CONTROLLER CONSTANTS - -float ki, kd, K; 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; - //// FOR MC 1 //const float K_f = 50; // gain for when we want to fall (ref > dist or error > 0) //const float ki_f = 0.01; @@ -46,24 +33,44 @@ const float ki_current = 0; //const float ki_a = ki_f; //const float kd_a = 10; //30; -// FOR MC 1 -const float K_f = 40; // gain for when we want to fall (ref > dist or error > 0) -const float ki_f = 0.01; -const float kd_f = 7; // 25 +typedef struct Constants { + float K; + float ki; + float kd; +} Constants; -const float K_a = 20; -const float ki_a = ki_f; -const float kd_a = 20; //30; +typedef struct K_MAP { + Constants falling; + Constants attracting; +} K_MAP; -// FOR MC 2 +typedef struct Collective { + K_MAP constants; + float e; + float eDiff; + float eInt; + float ref; +} Collective; -const float K2_f = K_f;// gain for when we want to fall (ref > dist or error > 0) -const float ki2_f = ki_f; -const float kd2_f = kd_f; +Collective collLeft = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 21.0}; +Collective collRight = {{{40, 0.01, 7}, {20, 0.01, 20}}, 0, 0, 0, 22.0}; -const float K2_a = K_a; -const float ki2_a = ki_a; -const float kd2_a = kd_a; +int levCollective(Collective collective, bool oor){ + if (oor){ + pwm = 0; + } + else{ + Constants pidConsts; + + // this means that dist > ref so we gotta attract to track now vv + if (collective.e < 0) pidConsts = collective.constants.attracting; + // this is falling vv + else pidConsts = collective.constants.falling; + + pwm = constrain(pidConsts.K*(collective.e + pidConsts.ki*collective.eInt + pidConsts.kd*collective.eDiff), -CAP,CAP); + } + return (int)pwm; +} #define sampling_rate 1000 const int dt_micros = 1e6/sampling_rate; @@ -86,15 +93,12 @@ void setup() { // ATTRACT IS B // REPEL IS A - //when error is negative, I want to attract. send_pwmFL(0); send_pwmFR(0); } - - void loop() { if (Serial.available() > 0) { String inputString = Serial.readStringUntil('\n'); // Read the full input @@ -113,6 +117,10 @@ void loop() { if (telapsed >= dt_micros){ // put your main code here, to run repeatedly: + indL.read(); + indR.read(); + indF.read(); + indB.read(); dist_raw = analogRead(indL); if (dist_raw > 870) oor = true; dist = indToMM(ind0Map, dist_raw); // 189->950, 16->26 @@ -138,8 +146,7 @@ void loop() { if (ON) { - int collective1 = levitate(ecurr, derror, ecum, oor); - int collective2 = levitate2(ecurr2, derror2, ecum2, oor2); + int collective1 = send_pwmFL(pwm); send_pwmFR(pwm2); Serial.print(pwm); @@ -170,51 +177,6 @@ void loop() { //Serial.println(telapsed); } -int levitate(float e, float de, float ecum, int oor){ - if (oor){ - pwm = 0; - } - else{ - if (e < 0) { // this means that dist > ref so we gotta attract to track now - kd = kd_a; - ki = ki_a; - K = K_a; - } - else{ - kd = kd_f; - ki = ki_f; - K = K_f; - } - - pwm = constrain(K*(e + ki*ecum + kd*de), -CAP,CAP); - - } - return (int)pwm; -} - -int levitate2(float e, float de, float ecunm, int oor){ - if (oor){ - pwm2 = 0; - } - else{ - if (e < 0) { // this means that dist > ref so we gotta attract to track now - kd = kd2_a; - ki = ki2_a; - K = K2_a; - } - else{ - kd = kd2_f; - ki = ki2_f; - K = K2_f; - } - - pwm2 = constrain(K*(e + ki*ecum + kd*de), -CAP,CAP); - - } - return (int)pwm2; - -} - void send_pwmFL(int val){ if (val > 0) { digitalWrite(dirFL, LOW); diff --git a/IndSensorMap.cpp b/IndSensorMap.cpp index d77439b..c95108f 100644 --- a/IndSensorMap.cpp +++ b/IndSensorMap.cpp @@ -1,4 +1,5 @@ #include "IndSensorMap.hpp" +#include #include // Sensor calibration data @@ -7,7 +8,24 @@ IndSensorMap ind1Map = {-4.831976283950702, 885.9877001844566, 0.279328461810928 IndSensorMap ind2Map = {-9.824360913609562, 871.4744633266955, 0.2909366235093304, 4.3307594408159495, 0.2822807132259202}; IndSensorMap ind3Map = {-13.891292062248292, 990.6819962477331, 0.16376045588859353, -0.074904004740735, 0.17727132893449118}; +// IndSensor class implementation +IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin) + : consts(calibration), pin(analogPin), oor(false) {} + // Convert raw analog reading to millimeters using sensor calibration -float indToMM(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); +float IndSensor::toMM(unsigned int raw) { + return consts.C - (1.0 / consts.B) * log(pow((consts.K - consts.A) / ((float)raw - consts.A), consts.v) - 1.0); } + +// Read sensor directly from pin and convert to millimeters +float IndSensor::read() { + unsigned int raw = analogRead(pin); + oor = (raw == 0 || raw > 870); // Update out-of-range flag + return toMM(raw); +} + +// Predefined sensor instances +IndSensor indL(ind1Map, A0); +IndSensor indR(ind0Map, A1); +IndSensor indF(ind3Map, A5); +IndSensor indB(ind2Map, A4); diff --git a/IndSensorMap.hpp b/IndSensorMap.hpp index ff70533..f6d0d26 100644 --- a/IndSensorMap.hpp +++ b/IndSensorMap.hpp @@ -10,13 +10,28 @@ typedef struct IndSensorMap { double v; } IndSensorMap; -// Predefined sensor calibrations -extern IndSensorMap ind0Map; -extern IndSensorMap ind1Map; -extern IndSensorMap ind2Map; -extern IndSensorMap ind3Map; +class IndSensor { +public: + bool oor; + int mmVal; -// Convert raw analog reading to millimeters using sensor calibration -float indToMM(IndSensorMap ind, unsigned int raw); + // Constructor + IndSensor(IndSensorMap calibration, uint8_t analogPin); + // Read sensor directly from pin and convert to millimeters + float read(); + +private: + IndSensorMap consts; + uint8_t pin; + + // Read sensor and convert to millimeters + float toMM(unsigned int raw); +}; + +// sensor instances +extern IndSensor indL; +extern IndSensor indR; +extern IndSensor indF; +extern IndSensor indB; #endif // IND_SENSOR_MAP_HPP