Revision 3:463edcfc09c5, committed 2016-06-08

Comitter:

acodd

Date:

Wed Jun 08 13:56:25 2016 +0000

Parent:

2:be3b6a72f823

Commit message:

As used for scoping motors May / June 2016

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis_Control.cpp	Wed Jun 08 13:56:25 2016 +0000
@@ -0,0 +1,403 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "MyAxis.h"
+
+extern iC_MU pan_Motor, PanPos;
+extern PwmOut Pan_Motor_PWM;
+
+extern iC_MU tilt_Motor, TiltPos;
+extern PwmOut Tilt_Motor_PWM;
+
+extern DigitalOut Pan_Motor_Direction;
+extern DigitalOut Tilt_Motor_Direction;
+
+extern Serial pc;
+
+extern float Pan_count;
+
+// Deal with profiling and scoping (generic)
+bool scoping;
+int scope_duration = 1000;
+int scope_time = 0;
+float amplitude;
+float a;
+float Hz;
+extern float start_Hz;
+extern float stop_Hz;
+extern float run_time;
+float jogaccel_time = 2000;
+float jogaccel = 1;
+float dwell = 1000;
+float bits = 19;
+int OneTurnCts = pow(2, bits); //491520
+int MyLower = pow(2,(bits-4));
+int MyUpper = OneTurnCts - MyLower;
+int skip = 0;
+int primed = 0;
+
+
+// Motor Generics
+float c;
+float m;
+float Duty_Cycle = 0;
+float Motor_Direction;
+float Motor_PWM;
+float DeadBand = 0.012;
+int Motor_wrap = 4194304;
+
+
+void PanAxisInitialise()
+{
+    //MyAxis Pan;
+    Pan.Number = 1;
+    Pan.jog = 3;
+    Pan.sysjogdir = 1;
+    Pan.sinusoidrun = 0;
+    Pan.scoping = 0;
+    Pan.setramp = 0;
+    Pan.setramp_x = 0;
+    Pan.moveme = 0;
+    Pan.ramp = 0.05;
+    Pan.run_count = 0;
+    Pan.ratio = 115;  //This is 115 on original breadboard, -113 on a prototype MS5
+    Pan.Motor_scale = 11100; //15728;  //(based on 1800RPM, 2^19 encoder and 0.001s loop)
+    Pan.c_moving = 20;
+    Pan.c_stationary = 2;
+    Pan.Jog_1 = 0;
+    Pan.Jog_2 = 60;
+    Pan.Kp = 0.060;
+    Pan.Kp_Sys = 0.000;
+    Pan.Kff = 1;
+    Pan.Kff_Sys = 1;
+    Pan.Feed_Forward = 0;
+    Pan.Prev_Motor_final = 0;
+    Pan.Motor_Vel = 0;
+    Pan.Motor_position = 0;
+    Pan.dojog = 0;
+    Pan.sysdojog = 0;
+    Pan.jogdir = 1;
+    Pan.sysjogdir = 1;
+    Pan.start_MotPos = 0;
+
+
+    // Set up the Pan motor
+    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
+    Pan_Motor_PWM = 1;                          // Start with motor static
+    Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();
+    Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;
+
+    Pan.Motor_position = 0;
+    Pan.MotorDemandPos = 0;
+    Pan.System_position = PanPos.ReadPOSITION();
+    Pan.SysDemandPos = Pan.System_position;
+    Pan.SysDemand_Prev = Pan.SysDemandPos;
+    Pan.Sys_follow_error = Pan.SysDemandPos - Pan.System_position;
+    //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
+}
+
+void TiltAxisInitialise()
+{
+    //MyAxis Pan;
+    Tilt.Number = 2;
+    Tilt.jog = 3;
+    Tilt.sysjogdir = 1;
+    Tilt.sinusoidrun = 0;
+    Tilt.scoping = 0;
+    Tilt.setramp = 0;
+    Tilt.setramp_x = 0;
+    Tilt.moveme = 0;
+    Tilt.ramp = 0.05;
+    Tilt.run_count = 0;
+    Tilt.ratio = -115;  // This is -115 on original prototype unit, -113 on a prototype MS5)
+    Tilt.Motor_scale = 11100; //15728;  //(based on 1800RPM, 2^19 encoder and 0.001s loop)
+    Tilt.c_moving = 20;
+    Tilt.c_stationary = 2;
+    Tilt.Jog_1 = 0;
+    Tilt.Jog_2 = 60;
+    Tilt.Kp = 0.06;
+    Tilt.Kp_Sys = 0.0005;
+    Tilt.Kff = 1;
+    Tilt.Kff_Sys = 1;
+    Tilt.Feed_Forward = 0;
+    Tilt.Prev_Motor_final = 0;
+    Tilt.Motor_Vel = 0;
+    Tilt.Motor_position = 0;
+    Tilt.dojog = 0;
+    Tilt.sysdojog = 0;
+    Tilt.jogdir = 1;
+    Tilt.sysjogdir = 1;
+    Tilt.start_MotPos = 0;
+
+
+    // Set up the Tilt motor
+    Tilt_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
+    Tilt_Motor_PWM = 1;                          // Start with motor static
+    Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();
+    Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;
+
+    Tilt.Motor_position = 0;
+    Tilt.MotorDemandPos = 0;
+    Tilt.System_position = TiltPos.ReadPOSITION();
+    Tilt.SysDemandPos = Tilt.System_position;
+    Tilt.SysDemand_Prev = Tilt.SysDemandPos;
+    Tilt.Sys_follow_error = Tilt.SysDemandPos - Tilt.System_position;
+    //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
+}
+
+void UpdatePanFeedback()
+{
+    Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;
+    Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();
+
+    Pan.Motor_Vel =  (Pan.Motor_Inst_Pos - Pan.MotorLast_Inst_Pos);
+
+    // Check to see if we have gone past the index point
+    if( (Pan.Motor_Inst_Pos < MyLower) && (Pan.MotorLast_Inst_Pos > MyUpper) ) {            // We have gone over the index point in 1 direction
+        Pan.Motor_Vel += OneTurnCts;
+    } else if( (Pan.Motor_Inst_Pos > MyUpper) && (Pan.MotorLast_Inst_Pos < MyLower) ) {     // We have gone over the index point in the other direction
+        Pan.Motor_Vel -= OneTurnCts;
+    }
+
+    Pan.Motor_position = Pan.Motor_position + Pan.Motor_Vel;
+
+    Pan.System_position_prev = Pan.System_position;
+    Pan.System_position = PanPos.ReadPOSITION();
+        
+    Pan.SysActualVel = Pan.System_position - Pan.System_position_prev;
+
+}
+void UpdateTiltFeedback()
+{
+    Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;
+    Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();
+
+    Tilt.Motor_Vel =  (Tilt.Motor_Inst_Pos - Tilt.MotorLast_Inst_Pos);
+
+    // Check to see if we have gone past the index point
+    if( (Tilt.Motor_Inst_Pos < MyLower) && (Tilt.MotorLast_Inst_Pos > MyUpper) ) {            // We have gone over the index point in 1 direction
+        Tilt.Motor_Vel += OneTurnCts;
+    } else if( (Tilt.Motor_Inst_Pos > MyUpper) && (Tilt.MotorLast_Inst_Pos < MyLower) ) {     // We have gone over the index point in the other direction
+        Tilt.Motor_Vel -= OneTurnCts;
+    }
+
+    Tilt.Motor_position = Tilt.Motor_position + Tilt.Motor_Vel;
+    
+  //  Tilt.System_position_prev = Tilt.System_position;
+    Tilt.System_position = TiltPos.ReadPOSITION();
+    
+  //  Tilt.SysActualVel = Tilt.System_position - Tilt.System_position_prev;
+
+}
+
+void AxisControlLoop(MyAxis& Axis)
+{
+    if (Axis.Number == 1) {
+        UpdatePanFeedback();
+    } else if (Axis.Number == 2) {
+        UpdateTiltFeedback();
+    }
+
+
+    // ***********************************************************
+    // Sinusoid test
+
+    if ((Axis.sinusoidrun) && (Axis.jog > 0)) {
+        //pc.printf("\n\r%d,%d,%f,%f",run_count,(Motor_Inst_Pos-262144),outprint,(b*1000));
+        //Time (ms),Motor,System,SysDemandPos,frequency (Hz),Demand")
+        pc.printf("\n\r%f,%f,%f,%f,%f",Axis.run_count,(Axis.Motor_position - Axis.start_MotPos),((Axis.System_position - Axis.start_SysPos)* Axis.ratio),(Hz*100),((Axis.SysDemandPos - Axis.start_SysPos) * Axis.ratio));
+
+        if(Axis.run_count < (run_time * 1000)) {
+            // demandpos = 262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count)));
+            Hz = (start_Hz + (a * Axis.run_count));  // This is the demand frequency
+            //SysDemandPos = SysDemandPos + (amplitude * sin(2 * 3.14159 * Hz * run_count / 1000));
+            Axis.SysDemandPos = Axis.start_SysPos + (amplitude * sin(2 * 3.14159 * Hz * Axis.run_count / 1000));
+            // PanVelocityPID.setSetPoint(demandpos);
+            Axis.run_count++;        // Inc run_count
+        } else {
+            Axis.sinusoidrun = 0;        // Stop the test
+            Axis.run_count = 0;
+        }
+    }
+
+    // ***********************************************************
+    // Calculate the effective ratio on the fly
+
+    if ((Axis.calcmyratio) && (fabs(Axis.SysActualVel) > 0)) {
+       
+        Axis.dynamicratio = fabs((Axis.Motor_Vel) / (Axis.SysActualVel));
+
+
+    }
+
+    // ***********************************************************
+    // Go ahead and move at a set speed
+
+    if (Axis.moveme) {
+        Axis.SysDemandPos = Axis.SysDemandPos + (Axis.jog * Axis.sysjogdir);
+    }
+
+    // ***********************************************************
+    // Create set ramps to follow
+    // sysjog is System speed demand in counts/ms at the System encoder (output)
+    if (Axis.setramp) {
+        if (Axis.run_count_jog < jogaccel_time) {
+            if (primed) {
+                scoping = true;
+                primed = false;
+            }
+            Axis.sysjogdir = 1;
+            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + Axis.sysdojog;
+            Axis.SysDemandPos = Axis.SysDemandPos * Axis.sysjogdir;
+        } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
+            Axis.sysdojog = Axis.jog;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
+            Axis.sysdojog = Axis.sysdojog - Axis.ramp;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3 * jogaccel_time) + dwell))) {
+            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
+            Axis.run_count_jog++;
+            Axis.sysjogdir = -1;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= ((3* jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3* jogaccel_time) + dwell + dwell))) {
+            Axis.sysdojog = 0;
+            Axis.run_count_jog++;
+        } else if (Axis.run_count_jog >= ((3* jogaccel_time) + dwell + dwell)) {
+            Axis.setramp = false;
+            Axis.run_count_jog = 0;
+            Axis.sysjogdir = 1;
+            scoping = false;
+        }
+    }
+
+    if (Axis.setramp_x) {
+        if (Axis.run_count_jog < jogaccel_time) {
+            if (primed) {
+                scoping = true;
+                primed = false;
+            }
+            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
+            Axis.sysdojog = Axis.jog;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
+            Axis.sysdojog = Axis.sysdojog - Axis.ramp;
+            Axis.run_count_jog++;
+            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
+        } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell + dwell))) {
+            Axis.sysdojog = 0;
+            Axis.run_count_jog++;
+        } else if (Axis.run_count_jog >= ((2* jogaccel_time) + dwell + dwell)) {
+            Axis.setramp_x = false;
+            Axis.run_count_jog = 0;
+            scoping = false;
+        }
+    }
+
+
+    // **********************************************************
+    // Generate perfect velocity demand for the System:
+    Axis.SysProfilerVel = Axis.SysDemandPos - Axis.SysDemand_Prev;
+    Axis.SysDemand_Prev = Axis.SysDemandPos;
+
+    // **********************************************************
+    // Apply proportional gain to the System Velocity demand if sufficient
+    Axis.Sys_follow_error = Axis.System_position - Axis.SysDemandPos;
+    Axis.SysDerivedVel = Axis.SysProfilerVel - (Axis.Sys_follow_error * Axis.Kp_Sys);
+
+    // ^ LOAD SIDE
+    // *************************************************
+    // MOTOR CONTROL LOOP
+    // *************************************************
+    // V MOTOR SIDE
+
+    // Apply the system demand to the motor via the gearbox ratio:
+    Axis.dojog = Axis.SysDerivedVel * Axis.ratio;
+
+    //Derive a motor postion demand
+    Axis.MotorDemandPos = Axis.MotorDemandPos + Axis.dojog;
+
+    if (Axis.Motor_position <= -Motor_wrap) {
+        // Motor is moving to wrap in a negative direction so bulk values to the mid point of the encoder
+        Axis.Motor_position = Axis.Motor_position + (Motor_wrap);
+        Axis.MotorDemandPos = Axis.MotorDemandPos + (Motor_wrap);
+        //pc.printf("Wrap neg");
+    } else if (Axis.Motor_position >= Motor_wrap) {
+        // Motor is moving to wrap in a positive direction so bulk values back towards the mid point
+        Axis.Motor_position = Axis.Motor_position - (Motor_wrap);
+        Axis.MotorDemandPos = Axis.MotorDemandPos - (Motor_wrap);
+        //pc.printf("Wrap pos");
+    }
+
+    // *********************************************************
+    // Apply proportional position loop gain
+    Axis.follow_error = Axis.MotorDemandPos - Axis.Motor_position;
+    Axis.Motor_final = (Axis.Kp * Axis.follow_error);
+
+    // *********************************************************
+    // Apply feed forward profiling:
+    // Gain Schedule according to system demand - this prevent stationary oscillation
+    if (fabs(Axis.SysProfilerVel) > 0) {
+        c = Axis.c_moving;
+    } else {
+        c = Axis.c_stationary;
+    }
+    m = (Axis.Jog_2 - c)/Axis.Jog_2;
+    // Calculate the feed forward value
+    if ((fabs(Axis.dojog) > (Axis.Jog_1 + 0)) && (fabs(Axis.dojog) <= Axis.Jog_2)) {
+        Axis.Feed_Forward = ((m*fabs(Axis.dojog)) + c);
+        if (Axis.dojog <= 0) {
+            Axis.Feed_Forward = -Axis.Feed_Forward;
+        }
+    } else if (fabs(Axis.dojog) > Axis.Jog_2) {
+        Axis.Feed_Forward = (Axis.Kff * Axis.dojog);
+    } else {
+        Axis.Feed_Forward = 0;
+    }
+    // Add the feedforward term to the demand already calculated:
+    Axis.Motor_final = Axis.Motor_final + Axis.Feed_Forward;
+
+    // *****************************************************
+    // NOW PREPARE AND SEND DEMAND TO THE MOTOR
+    // Apply scale factor from user units to motor duty cycle.
+    Duty_Cycle = Axis.Motor_final / Axis.Motor_scale;
+
+    if(Duty_Cycle < 0) {
+        Motor_Direction = 1;
+        Motor_PWM = 1 - ((Duty_Cycle - DeadBand) * -1.0);
+    } else {
+        Motor_Direction = 0;
+        Motor_PWM = 1 - Duty_Cycle - DeadBand;
+    }
+
+    // Send demand to the (correct) motor:
+    if (Axis.Number == 1) {
+        Pan_Motor_Direction = Motor_Direction;
+        Pan_Motor_PWM = Motor_PWM;
+    } else if (Axis.Number == 2) {
+        Tilt_Motor_Direction = Motor_Direction;
+        Tilt_Motor_PWM = Motor_PWM;
+    }
+
+// ***************************************************
+// CHOOSE VALUES AND SCOPE THEM
+    if ((scoping) && (Axis.Number == 2)) {
+        if (scope_time < 1) {
+            // pc.printf("\n\r \n\rTime,Motor_position,follow_error,Sys_follow_error,dojog,SysProfilerVel,sysdojog,Feed_Forward");
+            pc.printf("\n\r \n\rscope_time,Motor Error,Motor Virtual,Motor Position,System speed,Hall");
+        }
+        pc.printf("\n\r %d,%f,%f,%f,%f,%f",scope_time,Pan.follow_error, (Pan.follow_error / Pan.ratio),Pan.Motor_position, Pan.sysdojog, Pan_count);
+        scope_time = scope_time + 1;
+    }
+
+
+
+}
\ No newline at end of file

