Diff: main.cpp

Revision:

17:04eebb7c29b5

Parent:

16:ff375f62a95f

Child:

18:3b1aa67e11ca

--- a/main.cpp	Fri May 29 12:16:32 2020 +0000
+++ b/main.cpp	Fri May 29 13:57:32 2020 +0000
@@ -138,8 +138,8 @@
 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;
-float Ki_2 = 0.25;
+float Kp_2 = 0.25/4.0;
+float Ki_2 = 0.25/4.0;
 float Kd_2 = 0; // No D-Part
 float Tf_2 = 1; // No D-Part
 
@@ -172,7 +172,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,Ki_1,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
+PID_Cntrl  C3(Kp_1*2.5,Ki_1*1.5,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
 
 // Accelerometer and Gyroscope Filters
 IIR_filter FilterAccX(t, Ts, 1.0f);
@@ -202,11 +202,20 @@
 //******************************************************************************
 int main()
 {
+
+    VoltageOut.write(2.5); // Output Zero Current to the Motor
+
     // Microcontroller initialization
     Pin_3V3.write(1);
     *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
     // IMU - MPU6050 initialization - Do NOT Modify
     mpu.initialize();
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult) {
+        pc.printf("MPU6050 test passed \n\r");
+    } else {
+        pc.printf("MPU6050 test failed \n\r");
+    }
     mpu.setDLPFMode(MPU6050_DLPF_BW_42); // Set Low Pass Filter Bandwidth to 44Hz (4.9ms) for the Acc and 42Hz (4.8ms) for the Gyroscope
     mpu.setFullScaleGyroRange(2u);  // Change the scale of the Gyroscope to +/- 1000 degrees/sec
     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
@@ -214,7 +223,7 @@
     // Serial Communication
     pc.baud(115200);
 
-    VoltageOut.write(2.5); // Output Zero Current to the Motor
+
 
     // Reset Filters and PID Controller
     FilterAccX.reset(0.0f);
@@ -251,20 +260,21 @@
 
     // -------------- Convert Raw data to SI Units --------------------
 
-    //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
+//Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
     AccX_g = AccX_Raw / 8192.0f;
     AccY_g = AccY_Raw / 8192.0f;
     AccZ_g = AccZ_Raw / 8192.0f;
 
-    //Convert Gyroscope Raw Data to Degrees per second - (2^15/1000 = 32.768)
-    GyroX_Degrees = GyroX_Raw / 32.768f;
-    GyroY_Degrees = GyroY_Raw / 32.768f;
-    GyroZ_Degrees = GyroZ_Raw / 32.768f;
+    //Convert Gyroscope Raw Data to Degrees per second
+    GyroX_Degrees = GyroX_Raw / 32.768f;  // (2^15/1000 = 32.768)
+    GyroY_Degrees = GyroY_Raw / 32.768f;  // (2^15/1000 = 32.768)
+    GyroZ_Degrees = GyroZ_Raw / 32.768f;  // (2^15/1000 = 32.768)
 
     //Convert Gyroscope Raw Data to Degrees per second
     GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
 
     // ----- 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_Degrees = Cuboid_Angle_Radians*180.0f/pi;
 
@@ -275,8 +285,7 @@
     switch(Button_Status) {
 
         case 0: // Output of 0 Amps
-
-            VoltageOut.write(CurrentToVoltage(0));
+            Sys_input_Amps = 0.0;
             break;
 
         case 1: // Breaking
@@ -287,13 +296,11 @@
 
             // System input
             Sys_input_Amps = PID_Output;
-            // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
             break;
 
         case 2: // Balancing and lift-up
 
-            // Current Input Updater - In Amps
+            // Current Input Updater - Amperes
             // Loop 1
             Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
             // Loop 2
@@ -304,8 +311,6 @@
 
             // System input
             Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
-            // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
 
             // 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
@@ -316,13 +321,13 @@
             break;
 
         case 3: // Fall
-            
-            VoltageOut.write(CurrentToVoltage(0.0f));
+        
+            Sys_input_Amps = 0.0;
             // Not implemented Yet :)
             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
+            // 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
 
             PID_Input2 = 0.0f - Velocity;
             PID_Output2 = C3.update(-1*PID_Input2);
@@ -338,28 +343,30 @@
             VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
             //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));
 
-            if (abs(Velocity) < 20.0f) {
-
+            if (abs(Velocity) < 10.0f) {
+                Sys_input_Amps = 0.0;
                 Button_Status = 2;
                 break;
             }
             break;
     }
     // End of Switch Statement
-    
+
     if (Sys_input_Amps > uMax) {
         Sys_input_Amps = uMax;
     }
     if (Sys_input_Amps < uMin) {
         Sys_input_Amps = uMin;
     }
+    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
+    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
     // ----------------
 
     // 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\n\r", Sys_input_Amps, Velocity);
+        pc.printf("Some Outputs: %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, Button_Status);
     }
 
 
@@ -386,6 +393,6 @@
         if (Button_Status > 3.0f) {
             Button_Status = 1.0;
         }
-        pc.printf("Button Status: %i\r\n", Button_Status);
+        //pc.printf("Button Status: %i\r\n", Button_Status);
     }
 }