diff --git a/PIDTesting-2yoke4coil/Controller.cpp b/PIDTesting-2yoke4coil/Controller.cpp index 7f42115..919ee7e 100644 --- a/PIDTesting-2yoke4coil/Controller.cpp +++ b/PIDTesting-2yoke4coil/Controller.cpp @@ -58,10 +58,10 @@ void FullController::sendOutputs() { } void FullController::avgControl() { - float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) / 4.0; - float eCurr = AvgRef - avg; // assuming up is positive and down is negative + float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f; + float eCurr = AvgRef - avg; - avgError.eDiff = (tDiff == 0) ? 0:(eCurr - avgError.e) / tDiff; // rise over run + 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; @@ -74,7 +74,7 @@ void FullController::LRControl() { 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:(eCurr - LRDiffErr.e) / tDiff; + 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; @@ -88,7 +88,7 @@ void FullController::FBControl() { 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:(eCurr - FBDiffErr.e) / tDiff; + 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; diff --git a/PIDTesting-2yoke4coil/Controller.hpp b/PIDTesting-2yoke4coil/Controller.hpp index 2988335..eb5272d 100644 --- a/PIDTesting-2yoke4coil/Controller.hpp +++ b/PIDTesting-2yoke4coil/Controller.hpp @@ -100,6 +100,6 @@ class FullController { int16_t FRPrev; int16_t BRPrev; - unsigned int tDiff; + float tDiff; }; #endif // CONTROLLER_HPP \ No newline at end of file diff --git a/PIDTesting-2yoke4coil/IndSensorMap.hpp b/PIDTesting-2yoke4coil/IndSensorMap.hpp index 24a8b9b..0705fd9 100644 --- a/PIDTesting-2yoke4coil/IndSensorMap.hpp +++ b/PIDTesting-2yoke4coil/IndSensorMap.hpp @@ -5,11 +5,11 @@ // Inductive Sensor Mapping Struct typedef struct IndSensorMap { - double A; - double K; - double B; - double C; - double v; + float A; + float K; + float B; + float C; + float v; } IndSensorMap; class IndSensor { diff --git a/PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino b/PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino index a5566e6..0a9ad32 100644 --- a/PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino +++ b/PIDTesting-2yoke4coil/PIDTesting-2yoke4coil.ino @@ -60,12 +60,11 @@ void setup() { void loop() { if (Serial.available() > 0) { - String inputString = Serial.readStringUntil('\n'); // Read the full input - inputString.trim(); // Remove leading/trailing whitespace, including \n and \r + // this might need to be changed if we have trouble getting serial to read. + char c = Serial.read(); + while(Serial.available()) Serial.read(); // flush remaining - // determine whether control is on or off based on serial input. - if (inputString == "0") controller.outputOn=0; - else controller.outputOn=1; + controller.outputOn = (c != '0'); } tDiffMicros = micros() - tprior;