Diff: main.cpp

Revision:

8:71babe904b92

Parent:

7:07c5b6709d87

Child:

9:43e5e3430621

--- a/main.cpp	Tue May 12 16:51:44 2020 +0000
+++ b/main.cpp	Tue May 12 19:23:59 2020 +0000
@@ -46,7 +46,7 @@
 // ------------- Variables -------------------
 
 // Sample time of main loops
-float Ts = 0.02;
+float Ts = 0.005;
 
 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
@@ -80,7 +80,7 @@
 // Variables concerning the Controller, the Design is in reference to the Matlab Simulink .............. and Variables are in reference to the ............. File
 
 // Sate Space Controller Values
-float K_SS_Controller [2] = {-57.1176, -2.6398}; // From Matlab
+float K_SS_Controller [2] = {-57.1176*0.3, -2.6398}; // From Matlab
 
 // Controller Variables
 float Loop1_output; // Loop 1 controller output
@@ -93,8 +93,8 @@
 const float uMax1= 5.0f;
 
 // Cuboid Driver Input Limits
-const float uMin2 = -15.0f;
-const float uMax2= 15.0f;
+const float uMin2 = -12.0f;
+const float uMax2= 12.0f;
 
 // Controller Loop 2 (PI-Part)
 float Kp_1 = -0.09;
@@ -146,7 +146,7 @@
     pc.baud(115200);
     Pin_3V3.write(1);
 
-    Loop.start();
+    //Loop.start();
 
     // Reset Filters
     FilterAccX.reset(0.0f);
@@ -184,14 +184,16 @@
 
     pc.printf("Hello World!\n\r");
     wait(1);
-
+    Loop.start();
     ControllerLoopTimer.attach(&updateControllers, Ts);
 
 }
 
+   // Loop.reset();
+
 void updateControllers(void)
 {
-    //Loop.reset();
+
 
     // Counter
     //LoopCounter++;
@@ -235,15 +237,15 @@
     Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
     // PI Controller
     PID_Input = Desired_input - Velocity;
-    PID_Output = C1.update(-1*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
     */
 
     // System input
-    Sys_input_Amps = PID_Output; //- Loop1_output - Loop2_output;
+    Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
 
 
     if (Sys_input_Amps > uMax2) {
@@ -257,12 +259,12 @@
     VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
     // Print Data
-    if(++k >= 30) {
+    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("\n\r");
+        pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
     }
 
     //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());