Diff: main.cpp

Revision:

1:d3406369c297

Parent:

0:8e87cdf07037

Child:

2:e088fa08e244

--- a/main.cpp	Fri Jan 31 17:59:21 2020 +0000
+++ b/main.cpp	Fri Jan 31 18:22:51 2020 +0000
@@ -45,7 +45,7 @@
 // ------------- Variables -------------------
 
 // Sample time of main loops
-float Ts = 0.01;
+float Ts = 0.015;
 
 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
@@ -67,6 +67,7 @@
 // printf Variable
 int k = 0;
 int LoopCounter = 0;
+float LoopTime = 0.0;
 
 // Flywheel Position and Velocity variables
 double Velocity_Input_Voltage = 0.0f;
@@ -142,6 +143,8 @@
 {
 
     pc.baud(115200);
+    
+    Loop.start();
 
     // Reset Filters
     FilterAccX.reset(0.0f);
@@ -178,15 +181,16 @@
     C1.reset();
 
     pc.printf("Hello World!\n\r");
-    ControllerLoopTimer.attach(&updateControllers, Ts);
+    // ControllerLoopTimer.attach(&updateControllers, Ts);
+
+
 
-}
-
-void updateControllers(void)
+while(1)
 {
+    Loop.reset();
+    
     // Counter 
     LoopCounter++;
-    Loop.start();
 
     // Acquire Velocity
     Velocity_Voltage_Read = Velocity_Voltage_Input.read();
@@ -246,9 +250,21 @@
     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, %0.5f \n\r", LoopCounter, Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_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));
+    
       // pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
-    Loop.reset();
+    
+    // 
+    
+    // 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);
     
@@ -263,6 +279,7 @@
 */
 
 }
+}