Diff: main.cpp

Revision:

3:463edcfc09c5

Parent:

2:be3b6a72f823

--- a/main.cpp	Tue Mar 22 15:35:01 2016 +0000
+++ b/main.cpp	Wed Jun 08 13:56:25 2016 +0000
@@ -1,85 +1,134 @@
 #include "mbed.h"
 #include "iC_MU.h"
 #include "rtos.h"
-#include "Classic_PID.h"
+#include "MyAxis.h"
 
 // iC-MU Encoder Objects
-iC_MU icMu(p5,p6,p7,p8);
-iC_MU icMu_Output(p5,p6,p7,p11);
+iC_MU tilt_Motor(p5,p6,p7,p8);
+iC_MU TiltPos(p5,p6,p7,p11);
+iC_MU pan_Motor(p5,p6,p7,p12);
+iC_MU PanPos(p5,p6,p7,p13);
 
+// Tilt Motor
+PwmOut Tilt_Motor_PWM(p23);                     // Purple wire
+DigitalOut Tilt_Motor_Direction(p24);           // Yellow wire
 // 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.0);     //Kp, ki, kd, kvelff
-Classic_PID PanVelocityPID(0.000006, 0.0000001, 0.0, 0.0);     //Kp, ki, kd, kvelff
-
 // Serial objects
 Serial pc (USBTX,USBRX);
 
-// Global Variables
-float run_time = 5.0;
+int ServiceKeyboard();
+
+
+// Global Variables for sinusoidrun
+float run_time = 10.0;
 float start_Hz = 1.0, stop_Hz = 2.0;
-float amplitude;
-bool running = 0;
-float w1;
-float w2;
-float a;
-float b;
+extern float amplitude;
+bool sinusoid = 0;
+extern float a;
+extern float Hz;
+
+// Global variables for auto gearbox calculator
+bool ratiocalc = 0;
+
 
 // External variables
 extern int LasticMuPosition;
 
 // Functions
-void PanVelocityLoop(void const *args);
+void PanAxisInitialise();
+void TiltAxisInitialise();
+void AxisControlLoop(MyAxis& Axis);
+void Pan_Counter(void const *args);
+
+void RunLoop(void const *args)
+{
+    AxisControlLoop(Pan);
+    AxisControlLoop(Tilt);
+}
 
 int main()
 {
-    // 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(262144);    // Set the motor to 2^18
-    //PanVelocityPID.setSetPoint(0);
-    //LasticMuPosition = icMu.ReadPOSITION() >> 1;
+    //Startme();
+    pc.baud(921600);
+    pc.printf("\n\r AH-10 Motor Scope Tool \n\r");
+    pc.printf("\n\r Press q for menu \n\r");
+    PanAxisInitialise();
+    TiltAxisInitialise();
+    RtosTimer RunLoop_timer(RunLoop, osTimerPeriodic);
+    RunLoop_timer.start(1);             // Run at 1kHz
     
-
-    // Increase terminal speed
-    pc.baud(921600);
+    
+    // Initalise Pan counter RtosTimer thread
+    RtosTimer Pan_Counter_timer(Pan_Counter, osTimerPeriodic);
+    Pan_Counter_timer.start(1);
 
-    // 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();
-
-    w1 = start_Hz * 3.14159 * 2;
-    w2 = stop_Hz * 3.14159 * 2;
-    a = (w2 - w1) / (2 * run_time * 1000000);
-    b = w1 / 1000;
-
-    // Set the test running
-    running = 1;
 
     while(1) {
-        ;
+
+        if(pc.readable()) {
+            ServiceKeyboard();
+        }
+
+        if (sinusoid) {
+            sinusoid = false;
+
+            // 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\r Enter Time (s): ");
+            pc.scanf("%f", &run_time);
+            pc.printf("%f",run_time);
+
+            pc.printf("\n\n\r Press any key to start test...");
+            pc.getc();
+
+            a = (stop_Hz - start_Hz) / (run_time * 1000);  // This is the incrementor
+            pc.printf("\n\n\n\r ********************************************* \n\n\n\r *************************************************");
+            // pc.printf("\n\n\r Time (ms),input,output,frequency (Hz)");
+            pc.printf("\n\n\r Time (ms),Motor,System,frequency (Hz x 100),Demand");
+
+            Pan.start_SysPos = Pan.System_position;
+            Pan.start_MotPos = Pan.Motor_position;
+
+            Tilt.start_SysPos = Tilt.System_position;
+            Tilt.start_MotPos = Tilt.Motor_position;
+
+            // Set the test running#
+            if (Pan.jog > 0){
+            Pan.sinusoidrun = 1;
+            }
+            if (Tilt.jog > 0) {
+            Tilt.sinusoidrun = 1;
+            }
+        }
+
+        if (ratiocalc) {
+            ratiocalc = false;
+
+            Pan.start_SysPos = Pan.System_position;
+            Pan.start_MotPos = Pan.Motor_position;
+
+            Pan.calcmyratio = true;
+
+            Tilt.start_SysPos = Tilt.System_position;
+            Tilt.start_MotPos = Tilt.Motor_position;
+
+            Tilt.calcmyratio = true;
+        }
     }
 
 }