Diff: main.cpp

Revision:

10:4e9899cef12c

Parent:

9:43e5e3430621

Child:

11:af811e6f9a04

--- a/main.cpp	Wed May 13 10:46:05 2020 +0000
+++ b/main.cpp	Tue May 19 12:27:42 2020 +0000
@@ -69,6 +69,7 @@
 int k = 0;
 int LoopCounter = 0;
 float LoopTime = 0.0;
+int Case1_Counter = 0;
 
 // Flywheel Position and Velocity variables
 double Velocity_Input_Voltage = 0.0f;
@@ -93,8 +94,8 @@
 const float uMax1= 5.0f;
 
 // Cuboid Driver Input Limits
-const float uMin2 = -12.0f;
-const float uMax2= 12.0f;
+const float uMin2 = -13.0f;
+const float uMax2= 13.0f;
 
 // Controller Loop 2 (PI-Part)
 float Kp_1 = -0.09;
@@ -108,6 +109,9 @@
 float Kd_2 = 0; // No D-Part
 float Tf_2 = 1; // No D-Part
 
+// User Button
+int Button_Status = 0;
+
 // ---------- Functions -------------
 
 // Interrupts
@@ -143,7 +147,7 @@
 int main()
 {
     VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-    wait(20);
+    Button_Status = 0;
 
     pc.baud(115200);
     Pin_3V3.write(1);
@@ -183,15 +187,19 @@
 
     // Reset PID
     C1.reset(0.0f);
+    
+    // Wait 
+    wait(10);
 
     pc.printf("Hello World!\n\r");
     //wait(15);
     Loop.start();
+    Button_Status = 0;
     ControllerLoopTimer.attach(&updateControllers, Ts);
 
 }
 
-   // Loop.reset();
+// Loop.reset();
 
 void updateControllers(void)
 {
@@ -205,7 +213,7 @@
     Velocity_Voltage = 3.2442178*(Velocity_Voltage_Read);
     Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
     Velocity = Velocity_rpm*2.0*pi/60.0;
-    
+
 
     // Aquire Raw Acceleration and Gyro Data form the IMU
     mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
@@ -232,72 +240,95 @@
 
 
     // ------------------------- Controller -----------------------------
-    // Current Input Updater - Amperes
-    // Loop 1
-    Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
-    // Loop 2
-    Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
-    // PI Controller
-    PID_Input = Desired_input - Velocity;
-    PID_Output = C1.update(PID_Input);
-    /*
+
+    switch(Button_Status) {
+
+        case 0:
+ 
+            // PI Controller
+            PID_Input = Desired_input - Velocity;
+            PID_Output = C1.update(PID_Input);
+
+            // System input
+            Sys_input_Amps = PID_Output;
+            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+            
+            // Counter of 10 seconds then go to Case 1
+            Case1_Counter++;
+            if (Case1_Counter > 2000) {
+                Button_Status = 1;
+            }
 
-    //PID_Output = C1.update(-1*PID_Input);
-    //PID_Output = 0; // CHANGE LATER - This is for testing purposes
-    */
+        case 1:
 
-    // System input
-    Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
+            // Current Input Updater - Amperes
+            // Loop 1
+            Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
+            // Loop 2
+            Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
+            // PI Controller
+            PID_Input = Desired_input - Velocity;
+            PID_Output = C1.update(PID_Input);
+            /*
+
+            //PID_Output = C1.update(-1*PID_Input);
+            //PID_Output = 0; // CHANGE LATER - This is for testing purposes
+            */
+
+            // System input
+            Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
 
 
-    if (Sys_input_Amps > uMax2) {
-        Sys_input_Amps = uMax2;
-    }
-    if (Sys_input_Amps < uMin2) {
-        Sys_input_Amps = uMin2;
-    }
-
-    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-
-    // Print Data
-    if(++k >= 200) {
-        k = 0;
-
-        //1LoopCounter,2.1CurrentToVoltage, 2Sys_input_Amps, 3Cuboid_Angle_Degrees, 4GyroZ_RadiansPerSecond, 5Velocity, 6Velocity_Voltage, 7AccX_g, 8AccY_g, 9PID_Input, 10PID_Output, (Loop1_output+Loop2_output), Loop.read())
-        pc.printf(" %i, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, ", LoopCounter, CurrentToVoltage(Sys_input_Amps), Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
-        pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
-    }
-
-    //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
+            if (Sys_input_Amps > uMax2) {
+                Sys_input_Amps = uMax2;
+            }
+            if (Sys_input_Amps < uMin2) {
+                Sys_input_Amps = uMin2;
+            }
 
-    //
-
-    // wait(0.0025);
-    // pc.printf(" %0.5f \n\r", Loop.read());
-
-    /*
-     LoopTime = Loop.read();
-     while(LoopTime < Ts) {
-         LoopTime = Loop.read();
-     }
-     */
+            // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
+            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
-    // pc.printf(" %0.5f \n\r", LoopTime);
-
-    //pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
-
+            // Print Data
     /*
-            // Print Data
-            if(++k >= 300) {
+            if(++k >= 200) {
                 k = 0;
-                //pc.printf("%0.8f\n\r", Loop.read());
-                pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
 
+                //1LoopCounter,2.1CurrentToVoltage, 2Sys_input_Amps, 3Cuboid_Angle_Degrees, 4GyroZ_RadiansPerSecond, 5Velocity, 6Velocity_Voltage, 7AccX_g, 8AccY_g, 9PID_Input, 10PID_Output, (Loop1_output+Loop2_output), Loop.read())
+                pc.printf(" %i, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, ", LoopCounter, CurrentToVoltage(Sys_input_Amps), Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
+                pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
             }
     */
 
+            //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
 
+            //
+
+            // wait(0.0025);
+            // pc.printf(" %0.5f \n\r", Loop.read());
+
+            /*
+             LoopTime = Loop.read();
+             while(LoopTime < Ts) {
+                 LoopTime = Loop.read();
+             }
+             */
+
+            // pc.printf(" %0.5f \n\r", LoopTime);
+
+            //pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
+
+            /*
+                    // Print Data
+                    if(++k >= 300) {
+                        k = 0;
+                        //pc.printf("%0.8f\n\r", Loop.read());
+                        pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
+
+                    }
+            */
+
+    }
 }