Diff: main.cpp

Revision:

21:762f8769cba4

Parent:

20:b142ae11a12a

Child:

23:b7b7271deb11

--- a/main.cpp	Thu Aug 20 13:50:22 2020 +0000
+++ b/main.cpp	Fri Aug 21 14:55:17 2020 +0000
@@ -12,30 +12,30 @@
 /*
 
     // Physical Model of Cuboid 2.0
-    
+
     % Mass Parameters
     Cube Mini - Mass = 0.6 + 0.170 + 0.015 = 0.7850 Kg, Mass of the Full Cuboid with Motor and Flywheel
     Cube Mini - Flywheel Mass = 0.170 kg
     Cube Mini - Without Flywheel Mass = 0.6150 kg
-    
+
     % Dimensions Parameters
     % Cube Mini Dimensions (10x10x10) cm
     Cube Mini - Length = 0.90*100e-3 m (The multiplication by 0.9 is to compensate for the rounded edges)
-    Cube Mini - CoM = Cube Mini - Length * 0.5,  Center of Mass Location 
+    Cube Mini - CoM = Cube Mini - Length * 0.5,  Center of Mass Location
     % Inertia Parameters
     Cube Mini - Inertia (J) of the Body from the edge Edge = 0.003044 Kg.m^2,  Inertia about the edge
     Cube Mini - Inertia (J) of Flyhweel and Rotor from Center = (0.0002104064 + 0.0000181) kg.m^2,
-    
+
     % Motor Parameters
     % Motor: Maxon Flat EC45, Part number: 411812, Specs: 24V, Km 36.9e-3
     Cube Mini - Motor.Km = 36.9e-3 % Torque Constant % Unit: Nm/A
     Cube Mini - Motor.Resistance = 0.608 % Terminal Resistance % Unit: Ohm
     Cube Mini - Motor.Inductance = 0.463e-3 % Unit: H, Henry
-    
-    % PI Controller Escon 
+
+    % PI Controller Escon
     Cube Mini - Escon.PI_Tn = 500e-6; %945e-6
     Cube Mini - Escon.PI_Kp = 100*(10/2^11);%576*(10/2^11)
-    
+
     % RC Physical Velocity Voltage Filter
     Filter.Velocity.R = 1e3 Ohm
     Filter.Velocity.C = 10e-6 F
@@ -154,7 +154,9 @@
 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] = {-57.1176*0.3, -2.6398*1.35}; // From Matlab
+float K_SS_Controller [2] = {-64.6865,  -4.2719}; // From Matlab
+
 
 // Controller Variables
 float Loop1_output; // Loop 1 controller output
@@ -164,8 +166,10 @@
 // PID (PI Parameters)
 
 // PID 1 - Velocity control after lift-up
-float Kp_1 = -0.09;
-float Ki_1 = -0.09*0.5*0.5;
+float Kp_1 = - 0.01;
+float Ki_1 = - 0.05;
+//float Ki_1 = 0;
+
 float Kd_1 = 0; // No D-Part
 float Tf_1 = 1; // No D-Part
 // Controller Loop (PI-Part) in Case 2 (breaking case)
@@ -176,12 +180,15 @@
 
 // Saturation Parameters
 // PI Controller Limits
-const float uMin1 = -13.0f;
-const float uMax1= 13.0f;
+const float uMin1 = -15.0f;
+const float uMax1= 15.0f;
 
 // Cuboid Escon Input Limits in Amps
-const float uMin = -13.0f;        // Minimum Current Allowed
-const float uMax =  13.0f;        // Maximum Current Allowe
+const float uMin = -15.0f;        // Minimum Current Allowed
+const float uMax =  15.0f;        // Maximum Current Allowe
+
+// temp
+int counter = 0;
 
 
 // -------------------------------
@@ -233,7 +240,7 @@
 //******************************************************************************
 int main()
 {
-    
+
     VoltageOut.write(2.5); // Output Zero Current to the Motor
     GLED = 1;
 
@@ -311,6 +318,7 @@
     Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
 
 
+
     // ------------------------- Controller -----------------------------
 
     // Switch Statement Maybe?......
@@ -318,7 +326,8 @@
 
         case 0: // Output of 0 Amps
             Sys_input_Amps = 0.0;
-            
+
+            counter = 0;
             C1.reset(0.0f);
             break;
 
@@ -334,11 +343,16 @@
             // System input
             //Sys_input_Amps = PID_Output;
             Sys_input_Amps = 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];
@@ -349,8 +363,16 @@
             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;
+
+
+
+            //if(++counter < 30) {
+              //  Sys_input_Amps = 12.5;
+            //} else {
+              //  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,14 +383,16 @@
             break;
 
         case 3: // Fall
-        
+
             Sys_input_Amps = 0.0;
             // Not implemented Yet :)
+            counter = 0;
             break;
 
         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
 
+            counter = 0;
             C1.reset(0.0f);
 
             PID_Input2 = 0.0f - Velocity;
@@ -401,13 +425,12 @@
         Sys_input_Amps = uMin;
     }
     if (Cuboid_Angle_Degrees > -50.0f && Cuboid_Angle_Degrees < 50.0f) {
-    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-    }
-    else {
+        // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
+        VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+    } else {
         Sys_input_Amps = 0.0f;
         VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-        }
+    }
 
     // ----------------