Diff: main.cpp

Revision:

1:0f0423207b62

Parent:

0:3a132f85c1a8

Child:

2:be3b6a72f823

--- a/main.cpp	Tue Mar 22 08:33:17 2016 +0000
+++ b/main.cpp	Tue Mar 22 10:57:41 2016 +0000
@@ -1,42 +1,81 @@
 #include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// iC-MU Encoder Objects
+iC_MU icMu(p5,p6,p7,p8);
+
+// Pan Motor
+PwmOut Pan_Motor_PWM(p21);                      // Purple wire
+DigitalOut Pan_Motor_Direction(p22);            // Yellow wire
+
+/* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
+Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff
 
-DigitalOut myled(LED1);
+// Serial objects
 Serial pc (USBTX,USBRX);
-Timer t;
+
+// Global Variables
+float run_time = 5.0;
+float start_Hz = 1.0, stop_Hz = 2.0;
+float amplitude;
+bool running = 0;
+float w1;
+float w2;
+float a;
+float b;
+
+// External variables
+extern int LasticMuPosition;
+
+// Functions
+void PanVelocityLoop(void const *args);
 
 int main()
 {
-    float run_time = 5.0;
-    float start_Hz = 1.0, stop_Hz = 2.0;
-    float amplitude;
-    
+    // Set up the Pan motor
+    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
+    Pan_Motor_PWM = 1;                          // Start with motor static
+
+    // Initalise Pan Velocity loop RtosTimer thread
+    RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
+    PanVelocityLoop_timer.start(1);             // Run at 1kHz
 
+    // Initalise Pan PID Loop
+    PanVelocityPID.setProcessLimits(1.0, -1.0);
+    PanVelocityPID.setSetPoint(0);
+    LasticMuPosition = icMu.ReadPOSITION() >> 1;
+
+    // Increase terminal speed
+    pc.baud(921600);
+
+    // Prompt the user to enter the test parameters
     pc.printf("\n\r Enter Start frequency (Hz): ");
     pc.scanf("%f", &start_Hz);
     pc.printf("%f",start_Hz);
-    
+
     pc.printf("\n\r Enter Stop frequency (Hz): ");
     pc.scanf("%f", &stop_Hz);
     pc.printf("%f",stop_Hz);
-    
+
     pc.printf("\n\r Enter Amplitude (encoder counts): ");
     pc.scanf("%f", &amplitude);
     pc.printf("%f",amplitude);
-    
+
     pc.printf("\n\n\r Press any key to start test...");
     pc.getc();
-    
-    float w1 = start_Hz * 3.14159 * 2;
-    float w2 = stop_Hz * 3.14159 * 2; 
-    float a = (w2 - w1) / (2 * run_time); 
-    float b = w1;    
-    float now;
-       
-    t.start();
+
+    w1 = start_Hz * 3.14159 * 2;
+    w2 = stop_Hz * 3.14159 * 2;
+    a = (w2 - w1) / (2 * run_time * 1000000);
+    b = w1 / 1000;
 
-    while(t.read() < run_time) {
-        now = t.read();
-        pc.printf("\n\r%f",amplitude * sin((a*now*now)+(b*now)));
+    // Set the test running
+    running = 1;
+
+    while(1) {
+        ;
     }
 
 }