diff --git a/DistanceReporting/DistanceReporting.ino b/DistanceReporting/DistanceReporting.ino new file mode 100644 index 0000000..a5dbdd7 --- /dev/null +++ b/DistanceReporting/DistanceReporting.ino @@ -0,0 +1,6 @@ +#include + +void setup() { + Serial.begin(57600); + +} \ No newline at end of file diff --git a/DistanceReporting/IndSensorMap.cpp b/DistanceReporting/IndSensorMap.cpp new file mode 120000 index 0000000..82f0b2a --- /dev/null +++ b/DistanceReporting/IndSensorMap.cpp @@ -0,0 +1 @@ +../lib/IndSensorMap.cpp \ No newline at end of file diff --git a/DistanceReporting/IndSensorMap.hpp b/DistanceReporting/IndSensorMap.hpp new file mode 120000 index 0000000..b101f1f --- /dev/null +++ b/DistanceReporting/IndSensorMap.hpp @@ -0,0 +1 @@ +../lib/IndSensorMap.hpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.cpp b/PIDTesting-2yoke4coil/Controller.cpp deleted file mode 100644 index 919ee7e..0000000 --- a/PIDTesting-2yoke4coil/Controller.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include "Controller.hpp" -#include - -// CONTROLLER CONSTANTS -float MAX_INTEGRAL_TERM = 1e4; - -void FullController::update(float tDiff) { - this->tDiff = tDiff; // store time step for use in differential and integral portions - - Left.readMM(); - Right.readMM(); - Front.readMM(); - Back.readMM(); // read and update dists/oor for all sensors. - - oor = Left.oor || Right.oor || Front.oor || Back.oor; - - avgControl(); - LRControl(); // run pwm functions. - FBControl(); - - int16_t flTarget = avgPWM + LDiffPWM + FDiffPWM; - int16_t blTarget = avgPWM + LDiffPWM + BDiffPWM; - int16_t frTarget = avgPWM + RDiffPWM + FDiffPWM; - int16_t brTarget = avgPWM + RDiffPWM + BDiffPWM; - - FLPWM = slewLimit(flTarget, FLPrev); - BLPWM = slewLimit(blTarget, BLPrev); - FRPWM = slewLimit(frTarget, FRPrev); - BRPWM = slewLimit(brTarget, BRPrev); - - FLPrev = FLPWM; - BLPrev = BLPWM; - FRPrev = FRPWM; - BRPrev = BRPWM; -} - -void FullController::zeroPWMs() { - FLPWM = 0; - BLPWM = 0; - FRPWM = 0; - BRPWM = 0; -} - -void FullController::sendOutputs() { - if (!outputOn) { - zeroPWMs(); - } - - // The following assumes 0 direction drives repulsion and 1 direction drives attraction. - digitalWrite(dirFL, FLPWM < 0); - analogWrite(pwmFL, abs(FLPWM)); - digitalWrite(dirBL, BLPWM < 0); - analogWrite(pwmBL, abs(BLPWM)); - digitalWrite(dirFR, FRPWM < 0); - analogWrite(pwmFR, abs(FRPWM)); - digitalWrite(dirBR, BRPWM < 0); - analogWrite(pwmBR, abs(BRPWM)); -} - -void FullController::avgControl() { - float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f; - float eCurr = AvgRef - avg; - - avgError.eDiff = (tDiff == 0.0) ? 0:(eCurr - avgError.e) / tDiff; // rise over run - avgError.eInt += eCurr * tDiff; - avgError.eInt = constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); - avgError.e = eCurr; - - avgPWM = pwmFunc(avgConsts, avgError); -} - -void FullController::LRControl() { - float diff = Right.mmVal - Left.mmVal; // how far above the right is the left? - float eCurr = diff - LRDiffRef; // how different is that from the reference? positive -> Left repels, Right attracts. - K_MAP rConsts = {LConsts.attracting, LConsts.repelling}; // apply attracting to repelling and vice versa. - - LRDiffErr.eDiff = (tDiff == 0.0) ? 0:(eCurr - LRDiffErr.e) / tDiff; - LRDiffErr.eInt += eCurr * tDiff; - LRDiffErr.eInt = constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); - LRDiffErr.e = eCurr; - - LDiffPWM = pwmFunc(LConsts, LRDiffErr); - RDiffPWM = -pwmFunc(rConsts, LRDiffErr); -} - -void FullController::FBControl() { - float diff = Back.mmVal - Front.mmVal; // how far above the back is the front? - float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front must repel, Back must attract - K_MAP bConsts = {FConsts.attracting, FConsts.repelling}; - - FBDiffErr.eDiff = (tDiff == 0.0) ? 0:(eCurr - FBDiffErr.e) / tDiff; - FBDiffErr.eInt += eCurr * tDiff; - FBDiffErr.eInt = constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); - FBDiffErr.e = eCurr; - - FDiffPWM = pwmFunc(FConsts, FBDiffErr); - BDiffPWM = -pwmFunc(bConsts, FBDiffErr); -} - -int16_t FullController::pwmFunc(K_MAP consts, Errors errs) { - if (oor) return 0; - Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling; - return (int)constrain(constants.K*(errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff), -(float)CAP,(float)CAP); -} - -int16_t FullController::slewLimit(int16_t target, int16_t prev) { - int16_t maxChange = (int16_t)(slewRateLimit * tDiff); - int16_t delta = target - prev; - if (abs(delta) <= maxChange) return target; - return prev + (delta > 0 ? maxChange : -maxChange); -} - -void FullController::report() { - Serial.print("SENSORS - Left: "); - Serial.print(Left.mmVal); - Serial.print("mm, Right: "); - Serial.print(Right.mmVal); - Serial.print("mm, Front: "); - Serial.print(Front.mmVal); - Serial.print("mm, Back: "); - Serial.print(Back.mmVal); - Serial.print("mm,\n"); - - Serial.print("PWMS - FL_PWM: "); - Serial.print(FLPWM); - Serial.print(", BL_PWM: "); - Serial.print(BLPWM); - Serial.print("FR_PWM: "); - Serial.print(FRPWM); - Serial.print("BR_PWM: "); - Serial.print(BRPWM); - Serial.print("\n"); - - Serial.print("CONTROL ON - "); - Serial.print(outputOn); - Serial.print("\n"); -} \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.cpp b/PIDTesting-2yoke4coil/Controller.cpp new file mode 120000 index 0000000..cc87053 --- /dev/null +++ b/PIDTesting-2yoke4coil/Controller.cpp @@ -0,0 +1 @@ +../lib/Controller.cpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/PIDTesting-2yoke4coil/Controller.hpp deleted file mode 100644 index eb5272d..0000000 --- a/PIDTesting-2yoke4coil/Controller.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef CONTROLLER_HPP -#define CONTROLLER_HPP - -#include -#include "IndSensorMap.hpp" - -// PIN MAPPING -#define dirFR 2 -#define pwmFR 3 -#define dirBR 4 -#define pwmBR 5 -#define pwmFL 6 -#define dirFL 7 -#define dirBL 8 -#define pwmBL 9 - -#define CAP 200 - -typedef struct Constants { - float K; - float ki; - float kd; -} Constants; - -typedef struct K_MAP { - Constants repelling; - Constants attracting; -} K_MAP; - -typedef struct FullConsts { - K_MAP avg; - K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives. - K_MAP fColl; -} FullConsts; - -typedef struct Errors { - float e; - float eDiff; - float eInt; -} Errors; - -class FullController { - public: - bool oor; - bool outputOn; - - FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, - FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef, float slewRate) - : Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef), - FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl), - FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}), - FBDiffErr({0,0,0}), oor(false), outputOn(false), - FLPrev(0), BLPrev(0), FRPrev(0), BRPrev(0), slewRateLimit(slewRate) {} - - void update(float tDiff); - void zeroPWMs(); - void sendOutputs(); - void report(); - - private: - void avgControl(); - void LRControl(); - void FBControl(); - int16_t pwmFunc(K_MAP consts, Errors errs); - int16_t slewLimit(int16_t target, int16_t prev); - - IndSensor& Front; - IndSensor& Back; - IndSensor& Right; - IndSensor& Left; - - K_MAP avgConsts; - K_MAP LConsts; - K_MAP FConsts; - - Errors avgError; - Errors LRDiffErr; - Errors FBDiffErr; - - float AvgRef; - float LRDiffRef; - float FBDiffRef; - float slewRateLimit; - - int16_t avgPWM; - int16_t LDiffPWM; - int16_t RDiffPWM; - int16_t FDiffPWM; - int16_t BDiffPWM; - // Initially, I was going to make the Right and Back just the negatives, - // but having separate control functions running for each of these outputs might prove useful. - - int16_t FLPWM; - int16_t BLPWM; - int16_t FRPWM; - int16_t BRPWM; - - int16_t FLPrev; - int16_t BLPrev; - int16_t FRPrev; - int16_t BRPrev; - - float tDiff; -}; -#endif // CONTROLLER_HPP \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/PIDTesting-2yoke4coil/Controller.hpp new file mode 120000 index 0000000..f5297ba --- /dev/null +++ b/PIDTesting-2yoke4coil/Controller.hpp @@ -0,0 +1 @@ +../lib/Controller.hpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.cpp b/PIDTesting-2yoke4coil/IndSensorMap.cpp deleted file mode 100644 index fdae3cd..0000000 --- a/PIDTesting-2yoke4coil/IndSensorMap.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "IndSensorMap.hpp" -#include -#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}; - -// 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 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::readMM() { - unsigned int raw = analogRead(pin); - oor = (raw == 0 || raw > 870); // Update out-of-range flag - mmVal = toMM(raw); - return mmVal; -} - -// Predefined sensor instances -IndSensor indL(ind1Map, A0); -IndSensor indR(ind0Map, A1); -IndSensor indF(ind3Map, A5); -IndSensor indB(ind2Map, A4); diff --git a/PIDTesting-2yoke4coil/IndSensorMap.cpp b/PIDTesting-2yoke4coil/IndSensorMap.cpp new file mode 120000 index 0000000..82f0b2a --- /dev/null +++ b/PIDTesting-2yoke4coil/IndSensorMap.cpp @@ -0,0 +1 @@ +../lib/IndSensorMap.cpp \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PIDTesting-2yoke4coil/IndSensorMap.hpp deleted file mode 100644 index 0705fd9..0000000 --- a/PIDTesting-2yoke4coil/IndSensorMap.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef IND_SENSOR_MAP_HPP -#define IND_SENSOR_MAP_HPP - -#include - -// Inductive Sensor Mapping Struct -typedef struct IndSensorMap { - float A; - float K; - float B; - float C; - float v; -} IndSensorMap; - -class IndSensor { - public: - bool oor; - float mmVal; - - // Constructor - IndSensor(IndSensorMap calibration, uint8_t analogPin); - // Read sensor directly from pin and convert to millimeters - float readMM(); - - private: - IndSensorMap consts; - uint8_t pin; - - // helper function to convert analog reading 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 diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PIDTesting-2yoke4coil/IndSensorMap.hpp new file mode 120000 index 0000000..b101f1f --- /dev/null +++ b/PIDTesting-2yoke4coil/IndSensorMap.hpp @@ -0,0 +1 @@ +../lib/IndSensorMap.hpp \ No newline at end of file diff --git a/lib/Controller.cpp b/lib/Controller.cpp new file mode 100644 index 0000000..919ee7e --- /dev/null +++ b/lib/Controller.cpp @@ -0,0 +1,137 @@ +#include "Controller.hpp" +#include + +// CONTROLLER CONSTANTS +float MAX_INTEGRAL_TERM = 1e4; + +void FullController::update(float tDiff) { + this->tDiff = tDiff; // store time step for use in differential and integral portions + + Left.readMM(); + Right.readMM(); + Front.readMM(); + Back.readMM(); // read and update dists/oor for all sensors. + + oor = Left.oor || Right.oor || Front.oor || Back.oor; + + avgControl(); + LRControl(); // run pwm functions. + FBControl(); + + int16_t flTarget = avgPWM + LDiffPWM + FDiffPWM; + int16_t blTarget = avgPWM + LDiffPWM + BDiffPWM; + int16_t frTarget = avgPWM + RDiffPWM + FDiffPWM; + int16_t brTarget = avgPWM + RDiffPWM + BDiffPWM; + + FLPWM = slewLimit(flTarget, FLPrev); + BLPWM = slewLimit(blTarget, BLPrev); + FRPWM = slewLimit(frTarget, FRPrev); + BRPWM = slewLimit(brTarget, BRPrev); + + FLPrev = FLPWM; + BLPrev = BLPWM; + FRPrev = FRPWM; + BRPrev = BRPWM; +} + +void FullController::zeroPWMs() { + FLPWM = 0; + BLPWM = 0; + FRPWM = 0; + BRPWM = 0; +} + +void FullController::sendOutputs() { + if (!outputOn) { + zeroPWMs(); + } + + // The following assumes 0 direction drives repulsion and 1 direction drives attraction. + digitalWrite(dirFL, FLPWM < 0); + analogWrite(pwmFL, abs(FLPWM)); + digitalWrite(dirBL, BLPWM < 0); + analogWrite(pwmBL, abs(BLPWM)); + digitalWrite(dirFR, FRPWM < 0); + analogWrite(pwmFR, abs(FRPWM)); + digitalWrite(dirBR, BRPWM < 0); + analogWrite(pwmBR, abs(BRPWM)); +} + +void FullController::avgControl() { + float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f; + float eCurr = AvgRef - avg; + + avgError.eDiff = (tDiff == 0.0) ? 0:(eCurr - avgError.e) / tDiff; // rise over run + avgError.eInt += eCurr * tDiff; + avgError.eInt = constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + avgError.e = eCurr; + + avgPWM = pwmFunc(avgConsts, avgError); +} + +void FullController::LRControl() { + float diff = Right.mmVal - Left.mmVal; // how far above the right is the left? + float eCurr = diff - LRDiffRef; // how different is that from the reference? positive -> Left repels, Right attracts. + K_MAP rConsts = {LConsts.attracting, LConsts.repelling}; // apply attracting to repelling and vice versa. + + LRDiffErr.eDiff = (tDiff == 0.0) ? 0:(eCurr - LRDiffErr.e) / tDiff; + LRDiffErr.eInt += eCurr * tDiff; + LRDiffErr.eInt = constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + LRDiffErr.e = eCurr; + + LDiffPWM = pwmFunc(LConsts, LRDiffErr); + RDiffPWM = -pwmFunc(rConsts, LRDiffErr); +} + +void FullController::FBControl() { + float diff = Back.mmVal - Front.mmVal; // how far above the back is the front? + float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front must repel, Back must attract + K_MAP bConsts = {FConsts.attracting, FConsts.repelling}; + + FBDiffErr.eDiff = (tDiff == 0.0) ? 0:(eCurr - FBDiffErr.e) / tDiff; + FBDiffErr.eInt += eCurr * tDiff; + FBDiffErr.eInt = constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + FBDiffErr.e = eCurr; + + FDiffPWM = pwmFunc(FConsts, FBDiffErr); + BDiffPWM = -pwmFunc(bConsts, FBDiffErr); +} + +int16_t FullController::pwmFunc(K_MAP consts, Errors errs) { + if (oor) return 0; + Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling; + return (int)constrain(constants.K*(errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff), -(float)CAP,(float)CAP); +} + +int16_t FullController::slewLimit(int16_t target, int16_t prev) { + int16_t maxChange = (int16_t)(slewRateLimit * tDiff); + int16_t delta = target - prev; + if (abs(delta) <= maxChange) return target; + return prev + (delta > 0 ? maxChange : -maxChange); +} + +void FullController::report() { + Serial.print("SENSORS - Left: "); + Serial.print(Left.mmVal); + Serial.print("mm, Right: "); + Serial.print(Right.mmVal); + Serial.print("mm, Front: "); + Serial.print(Front.mmVal); + Serial.print("mm, Back: "); + Serial.print(Back.mmVal); + Serial.print("mm,\n"); + + Serial.print("PWMS - FL_PWM: "); + Serial.print(FLPWM); + Serial.print(", BL_PWM: "); + Serial.print(BLPWM); + Serial.print("FR_PWM: "); + Serial.print(FRPWM); + Serial.print("BR_PWM: "); + Serial.print(BRPWM); + Serial.print("\n"); + + Serial.print("CONTROL ON - "); + Serial.print(outputOn); + Serial.print("\n"); +} \ No newline at end of file diff --git a/lib/Controller.hpp b/lib/Controller.hpp new file mode 100644 index 0000000..eb5272d --- /dev/null +++ b/lib/Controller.hpp @@ -0,0 +1,105 @@ +#ifndef CONTROLLER_HPP +#define CONTROLLER_HPP + +#include +#include "IndSensorMap.hpp" + +// PIN MAPPING +#define dirFR 2 +#define pwmFR 3 +#define dirBR 4 +#define pwmBR 5 +#define pwmFL 6 +#define dirFL 7 +#define dirBL 8 +#define pwmBL 9 + +#define CAP 200 + +typedef struct Constants { + float K; + float ki; + float kd; +} Constants; + +typedef struct K_MAP { + Constants repelling; + Constants attracting; +} K_MAP; + +typedef struct FullConsts { + K_MAP avg; + K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives. + K_MAP fColl; +} FullConsts; + +typedef struct Errors { + float e; + float eDiff; + float eInt; +} Errors; + +class FullController { + public: + bool oor; + bool outputOn; + + FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, + FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef, float slewRate) + : Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef), + FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl), + FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}), + FBDiffErr({0,0,0}), oor(false), outputOn(false), + FLPrev(0), BLPrev(0), FRPrev(0), BRPrev(0), slewRateLimit(slewRate) {} + + void update(float tDiff); + void zeroPWMs(); + void sendOutputs(); + void report(); + + private: + void avgControl(); + void LRControl(); + void FBControl(); + int16_t pwmFunc(K_MAP consts, Errors errs); + int16_t slewLimit(int16_t target, int16_t prev); + + IndSensor& Front; + IndSensor& Back; + IndSensor& Right; + IndSensor& Left; + + K_MAP avgConsts; + K_MAP LConsts; + K_MAP FConsts; + + Errors avgError; + Errors LRDiffErr; + Errors FBDiffErr; + + float AvgRef; + float LRDiffRef; + float FBDiffRef; + float slewRateLimit; + + int16_t avgPWM; + int16_t LDiffPWM; + int16_t RDiffPWM; + int16_t FDiffPWM; + int16_t BDiffPWM; + // Initially, I was going to make the Right and Back just the negatives, + // but having separate control functions running for each of these outputs might prove useful. + + int16_t FLPWM; + int16_t BLPWM; + int16_t FRPWM; + int16_t BRPWM; + + int16_t FLPrev; + int16_t BLPrev; + int16_t FRPrev; + int16_t BRPrev; + + float tDiff; +}; +#endif // CONTROLLER_HPP \ No newline at end of file diff --git a/lib/IndSensorMap.cpp b/lib/IndSensorMap.cpp new file mode 100644 index 0000000..fdae3cd --- /dev/null +++ b/lib/IndSensorMap.cpp @@ -0,0 +1,32 @@ +#include "IndSensorMap.hpp" +#include +#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}; + +// 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 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::readMM() { + unsigned int raw = analogRead(pin); + oor = (raw == 0 || raw > 870); // Update out-of-range flag + mmVal = toMM(raw); + return mmVal; +} + +// Predefined sensor instances +IndSensor indL(ind1Map, A0); +IndSensor indR(ind0Map, A1); +IndSensor indF(ind3Map, A5); +IndSensor indB(ind2Map, A4); diff --git a/lib/IndSensorMap.hpp b/lib/IndSensorMap.hpp new file mode 100644 index 0000000..0705fd9 --- /dev/null +++ b/lib/IndSensorMap.hpp @@ -0,0 +1,39 @@ +#ifndef IND_SENSOR_MAP_HPP +#define IND_SENSOR_MAP_HPP + +#include + +// Inductive Sensor Mapping Struct +typedef struct IndSensorMap { + float A; + float K; + float B; + float C; + float v; +} IndSensorMap; + +class IndSensor { + public: + bool oor; + float mmVal; + + // Constructor + IndSensor(IndSensorMap calibration, uint8_t analogPin); + // Read sensor directly from pin and convert to millimeters + float readMM(); + + private: + IndSensorMap consts; + uint8_t pin; + + // helper function to convert analog reading 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