Diff: main.cpp

Revision:

11:af811e6f9a04

Parent:

10:4e9899cef12c

Child:

12:ba4ce3fa4f53

--- a/main.cpp	Tue May 19 12:27:42 2020 +0000
+++ b/main.cpp	Tue May 19 17:50:52 2020 +0000
@@ -187,8 +187,8 @@
 
     // Reset PID
     C1.reset(0.0f);
-    
-    // Wait 
+
+    // Wait
     wait(10);
 
     pc.printf("Hello World!\n\r");
@@ -196,6 +196,10 @@
     Loop.start();
     Button_Status = 0;
     ControllerLoopTimer.attach(&updateControllers, Ts);
+    
+   
+   // Configure the ADC to the internel reference of 1.2V. ADC -> Congif Reg 0x40007504 // Multiply the ADC output (PIN input) by 1.2 to get the actual input relative to 3.0V. Then multiply by 3.0 to get the actual voltage
+   *((unsigned int *)0x40007504) = 0x400A;
 
 }
 
@@ -210,7 +214,7 @@
 
     // Acquire Velocity
     Velocity_Voltage_Read = Velocity_Voltage_Input.read();
-    Velocity_Voltage = 3.2442178*(Velocity_Voltage_Read);
+    Velocity_Voltage = 3.0*1.1978917*(Velocity_Voltage_Read); // *1.2V because the Vref is 1.2V // *3 because the prescaling is 1/3 // *1.1978917 is for Calibration purposes instead of 1.2
     Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
     Velocity = Velocity_rpm*2.0*pi/60.0;
 
@@ -244,7 +248,7 @@
     switch(Button_Status) {
 
         case 0:
- 
+
             // PI Controller
             PID_Input = Desired_input - Velocity;
             PID_Output = C1.update(PID_Input);
@@ -252,7 +256,7 @@
             // System input
             Sys_input_Amps = PID_Output;
             VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-            
+
             // Counter of 10 seconds then go to Case 1
             Case1_Counter++;
             if (Case1_Counter > 2000) {
@@ -267,17 +271,15 @@
             // Loop 2
             Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
             // PI Controller
-            PID_Input = Desired_input - Velocity;
-            PID_Output = C1.update(PID_Input);
-            /*
+            //PID_Input = Desired_input - Velocity;
+            //PID_Output = C1.update(PID_Input);
 
-            //PID_Output = C1.update(-1*PID_Input);
-            //PID_Output = 0; // CHANGE LATER - This is for testing purposes
-            */
+            PID_Input = 52.36f - Velocity;
+            PID_Output = C1.update(-1*PID_Input);
 
             // System input
-            Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
-
+            //Sys_input_Amps = 2.0f; //PID_Output; //- Loop1_output - Loop2_output;
+            VoltageOut.write(0.5f);
 
             if (Sys_input_Amps > uMax2) {
                 Sys_input_Amps = uMax2;
@@ -287,18 +289,20 @@
             }
 
             // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+            //VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
 
             // Print Data
-    /*
+
             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("%i; %0.6f, \n\r", LoopCounter, Loop.read());
+                // Printing Register Values
+                //printf("ADC : 0x%x\r\n", *((unsigned int *)0x40007504));
+                //printf("ADC : %0.5f\n", Velocity_Voltage_Input.read());
             }
-    */
 
             //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());