Diff: Axis_Control.cpp

Revision:

3:4deb86f4ccfe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis_Control.cpp	Wed Jun 08 13:53:50 2016 +0000
@@ -0,0 +1,401 @@
+#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;
+
+// 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.0005;
+    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,Pan Sys Error,Pan Motor Error,Tilt Sys error,Tilt Motor Error, Pan Virtual, Tilt Virtual");
+        }
+        pc.printf("\n\r %d,%f,%f,%f,%f,%f,%f",scope_time,Pan.Sys_follow_error,Pan.follow_error,Tilt.Sys_follow_error,Tilt.follow_error, (Pan.follow_error / Pan.ratio), (Tilt.follow_error / Tilt.ratio));
+        scope_time = scope_time + 1;
+    }
+
+
+
+}
\ No newline at end of file