From 7546bdea84fad035eb5dc433e5648355c43728e1 Mon Sep 17 00:00:00 2001
From: SK21 <SK21@users.noreply.github.com>
Date: Sun, 31 Mar 2024 21:25:25 -0600
Subject: [PATCH] Hz, scale PID to match

---
 Modules/Teensy Rate/RCteensy/PID.ino      | 10 +++++-----
 Modules/Teensy Rate/RCteensy/RCteensy.ino | 18 +++++++++++++-----
 Modules/Teensy Rate/RCteensy/Rate.ino     | 18 +++++++++---------
 3 files changed, 27 insertions(+), 19 deletions(-)

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 <Adafruit_SPIDevice.h>
 
 // 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
 		{