Revision 24:c953b74ed88b, committed 2020-08-28

Comitter:

BoulusAJ

Date:

Fri Aug 28 09:23:40 2020 +0000

Parent:

23:b7b7271deb11

Commit message:

Prototype 2 - August 28, 2020.; Controller working well with updated parameters; State machine still needs updates

Changed in this revision

--- a/main.cpp	Fri Aug 21 16:21:12 2020 +0000
+++ b/main.cpp	Fri Aug 28 09:23:40 2020 +0000
@@ -133,7 +133,7 @@
 double Cuboid_Angle_Speed_Degrees = 0.0;
 
 // Low pass filter variables
-float t = 0.45f; //Old 0.5f;
+float t = 0.9f; //Old 0.5f;
 
 // Flywheel Position and Velocity variables
 double Velocity_Input_Voltage = 0.0f;
@@ -154,9 +154,10 @@
 float Sys_input_Amps = 0.0f;
 
 // Sate Space Controller Values
-float K_SS_Controller [2] = {-57.1176*0.3, -2.6398*1.5}; // From Matlab
-// float K_SS_Controller [2] = {-64.6865,  -4.2719}; // From Matlab
-
+//float K_SS_Controller [2] = {-57.1176*0.3, -2.6398*1.5}; // From Matlab
+ float K_SS_Controller [2] = {-64.6865,  -4.2719}; // From Matlab
+ //float K_SS_Controller [2] = {-45.7134,  -2.3733}; // From Matlab
+ //float K_SS_Controller [2] = {-27.775,  -2.034}; // From Matlab
 
 // Controller Variables
 float Loop1_output; // Loop 1 controller output
@@ -166,11 +167,11 @@
 // PID (PI Parameters)
 
 // PID 1 - Velocity control after lift-up
-//float Kp_1 = - 0.01;
-//float Ki_1 = - 0.05;
+float Kp_1 = -0.02;//- 0.01;
+float Ki_1 = -0.004;//- 0.05;
 //float Ki_1 = 0;
-float Kp_1 = - 0.09;
-float Ki_1 = - 0.09*0.5*0.5;
+//float Kp_1 = - 0.09;
+//float Ki_1 = - 0.09*0.5*0.5;
 float Kd_1 = 0; // No D-Part
 float Tf_1 = 1; // No D-Part
 
@@ -182,12 +183,12 @@
 
 // Saturation Parameters
 // PI Controller Limits
-const float uMin1 = -12.0f;
-const float uMax1= 12.0f;
+const float uMin1 = -5.0f;
+const float uMax1= 5.0f;
 
 // Cuboid Escon Input Limits in Amps
-const float uMin = -13.0f;        // Minimum Current Allowed
-const float uMax =  13.0f;        // Maximum Current Allowed
+const float uMin = -15.0f;        // Minimum Current Allowed
+const float uMax =  15.0f;        // Maximum Current Allowed
 
 // temp
 int counter = 0;
@@ -316,7 +317,7 @@
 
     // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
 
-    Cuboid_Angle_Radians =  -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond);
+    Cuboid_Angle_Radians =  -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); //- pi;
     Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
 
 
@@ -341,7 +342,7 @@
             PID_Output = 0;
             C1.reset(0.0f);
             float Test1 = C1.reset(0.0f);
-            pc.printf("PI Outpu: %0.6f\n\r", Test1);
+            //pc.printf("PI Outpu: %0.6f\n\r", Test1);
             // System input
             //Sys_input_Amps = PID_Output;
             Sys_input_Amps = 0;
@@ -364,7 +365,7 @@
             // System input
             Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
             //Sys_input_Amps =   0.0f - Loop1_output - Loop2_output;
-
+            //Sys_input_Amps = 13.0f;
 
 
             //if(++counter < 30) {
@@ -434,12 +435,12 @@
     // ----------------
 
     // Print Data // Printing Rate (in this case) is every Ts*100 = 0.007*100 = every 0.7 seconds
-    if(++k >= 100) {
-        k = 0;
-        pc.printf("Some Outputs: %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Button_Status);
-    }
+    //if(++k >= 100) {
+    //    k = 0;
+    //    pc.printf("Some Outputs: %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Button_Status);
+   // }
 
-
+pc.printf("%0.6f, %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Velocity, Button_Status);
 }
 //******************************************************************************
 //------------------ User functions like buttens handle etc. -------------------