Diff: main.cpp

Revision:

23:b7b7271deb11

Parent:

21:762f8769cba4

Child:

24:c953b74ed88b

--- a/main.cpp	Fri Aug 21 14:55:17 2020 +0000
+++ b/main.cpp	Fri Aug 21 16:21:12 2020 +0000
@@ -154,8 +154,8 @@
 float Sys_input_Amps = 0.0f;
 
 // Sate Space Controller Values
-//float K_SS_Controller [2] = {-57.1176*0.3, -2.6398*1.35}; // 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
 
 
 // Controller Variables
@@ -166,12 +166,14 @@
 // PID (PI Parameters)
 
 // PID 1 - Velocity control after lift-up
-float Kp_1 = - 0.01;
-float Ki_1 = - 0.05;
+//float Kp_1 = - 0.01;
+//float Ki_1 = - 0.05;
 //float Ki_1 = 0;
-
+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
+
 // Controller Loop (PI-Part) in Case 2 (breaking case)
 float Kp_2 = 0.25/4.0;
 float Ki_2 = 0.25/4.0;
@@ -180,12 +182,12 @@
 
 // Saturation Parameters
 // PI Controller Limits
-const float uMin1 = -15.0f;
-const float uMax1= 15.0f;
+const float uMin1 = -12.0f;
+const float uMax1= 12.0f;
 
 // Cuboid Escon Input Limits in Amps
-const float uMin = -15.0f;        // Minimum Current Allowed
-const float uMax =  15.0f;        // Maximum Current Allowe
+const float uMin = -13.0f;        // Minimum Current Allowed
+const float uMax =  13.0f;        // Maximum Current Allowed
 
 // temp
 int counter = 0;
@@ -273,7 +275,7 @@
     C3.reset(0.0f);
 
     pc.printf("Hello World!\n\r");
-    wait(5);
+    wait(1);
     // Control Loop Interrupt at Ts, 200Hz Rate
     ControllerLoopTimer.attach(&updateControllers, Ts);
     Button.fall(&pressed);          // attach key pressed function
@@ -325,9 +327,9 @@
     switch(Button_Status) {
 
         case 0: // Output of 0 Amps
+            
             Sys_input_Amps = 0.0;
-
-            counter = 0;
+            //counter = 0;
             C1.reset(0.0f);
             break;
 
@@ -344,15 +346,12 @@
             //Sys_input_Amps = PID_Output;
             Sys_input_Amps = 0;
 
-            counter = 0;
-
+            //counter = 0;
             break;
 
         case 2: // Balancing and lift-up
 
 
-
-
             // Current Input Updater - Amperes
             // Loop 1
             Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
@@ -363,8 +362,8 @@
             PID_Output = C1.update(PID_Input);
 
             // System input
-            //Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
-            Sys_input_Amps =   0.0f - Loop1_output - Loop2_output;
+            Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
+            //Sys_input_Amps =   0.0f - Loop1_output - Loop2_output;
 
 
 
@@ -376,7 +375,7 @@
 
 
             // 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
-            if ( abs(Velocity) > 250.0f && (abs(Cuboid_Angle_Degrees)<50.0f && abs(Cuboid_Angle_Degrees)>40.0f) ) {
+            if ( abs(Velocity) > 300.0f && (abs(Cuboid_Angle_Degrees)<50.0f && abs(Cuboid_Angle_Degrees)>40.0f) ) {
                 Button_Status = 4;
                 break;
             }
@@ -409,7 +408,7 @@
             VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
             //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));
 
-            if (abs(Velocity) < 1.0f) {
+            if (abs(Velocity) < 4.0f) {
                 Sys_input_Amps = 0.0;
                 Button_Status = 2;
                 break;