--- a/Classic_PID.lib	Tue Mar 22 15:35:01 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#972fa3842c17

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyAxis.cpp	Wed Jun 08 13:56:25 2016 +0000
@@ -0,0 +1,5 @@
+#include "MyAxis.h"
+
+
+MyAxis Pan;
+MyAxis Tilt;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyAxis.h	Wed Jun 08 13:56:25 2016 +0000
@@ -0,0 +1,55 @@
+//#ifndef MYAXIS_H
+#define MYAXIS_H
+
+// Axis Specific structure
+struct MyAxis {
+    int Number;    
+    float jog;
+    int sysjogdir;    
+    bool sinusoidrun;
+    bool scoping;
+    bool calcmyratio;
+    float dynamicratio;
+    bool setramp;
+    bool setramp_x;
+    bool moveme;
+    double ramp;
+    int run_count_jog;
+    float run_count;
+    float System_position;
+    float System_position_prev;
+    double Sys_follow_error;
+    double SysDemandPos;
+    double SysDemand;
+    double SysProfilerVel;
+    double SysDerivedVel;
+    double SysActualVel;
+    double SysDemand_Prev;
+    float start_SysPos;
+    double MotorDemand_DerivedVel;
+    float ratio;
+    float Motor_scale; //15728;  //(based on 1800RPM, 2^19 encoder and 0.001s loop)
+    double Motor_final;
+    float c_moving;
+    float c_stationary;
+    float Jog_1;
+    float Jog_2;
+    float Kp;
+    float Kp_Sys;
+    float Kff;
+    float Kff_Sys;
+    double follow_error;
+    float Feed_Forward;
+    float Prev_Motor_final;
+    int Motor_Inst_Pos;
+    double MotorDemandPos;
+    int MotorLast_Inst_Pos;
+    float Motor_Vel;
+    float Motor_position;
+    float dojog;
+    float sysdojog;
+    int jogdir;
+    float start_MotPos;
+};
+extern MyAxis Pan;
+extern MyAxis Tilt;

