From 06edd6e7794edf9aff630aa7d4a9a9013debc25b Mon Sep 17 00:00:00 2001
From: YohanHadji <hadji.yohan@gmail.com>
Date: Fri, 8 Apr 2022 15:19:48 +0200
Subject: [PATCH] Finishing the servo fix

---
 .../R2Home_OBC_V1.04/main/control.hpp         | 23 ++++++++++++-------
 .../R2Home_OBC_V1.04/main/main.ino            |  2 +-
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp
index 54cf9ba..ea01c28 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp
@@ -10,6 +10,8 @@ int SERVO_MAX_C = 0;
 double Setpoint, Input, Output;
 PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
 
+unsigned long time_gain = 0; 
+
 void navigation_setup() {
   myPID.SetTunings(NKP, NKI, NKD);
   myPID.SetOutputLimits(-180, 180);
@@ -29,7 +31,7 @@ void cmpt_weight_gain() {
     int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
     total_weight = constrain(total_weight, 500, 1500); 
     SERVO_MAX_M_W = map(total_weight, 500, 1500, 2000, 1500); 
-    SERVO_MAX_C_W = map(total_weight, 500, 1500, 1500, 1250); 
+    SERVO_MAX_C_W = map(total_weight, 500, 1500, 2000, 1500); 
   }
   else {
     SERVO_MAX_M_W = SERVO_MAX_M_DEF;
@@ -38,13 +40,18 @@ void cmpt_weight_gain() {
 }
 
 void cmpt_pressure_gain(float pressure_ratio) {
-  if (AUTO_GAIN_PRESSURE) {
-    SERVO_MAX_M = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 2000); 
-    SERVO_MAX_C = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 1500);
-  }
-  else {
-    SERVO_MAX_M = SERVO_MAX_M_W;
-    SERVO_MAX_C = SERVO_MAX_C_W; 
+  if (millis()-time_gain>1000) {
+    time_gain = millis();  
+    if (AUTO_GAIN_PRESSURE) {
+      SERVO_MAX_M = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 2000); 
+      SERVO_MAX_C = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 2000);
+      Serial.print(SERVO_MAX_M_W); Serial.print(","); Serial.print(SERVO_MAX_M); Serial.print(",");
+      Serial.print(SERVO_MAX_C_W); Serial.print(","); Serial.println(SERVO_MAX_C); 
+    }
+    else {
+      SERVO_MAX_M = SERVO_MAX_M_W;
+      SERVO_MAX_C = SERVO_MAX_C_W; 
+    }
   }
 }
 
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
index e9a0452..a78e4f0 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
@@ -71,7 +71,7 @@ void getdata() {
 
 void datacmpt() {
 
-  cmpt_pressure_gain(pressure_baro); 
+  cmpt_pressure_gain(pressure_sqrt_ratio()); 
   
   if (new_cog) {
     new_cog = false;