diff --git a/Modules/Teensy Rate/RCteensy/PID.ino b/Modules/Teensy Rate/RCteensy/PID.ino index 73e9910b..4943b8f7 100644 --- a/Modules/Teensy Rate/RCteensy/PID.ino +++ b/Modules/Teensy Rate/RCteensy/PID.ino @@ -3,7 +3,7 @@ uint32_t LastCheck[MaxProductCount]; const double SampleTime = 50; -const double Deadband = 0.04; // % error below which no adjustment is made +const double Deadband = 0.02; // % error below which no adjustment is made const double BrakePoint = 0.20; // % error below which reduced adjustment is used const double BrakeSet = 0.75; // low adjustment rate double SF; // Settings Factor used to reduce adjustment when close to set rate @@ -87,10 +87,10 @@ int PIDmotor(byte ID) // check deadband if (abs(RateError) > Deadband * Sensor[ID].TargetUPM) { - IntegralSum[ID] += Sensor[ID].KI * RateError; + IntegralSum[ID] += Sensor[ID].KI * RateError / 1000.0; IntegralSum[ID] *= (Sensor[ID].KI > 0); // zero out if not using KI - DifValue = Sensor[ID].KD * (LastUPM[ID] - Sensor[ID].UPM); + DifValue = Sensor[ID].KD * (LastUPM[ID] - Sensor[ID].UPM) * 10.0; Result += Sensor[ID].KP * SF * RateError + IntegralSum[ID] + DifValue; if (Result > Sensor[ID].MaxPWM) Result = Sensor[ID].MaxPWM; @@ -144,10 +144,10 @@ int PIDvalve(byte ID) // check deadband if (abs(RateError) > Deadband * Sensor[ID].TargetUPM) { - IntegralSum[ID] += Sensor[ID].KI * RateError; + IntegralSum[ID] += Sensor[ID].KI * RateError / 1000.0; IntegralSum[ID] *= (Sensor[ID].KI > 0); // zero out if not using KI - DifValue = Sensor[ID].KD * (LastUPM[ID] - Sensor[ID].UPM); + DifValue = Sensor[ID].KD * (LastUPM[ID] - Sensor[ID].UPM) * 10.0; Result = Sensor[ID].MinPWM + Sensor[ID].KP * SF * RateError + IntegralSum[ID] + DifValue; bool IsPositive = (Result > 0); diff --git a/Modules/Teensy Rate/RCteensy/RCteensy.ino b/Modules/Teensy Rate/RCteensy/RCteensy.ino index c4ed645b..e41d5862 100644 --- a/Modules/Teensy Rate/RCteensy/RCteensy.ino +++ b/Modules/Teensy Rate/RCteensy/RCteensy.ino @@ -16,8 +16,8 @@ #include // rate control with Teensy 4.1 -# define InoDescription "RCteensy : 30-Mar-2024" -const uint16_t InoID = 30034; // change to send defaults to eeprom, ddmmy, no leading 0 +# define InoDescription "RCteensy : 31-Mar-2024" +const uint16_t InoID = 31034; // change to send defaults to eeprom, ddmmy, no leading 0 const uint8_t InoType = 1; // 0 - Teensy AutoSteer, 1 - Teensy Rate, 2 - Nano Rate, 3 - Nano SwitchBox, 4 - ESP Rate #define MaxReadBuffer 100 // bytes @@ -241,8 +241,10 @@ elapsedMicros LoopTmr; byte ReadReset; uint32_t MaxLoopTime; -//double debug1; -//double debug2; +double debug1; +double debug2; +double debug3; +double debug4; void Blink() { @@ -265,7 +267,13 @@ void Blink() //Serial.print(debug1); //Serial.print(", "); - //Serial.print(MDL.WorkPin); + //Serial.print(debug2); + + //Serial.print(", "); + //Serial.print(debug3); + + //Serial.print(", "); + //Serial.print(debug4); Serial.println(""); diff --git a/Modules/Teensy Rate/RCteensy/Rate.ino b/Modules/Teensy Rate/RCteensy/Rate.ino index 7b1497b7..e8683768 100644 --- a/Modules/Teensy Rate/RCteensy/Rate.ino +++ b/Modules/Teensy Rate/RCteensy/Rate.ino @@ -8,7 +8,7 @@ byte DurCount[2]; uint32_t LastPulse[MaxProductCount]; uint32_t CurrentDuration; -double PPM[MaxProductCount]; // pulse per minute * 100 +double Hz[MaxProductCount]; double Osum[MaxProductCount]; double Omax[MaxProductCount]; double Omin[MaxProductCount]; @@ -162,11 +162,11 @@ void GetUPM() if (CurrentDuration == 0) { - PPM[ID] = 0; + Hz[ID] = 0; } else { - PPM[ID] = (double)(6000000000.0 / CurrentDuration); + Hz[ID] = (double)(1000000.0 / CurrentDuration); } LastPulse[ID] = millis(); @@ -175,23 +175,23 @@ void GetUPM() // check for no flow if (millis() - LastPulse[ID] > 4000) { - PPM[ID] = 0; + Hz[ID] = 0; Osum[ID] = 0; Oave[ID] = 0; Ocount[ID] = 0; } // olympic average - Osum[ID] += PPM[ID]; - if (Omax[ID] < PPM[ID]) Omax[ID] = PPM[ID]; - if (Omin[ID] > PPM[ID]) Omin[ID] = PPM[ID]; + Osum[ID] += Hz[ID]; + if (Omax[ID] < Hz[ID]) Omax[ID] = Hz[ID]; + if (Omin[ID] > Hz[ID]) Omin[ID] = Hz[ID]; Ocount[ID]++; if (Ocount[ID] > 4) { Osum[ID] -= Omax[ID]; Osum[ID] -= Omin[ID]; - Oave[ID] = Osum[ID] / 300.0; // divide by 3 samples and divide by 100 for decimal place + Oave[ID] = Osum[ID] / 3.0; // divide by 3 samples Osum[ID] = 0; Omax[ID] = 0; Omin[ID] = 5000000.0; @@ -201,7 +201,7 @@ void GetUPM() // units per minute if (Sensor[ID].MeterCal > 0) { - Sensor[ID].UPM = Oave[ID] / Sensor[ID].MeterCal; + Sensor[ID].UPM = (Oave[ID] * 60.0) / Sensor[ID].MeterCal; } else {