Diff: main.cpp

Revision:

20:b142ae11a12a

Parent:

19:2ffbd2af2b7f

Child:

21:762f8769cba4

--- a/main.cpp	Wed Aug 19 14:05:22 2020 +0000
+++ b/main.cpp	Thu Aug 20 13:50:22 2020 +0000
@@ -133,7 +133,7 @@
 double Cuboid_Angle_Speed_Degrees = 0.0;
 
 // Low pass filter variables
-float t = 0.5f;//0.5f;
+float t = 0.45f; //Old 0.5f;
 
 // Flywheel Position and Velocity variables
 double Velocity_Input_Voltage = 0.0f;
@@ -154,7 +154,7 @@
 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] = {-57.1176*0.3, -2.6398*1.35}; // From Matlab
 
 // Controller Variables
 float Loop1_output; // Loop 1 controller output
@@ -203,7 +203,7 @@
 // PID (PI) Controller (My PID Controller is fine but needs clarity updates)
 PID_Cntrl  C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);   // Defining the 1st Loop Controller (PI-Part)
 PID_Cntrl  C2(Kp_2,Ki_2,Kd_2,Tf_2,Ts,uMin1,uMax1);   // Defining the PI Controller for Chase (State 2) to keep motor velocity at zero
-PID_Cntrl  C3(Kp_1*2.5,Ki_1*1.5,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
+PID_Cntrl  C3(Kp_1*3,Ki_1*2.0,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
 
 // Accelerometer and Gyroscope Filters
 IIR_filter FilterAccX(t, Ts, 1.0f);
@@ -318,16 +318,23 @@
 
         case 0: // Output of 0 Amps
             Sys_input_Amps = 0.0;
+            
+            C1.reset(0.0f);
             break;
 
         case 1: // Breaking
 
             // PI Controller
             PID_Input = 0.0f - Velocity;
-            PID_Output = C2.update(PID_Input);
-
+            //PID_Output = C2.update(PID_Input);
+            PID_Output = 0;
+            C1.reset(0.0f);
+            float Test1 = C1.reset(0.0f);
+            pc.printf("PI Outpu: %0.6f\n\r", Test1);
             // System input
-            Sys_input_Amps = PID_Output;
+            //Sys_input_Amps = PID_Output;
+            Sys_input_Amps = 0;
+            
             break;
 
         case 2: // Balancing and lift-up
@@ -343,6 +350,7 @@
 
             // System input
             Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
+            //Sys_input_Amps =  0.0f - Loop1_output - Loop2_output;
 
 
             // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement
@@ -361,6 +369,8 @@
         case 4: // Lift up when stuck
             // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement
 
+            C1.reset(0.0f);
+
             PID_Input2 = 0.0f - Velocity;
             PID_Output2 = C3.update(-1*PID_Input2);
 
@@ -375,7 +385,7 @@
             VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
             //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));
 
-            if (abs(Velocity) < 15.0f) {
+            if (abs(Velocity) < 1.0f) {
                 Sys_input_Amps = 0.0;
                 Button_Status = 2;
                 break;
@@ -404,7 +414,7 @@
     // 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, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, Button_Status);
+        pc.printf("Some Outputs: %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Button_Status);
     }
 
 
@@ -428,6 +438,7 @@
     T_Button.reset();
     if(ButtonTime > 0.05f) {
         Button_Status = Button_Status + 1;
+        C1.reset(0.0f);
         if (Button_Status > 3.0f) {
             Button_Status = 1.0;
         }