Diff: main.cpp

Revision:

6:122879c1503a

Parent:

5:411f5351220f

Child:

7:07c5b6709d87

--- a/main.cpp	Tue May 12 14:57:55 2020 +0000
+++ b/main.cpp	Tue May 12 15:44:23 2020 +0000
@@ -14,7 +14,7 @@
 #include "mcp4725.h"
 #include <stdbool.h>
 #include "LinearCharacteristics.h"
-#include "PID_Cntrl_3.h"
+#include "PID_Cntrl.h"
 #include "PID_Cntrl_2.h"
 
 // Serial PC Communication
@@ -46,7 +46,7 @@
 // ------------- Variables -------------------
 
 // Sample time of main loops
-float Ts = 0.015;
+float Ts = 0.02;
 
 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
@@ -89,8 +89,8 @@
 
 // Saturation Parameters
 // PI Controller Limits
-const float uMin1 = -15.0f;
-const float uMax1= 15.0f;
+const float uMin1 = -5.0f;
+const float uMax1= 5.0f;
 
 // Cuboid Driver Input Limits
 const float uMin2 = -15.0f;
@@ -130,8 +130,8 @@
 LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
 
 // PID Controllers
-PID_Cntrl_3 C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);  // Defining the 1st Loop Controller (PI-Part)
-PID_Cntrl_3 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 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
 
 // Timers
 Timer Loop;
@@ -180,7 +180,7 @@
     //----------------------------------------------------------------------------------------------
 
     // Reset PID
-    C1.reset(0.0);
+    C1.reset(0.0f);
 
     pc.printf("Hello World!\n\r");
     // ControllerLoopTimer.attach(&updateControllers, Ts);
@@ -231,16 +231,13 @@
         Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
         // PI Controller
         PID_Input = Desired_input - Velocity;
-        PID_Output = C1(PID_Input);
+        PID_Output = C1.update(PID_Input);
         /*
         //PID_Input = 20.0 - Velocity;
         //PID_Output = C1.update(-1*PID_Input);
+        //PID_Output = 0; // CHANGE LATER - This is for testing purposes
         */
 
-
-        //PID_Output = 0; // CHANGE LATER - This is for testing purposes
-
-
         // System input
         Sys_input_Amps = PID_Output; //- Loop1_output - Loop2_output;
 
@@ -256,23 +253,23 @@
         VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
         //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.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();
-        }
-        */
+        /*
+         LoopTime = Loop.read();
+         while(LoopTime < Ts) {
+             LoopTime = Loop.read();
+         }
+         */
 
-       // pc.printf(" %0.5f \n\r", LoopTime);
+        // 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);