Team Virgo v3 / Orion_newPCB_test_LV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

Diff: 01_DriveTrain/pidControl.cpp

Revision:
6:690db8b5030b
Parent:
2:761e3c932ce0
diff -r 099cb2e76c7d -r 690db8b5030b 01_DriveTrain/pidControl.cpp
--- a/01_DriveTrain/pidControl.cpp	Mon Jan 25 07:28:40 2016 +0000
+++ b/01_DriveTrain/pidControl.cpp	Thu Feb 04 02:11:36 2016 +0000
@@ -4,21 +4,42 @@
 {
     for(int i=0; i<4; i++) PIDFf_gains[i]=0;
     for(int i=0; i<4; i++) {
-        for(int j=0; j<2; i++)
+        for(int j=0; j<2; j++)
             PIDFf_limits[i][j]=0;
     }
     for(int i=0; i<2; i++) output_limits[i]=0;
     for(int i=0; i<2; i++) controlError[i] =0;
     for(int i=0; i<4; i++) PIDFf_terms[i]=0;
-    Summ_term=0;
+    Summ_term=0; 
+
+    //load default control values from config file
+    PIDFf_limits[0][0] = (-1.0)*PIDFf_lim_P;
+    PIDFf_limits[0][1] = (1.0)*PIDFf_lim_P;
+
+    PIDFf_limits[1][0] = (-1.0)*PIDFf_lim_I;
+    PIDFf_limits[1][1] = (1.0)*PIDFf_lim_I;
+
+    PIDFf_limits[2][0] = (-1.0)*PIDFf_lim_D;
+    PIDFf_limits[2][1] = (1.0)*PIDFf_lim_D;
+
+    PIDFf_limits[3][0] = (-1.0)*PIDFf_lim_Ff;
+    PIDFf_limits[3][1] = (1.0)*PIDFf_lim_Ff;
+    
+    output_limits[0]=PIDFf_range_min;
+    output_limits[1]=PIDFf_range_max;
+    
+    PIDFf_gains[0]=PIDFf_kP/1000;
+    PIDFf_gains[1]=PIDFf_kI/1000;
+    PIDFf_gains[2]=PIDFf_kD/1000;
+    PIDFf_gains[3]=PIDFf_kFf/1000;
 }
 
 void pidControl::setGains(float kP,float kI, float kD, float kFf)
 {
-    PIDFf_gains[0]=kP;
-    PIDFf_gains[1]=kI;
-    PIDFf_gains[2]=kD;
-    PIDFf_gains[3]=kFf;
+    PIDFf_gains[0]=kP/1000;
+    PIDFf_gains[1]=kI/1000;
+    PIDFf_gains[2]=kD/1000;
+    PIDFf_gains[3]=kFf/1000;
 }
 
 void pidControl::setTermLimits(float P_min, float P_max, float I_min, float I_max, float D_min, float D_max, float Ff_min, float Ff_max)