Diff: main.cpp

Revision:

3:1a714eccfe9f

Parent:

2:e088fa08e244

Child:

4:2a5cd0ad8100

--- a/main.cpp	Fri May 08 14:51:09 2020 +0000
+++ b/main.cpp	Fri May 08 17:02:29 2020 +0000
@@ -1,6 +1,6 @@
 /*
     Boulus Abu Joudom
-    Version: January 28, 2020. 12:00
+    Version: May 5, 2020. 17:00
     ZHAW - IMS
 
     Cubiod Balancing Experiment - Control Lab
@@ -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] = {-33.5, -1.856}; // Currently for Cuboid 1.0 - UPDATE LATER for Cuboid 2.0 -
+float K_SS_Controller [2] = {-57.1176, -2.6398}; // From Matlab
 
 // Controller Variables
 float Loop1_output; // Loop 1 controller output
@@ -89,8 +89,8 @@
 
 // Saturation Parameters
 // PI Controller Limits
-const float uMin1 = -5.0f;
-const float uMax1= 5.0f;
+const float uMin1 = -15.0f;
+const float uMax1= 15.0f;
 
 // Cuboid Driver Input Limits
 const float uMin2 = -15.0f;
@@ -126,8 +126,8 @@
 IIR_filter FilterGyro(t, Ts, t);
 
 // Linear Scaler
-LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 4.94f);
-LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -2000.0f, 2000.0f);
+LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
+LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
 
 // PID Controllers
 PID_Cntrl_2 C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);  // Defining the 1st Loop Controller (PI-Part)
@@ -185,7 +185,7 @@
     pc.printf("Hello World!\n\r");
     // ControllerLoopTimer.attach(&updateControllers, Ts);
 
-
+wait(1);
 
 while(1)
 {
@@ -238,9 +238,14 @@
         //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 = - Loop1_output - Loop2_output;
+
+
     if (Sys_input_Amps > uMax2) {
         Sys_input_Amps = uMax2;
     }
@@ -248,11 +253,11 @@
         Sys_input_Amps = uMin2;
     }
 
-    // Scaling the controller output from -15 A --> 15 A to 0 V --> 1 V
+    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
     VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
-            //1LoopCounter, 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, ", LoopCounter, Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
+            //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());