--- a/PanVelocityLoop.cpp	Tue Mar 22 15:35:01 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,72 +0,0 @@
-#include "mbed.h"
-#include "iC_MU.h"
-#include "rtos.h"
-#include "Classic_PID.h"
-
-// Define limits for zero crossing
-// These values should allow operation upto 3750 RPM
-#define bits 18                 // The number of bits we want to use
-#define OneTurn (1<<bits)       // 262144 counts per rev
-#define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
-#define Upper OneTurn - Lower   // 262144 - 8192 = 253952
-
-extern iC_MU icMu, icMu_Output;
-extern PwmOut Pan_Motor_PWM;
-extern DigitalOut Pan_Motor_Direction;
-extern Classic_PID PanVelocityPID;
-extern Serial pc;
-
-extern bool running;
-extern float amplitude;
-extern float a;
-extern float b;
-
-int LasticMuPosition = 0;
-int run_count = 0;
-
-void PanVelocityLoop(void const *args)
-{
-    float Duty_Cycle = 0.0;
-
-    // Find the icMu data
-    int icMuPosition = icMu.ReadPOSITION();          // Read the current position from the iC-MU
-    PanVelocityPID.setProcessValue(icMuPosition);    // Pass the Position onto the PID loop
-
-    //int icMuPosition = icMu.ReadPOSITION() >> (19 - bits);          // Read the current position from the iC-MU and bitshift to reduce noise
-    //int icMuVelocity = icMuPosition - LasticMuPosition;             // Calculate change in position (i.e. Velocity)
-
-    if(running) {
-        pc.printf("\n\r%d %d %d",run_count,icMuPosition,icMu_Output.ReadPOSITION());
-
-        if(run_count < 5000) {
-            PanVelocityPID.setSetPoint(262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count))));
-            run_count++;        // Inc run_count
-        } else {
-            running = 0;        // Stop the test
-            //PanVelocityPID.setSetPoint(0);
-        }
-    }
-    /*
-       // Check to see if the icMu has gone past the index point
-       if(icMuPosition < Lower & LasticMuPosition > Upper) {           // The icMu has gone over the index point in 1 direction
-           icMuVelocity += OneTurn;
-       } else if(icMuPosition > Upper & LasticMuPosition < Lower) {    // The icMu has gone over the index point in the other direction
-           icMuVelocity -= OneTurn;
-       }
-
-       LasticMuPosition = icMuPosition;                                // Update new position from next time
-
-       // Set the process value
-       PanVelocityPID.setProcessValue(icMuVelocity);                   // Pass the Velocity onto the PID loop
-    */
-
-    Duty_Cycle = PanVelocityPID.compute_ff();
-
-    if(Duty_Cycle < 0) {
-        Pan_Motor_Direction = 1;
-        Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
-    } else {
-        Pan_Motor_Direction = 0;
-        Pan_Motor_PWM = 1 - Duty_Cycle;
-    }
-}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pan_Feedback.cpp	Wed Jun 08 13:56:25 2016 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "rtos.h"
+
+float Pan_count = 1000;
+
+InterruptIn pan_pulser(p30); // Pulse counter for Pan axis in this case 10 counts per rev. NOTE!! this has to be 3.3V,
+
+void func_fall(void)
+{
+    // pc.printf("fall.\n\r");
+    //Pan_count = Pan_count -1000;
+    if (Pan_count > 0) {
+        Pan_count = 0;
+    } else {
+        Pan_count = 1000;
+    }
+}
+void func_rise(void)
+{
+    //pc.printf("rise.\n\r");
+    //Pan_count = Pan_count +1000;
+    if (Pan_count > 0) {
+        Pan_count = 0;
+    } else {
+        Pan_count = 1000;
+    }
+}
+
+void Pan_Counter(void const *args)
+{
+    pan_pulser.fall(&func_fall);
+    pan_pulser.rise(&func_rise);
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServiceKeyboard.cpp	Wed Jun 08 13:56:25 2016 +0000
@@ -0,0 +1,349 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "MyAxis.h"
+
+
+extern Serial pc;
+extern float DeadBand;
+extern bool scoping;
+extern int scope_duration;
+extern int scope_time;
+extern bool sinusoid;
+extern bool ratiocalc;
+extern float jogaccel_time;
+extern float jogaccel;
+extern int pos_enc_start;
+extern int run_count_jog;
+extern int primed;
+float temp;
+
+
+// Returns the new set point entered via the keyboard
+int ServiceKeyboard ()
+{
+    int key;                                            // The keypress by the user
+    int value = 0;                                      // The variable to return
+
+    key = pc.getc();                                    // Read the intial keypress
+
+    switch (key)   {
+// List keypresses used:
+        case 'q':
+            // To move up and down: 8, 2:  To move left and right 4,6. To start stop both: 5:
+            pc.printf("\n\r Left, Right, Up Down:  4,6,8,2.  \n\r Start & Stop both: 5 \n\r Toggle all directions = d \n\r");
+            pc.printf("\n\r Scope = s \n\r Run Test sinusoid = t \n\r Run Ratio Calculator = r \n\r Set Motor Kp = g \n\r Set Sys Kp_Sys = h \n\r Fetch Gain = f \n\r Set Kff_Sys = k \n\r Prime Start Scope = p \n\r Set Kff = 2 \n\r Set Gearbox Ratio = 1  \n\r");
+            pc.printf(" Set Stationary c value = c \n\r Set Moving c value = b \n\r Set Jog_2 value = 3 \n\r \n\r \n\r Jog Speed PAN,TILT = j,m \n\r Set Jog Acceleration = a \n\r Cycle to hard Stop = z \n\r Fade = x \n\r");
+            break;
+
+// Turn on Scope Tool:
+        case 's':
+            if (!scoping) {
+                scope_time = 0;
+                scoping = true;
+            } else {
+                scoping = false;
+            }
+            break;
+// Turn on Ratio Calculator:
+        case 'r':
+            if (!ratiocalc) {
+                ratiocalc = true;
+                pc.printf("\n\r Running Ratio Calculator");
+            } else {
+                Tilt.calcmyratio = false;
+                Pan.calcmyratio = false;
+            }
+            break;
+// We are going to jog Tilt up
+        case '8':
+            Tilt.sysjogdir = -1;
+            if (Tilt.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Tilt.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Tilt.moveme = true;
+            }
+            break;
+// We are going to jog Tilt down
+        case '2':
+            Tilt.sysjogdir = 1;
+            if (Tilt.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Tilt.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Tilt.moveme = true;
+            }
+            break;
+// We are going to jog Tilt up
+        case '4':
+            Pan.sysjogdir = -1;
+            if (Pan.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Pan.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Pan.moveme = true;
+            }
+            break;
+// We are going to jog Tilt down
+        case '6':
+            Pan.sysjogdir = 1;
+            if (Pan.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Pan.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Pan.moveme = true;
+            }
+            break;
+// We are going to jog everything
+        case '5':
+            if (Pan.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Pan.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Pan.moveme = true;
+            }
+            if (Tilt.moveme) {
+                //pc.printf("\n\r Moving stop");
+                Tilt.moveme = false;
+            } else {
+                //pc.printf("\n\r Moving Start");
+                Tilt.moveme = true;
+            }
+            break;
+// We are going to toggle all directions
+        case 'd':
+            Pan.sysjogdir = -Pan.sysjogdir;
+            Tilt.sysjogdir = -Tilt.sysjogdir;
+            break;
+
+// Turn on Potentiometer control:
+        case 't':
+            if (!sinusoid) {
+                pc.printf("\n\r Running sinusoid test");
+                sinusoid = true;
+            } else {
+                sinusoid = false;
+            }
+            break;
+// We are going to set the Motor gain
+        case 'g':
+            pc.printf("\n\r Enter a new Motor Kp gain x 1000: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.Kp = value;
+            Pan.Kp = Pan.Kp / 1000;
+            Tilt.Kp = Pan.Kp;
+            //PanVelocityPID.setKp(Kp/1000);
+            pc.printf("\n\r");
+            break;
+// We are going to set the System gain
+        case 'h':
+            pc.printf("\n\r Enter a new System Sys_Kp gain x 10000: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.Kp_Sys = value;
+            Pan.Kp_Sys = Pan.Kp_Sys / 10000;
+            Tilt.Kp_Sys = Pan.Kp_Sys;
+            //PanVelocityPID.setKp(Kp/1000);
+            pc.printf("\n\r");
+            break;
+// We are going to confirm what the gain is
+        case 'f':
+            //Kp = PanVelocityPID.getKp();
+            pc.printf("\n\r Kp = %f, Kff = %f, Kp_Sys = %f, Kff_Sys = %f, Ratio = %f, c_stationary = %f, c_moving = %f, Jog_2 = %f \n\r", Pan.Kp,Pan.Kff,Pan.Kp_Sys,Pan.Kff_Sys,Pan.ratio,Pan.c_stationary, Pan.c_moving, Pan.Jog_2);
+            break;
+
+// We are going to set the Jog_2 Value:
+        case '3':
+            pc.printf("\n\r Enter a new Jog_2 value: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.Jog_2 = value;
+            pc.printf("\n\r");
+            break;
+// We are going to set the new threshold:
+        case '1':
+            pc.printf("\n\r Enter a new gearbox ratio: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.ratio = value;
+            pc.printf("\n\r");
+            break;
+// We are going to set the Kff gain
+        case 'w':
+            pc.printf("\n\r Enter a new Kff gain x 100: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.Kff = value;
+            Pan.Kff = Pan.Kff / 100;
+            pc.printf("\n\r");
+            break;
+// We are going to set the dead band for the Motor PWM
+        case 'k':
+            pc.printf("\n\r Enter a new Kff_Sys x 100: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            temp = value;
+            Pan.Kff_Sys = temp / 100;
+            pc.printf("\n\r");
+            break;
+// We are going to set a stationary c value
+        case 'c':
+            pc.printf("\n\r Enter a new stationary c value: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.c_stationary = value;
+            pc.printf("\n\r");
+            break;
+// We are going to set a moving b value
+        case 'b':
+            pc.printf("\n\r Enter a new moving c value: ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.c_moving = value;
+            pc.printf("\n\r");
+            break;
+//  Set a jog speed
+        case 'j':
+            pc.printf("\n\r Set Jog PAN speed (Deg/s) x100 ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Pan.jog = value;
+            Pan.jog = (Pan.jog * 0.0145);
+            pc.printf("\n\r");
+            break;
+//  Set a jog speed
+        case 'm':
+            pc.printf("\n\r Set Jog TILT speed (Deg/s) x100 ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            Tilt.jog = value;
+            Tilt.jog = (Tilt.jog * 0.0145);
+            pc.printf("\n\r");
+            break;
+// Set the jog function to cycle to a stop
+        case 'z':
+            pc.printf("\n\r Set to ramp to stop \n\r");
+            Pan.ramp = Pan.jog / jogaccel_time;
+            Pan.run_count_jog = 0;
+            scope_time = 0;
+            Pan.setramp = true;
+            break;
+// Set a Fade like move
+        case 'x':
+            pc.printf("\n\r Set to Fade like move \n\r");
+            Pan.ramp = Pan.jog / jogaccel_time;
+            Pan.run_count_jog = 0;
+            scope_time = 0;
+            Pan.setramp_x = true;
+
+            Tilt.ramp = Tilt.jog / jogaccel_time;
+            Tilt.run_count_jog = 0;
+            scope_time = 0;
+            Tilt.setramp_x = true;
+            break;
+//  Set a jog acceleration
+        case 'a':
+            pc.printf("\n\r Set ramp jogaccel_time, (ms) ");
+            //key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            temp = value;
+            jogaccel_time = temp;
+            //ramp = ramp / 1000;
+            pc.printf("\n\r");
+            break;
+//  Prime the scope tool
+        case 'p':
+            pc.printf("\n\r Prime the scope tool \n\r");
+            primed = true;
+            break;
+    }
+    return(1);
+}
+
+
+
+
+

--- 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;
+        }
     }
 
 }