Vitec Group / Mbed 2 deprecated 1_AH10_Full_Test

As used for BB testing April / May 2016 (with the odd mod to the scope outputs)

Dependencies:   iC_MU mbed-rtos mbed

Fork of SweptSine by Vitec Group

Axis_Control.cpp@3:4deb86f4ccfe, 2016-06-08 (annotated)

Committer:
acodd
Date:
Wed Jun 08 13:53:50 2016 +0000
Revision:
3:4deb86f4ccfe
As used for much of head tuning process documented in "T3 head Tuning" document.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acodd 3:4deb86f4ccfe 1 #include "mbed.h"
acodd 3:4deb86f4ccfe 2 #include "iC_MU.h"
acodd 3:4deb86f4ccfe 3 #include "rtos.h"
acodd 3:4deb86f4ccfe 4 #include "MyAxis.h"
acodd 3:4deb86f4ccfe 5
acodd 3:4deb86f4ccfe 6 extern iC_MU pan_Motor, PanPos;
acodd 3:4deb86f4ccfe 7 extern PwmOut Pan_Motor_PWM;
acodd 3:4deb86f4ccfe 8
acodd 3:4deb86f4ccfe 9 extern iC_MU tilt_Motor, TiltPos;
acodd 3:4deb86f4ccfe 10 extern PwmOut Tilt_Motor_PWM;
acodd 3:4deb86f4ccfe 11
acodd 3:4deb86f4ccfe 12 extern DigitalOut Pan_Motor_Direction;
acodd 3:4deb86f4ccfe 13 extern DigitalOut Tilt_Motor_Direction;
acodd 3:4deb86f4ccfe 14
acodd 3:4deb86f4ccfe 15 extern Serial pc;
acodd 3:4deb86f4ccfe 16
acodd 3:4deb86f4ccfe 17 // Deal with profiling and scoping (generic)
acodd 3:4deb86f4ccfe 18 bool scoping;
acodd 3:4deb86f4ccfe 19 int scope_duration = 1000;
acodd 3:4deb86f4ccfe 20 int scope_time = 0;
acodd 3:4deb86f4ccfe 21 float amplitude;
acodd 3:4deb86f4ccfe 22 float a;
acodd 3:4deb86f4ccfe 23 float Hz;
acodd 3:4deb86f4ccfe 24 extern float start_Hz;
acodd 3:4deb86f4ccfe 25 extern float stop_Hz;
acodd 3:4deb86f4ccfe 26 extern float run_time;
acodd 3:4deb86f4ccfe 27 float jogaccel_time = 2000;
acodd 3:4deb86f4ccfe 28 float jogaccel = 1;
acodd 3:4deb86f4ccfe 29 float dwell = 1000;
acodd 3:4deb86f4ccfe 30 float bits = 19;
acodd 3:4deb86f4ccfe 31 int OneTurnCts = pow(2, bits); //491520
acodd 3:4deb86f4ccfe 32 int MyLower = pow(2,(bits-4));
acodd 3:4deb86f4ccfe 33 int MyUpper = OneTurnCts - MyLower;
acodd 3:4deb86f4ccfe 34 int skip = 0;
acodd 3:4deb86f4ccfe 35 int primed = 0;
acodd 3:4deb86f4ccfe 36
acodd 3:4deb86f4ccfe 37
acodd 3:4deb86f4ccfe 38 // Motor Generics
acodd 3:4deb86f4ccfe 39 float c;
acodd 3:4deb86f4ccfe 40 float m;
acodd 3:4deb86f4ccfe 41 float Duty_Cycle = 0;
acodd 3:4deb86f4ccfe 42 float Motor_Direction;
acodd 3:4deb86f4ccfe 43 float Motor_PWM;
acodd 3:4deb86f4ccfe 44 float DeadBand = 0.012;
acodd 3:4deb86f4ccfe 45 int Motor_wrap = 4194304;
acodd 3:4deb86f4ccfe 46
acodd 3:4deb86f4ccfe 47
acodd 3:4deb86f4ccfe 48 void PanAxisInitialise()
acodd 3:4deb86f4ccfe 49 {
acodd 3:4deb86f4ccfe 50 //MyAxis Pan;
acodd 3:4deb86f4ccfe 51 Pan.Number = 1;
acodd 3:4deb86f4ccfe 52 Pan.jog = 3;
acodd 3:4deb86f4ccfe 53 Pan.sysjogdir = 1;
acodd 3:4deb86f4ccfe 54 Pan.sinusoidrun = 0;
acodd 3:4deb86f4ccfe 55 Pan.scoping = 0;
acodd 3:4deb86f4ccfe 56 Pan.setramp = 0;
acodd 3:4deb86f4ccfe 57 Pan.setramp_x = 0;
acodd 3:4deb86f4ccfe 58 Pan.moveme = 0;
acodd 3:4deb86f4ccfe 59 Pan.ramp = 0.05;
acodd 3:4deb86f4ccfe 60 Pan.run_count = 0;
acodd 3:4deb86f4ccfe 61 Pan.ratio = 115; //This is 115 on original breadboard, -113 on a prototype MS5
acodd 3:4deb86f4ccfe 62 Pan.Motor_scale = 11100; //15728; //(based on 1800RPM, 2^19 encoder and 0.001s loop)
acodd 3:4deb86f4ccfe 63 Pan.c_moving = 20;
acodd 3:4deb86f4ccfe 64 Pan.c_stationary = 2;
acodd 3:4deb86f4ccfe 65 Pan.Jog_1 = 0;
acodd 3:4deb86f4ccfe 66 Pan.Jog_2 = 60;
acodd 3:4deb86f4ccfe 67 Pan.Kp = 0.060;
acodd 3:4deb86f4ccfe 68 Pan.Kp_Sys = 0.0005;
acodd 3:4deb86f4ccfe 69 Pan.Kff = 1;
acodd 3:4deb86f4ccfe 70 Pan.Kff_Sys = 1;
acodd 3:4deb86f4ccfe 71 Pan.Feed_Forward = 0;
acodd 3:4deb86f4ccfe 72 Pan.Prev_Motor_final = 0;
acodd 3:4deb86f4ccfe 73 Pan.Motor_Vel = 0;
acodd 3:4deb86f4ccfe 74 Pan.Motor_position = 0;
acodd 3:4deb86f4ccfe 75 Pan.dojog = 0;
acodd 3:4deb86f4ccfe 76 Pan.sysdojog = 0;
acodd 3:4deb86f4ccfe 77 Pan.jogdir = 1;
acodd 3:4deb86f4ccfe 78 Pan.sysjogdir = 1;
acodd 3:4deb86f4ccfe 79 Pan.start_MotPos = 0;
acodd 3:4deb86f4ccfe 80
acodd 3:4deb86f4ccfe 81
acodd 3:4deb86f4ccfe 82 // Set up the Pan motor
acodd 3:4deb86f4ccfe 83 Pan_Motor_PWM.period_us(50); // Set PWM to 20 kHz
acodd 3:4deb86f4ccfe 84 Pan_Motor_PWM = 1; // Start with motor static
acodd 3:4deb86f4ccfe 85 Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();
acodd 3:4deb86f4ccfe 86 Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;
acodd 3:4deb86f4ccfe 87
acodd 3:4deb86f4ccfe 88 Pan.Motor_position = 0;
acodd 3:4deb86f4ccfe 89 Pan.MotorDemandPos = 0;
acodd 3:4deb86f4ccfe 90 Pan.System_position = PanPos.ReadPOSITION();
acodd 3:4deb86f4ccfe 91 Pan.SysDemandPos = Pan.System_position;
acodd 3:4deb86f4ccfe 92 Pan.SysDemand_Prev = Pan.SysDemandPos;
acodd 3:4deb86f4ccfe 93 Pan.Sys_follow_error = Pan.SysDemandPos - Pan.System_position;
acodd 3:4deb86f4ccfe 94 //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
acodd 3:4deb86f4ccfe 95 }
acodd 3:4deb86f4ccfe 96
acodd 3:4deb86f4ccfe 97 void TiltAxisInitialise()
acodd 3:4deb86f4ccfe 98 {
acodd 3:4deb86f4ccfe 99 //MyAxis Pan;
acodd 3:4deb86f4ccfe 100 Tilt.Number = 2;
acodd 3:4deb86f4ccfe 101 Tilt.jog = 3;
acodd 3:4deb86f4ccfe 102 Tilt.sysjogdir = 1;
acodd 3:4deb86f4ccfe 103 Tilt.sinusoidrun = 0;
acodd 3:4deb86f4ccfe 104 Tilt.scoping = 0;
acodd 3:4deb86f4ccfe 105 Tilt.setramp = 0;
acodd 3:4deb86f4ccfe 106 Tilt.setramp_x = 0;
acodd 3:4deb86f4ccfe 107 Tilt.moveme = 0;
acodd 3:4deb86f4ccfe 108 Tilt.ramp = 0.05;
acodd 3:4deb86f4ccfe 109 Tilt.run_count = 0;
acodd 3:4deb86f4ccfe 110 Tilt.ratio = -115; // This is -115 on original prototype unit, -113 on a prototype MS5)
acodd 3:4deb86f4ccfe 111 Tilt.Motor_scale = 11100; //15728; //(based on 1800RPM, 2^19 encoder and 0.001s loop)
acodd 3:4deb86f4ccfe 112 Tilt.c_moving = 20;
acodd 3:4deb86f4ccfe 113 Tilt.c_stationary = 2;
acodd 3:4deb86f4ccfe 114 Tilt.Jog_1 = 0;
acodd 3:4deb86f4ccfe 115 Tilt.Jog_2 = 60;
acodd 3:4deb86f4ccfe 116 Tilt.Kp = 0.06;
acodd 3:4deb86f4ccfe 117 Tilt.Kp_Sys = 0.0005;
acodd 3:4deb86f4ccfe 118 Tilt.Kff = 1;
acodd 3:4deb86f4ccfe 119 Tilt.Kff_Sys = 1;
acodd 3:4deb86f4ccfe 120 Tilt.Feed_Forward = 0;
acodd 3:4deb86f4ccfe 121 Tilt.Prev_Motor_final = 0;
acodd 3:4deb86f4ccfe 122 Tilt.Motor_Vel = 0;
acodd 3:4deb86f4ccfe 123 Tilt.Motor_position = 0;
acodd 3:4deb86f4ccfe 124 Tilt.dojog = 0;
acodd 3:4deb86f4ccfe 125 Tilt.sysdojog = 0;
acodd 3:4deb86f4ccfe 126 Tilt.jogdir = 1;
acodd 3:4deb86f4ccfe 127 Tilt.sysjogdir = 1;
acodd 3:4deb86f4ccfe 128 Tilt.start_MotPos = 0;
acodd 3:4deb86f4ccfe 129
acodd 3:4deb86f4ccfe 130
acodd 3:4deb86f4ccfe 131 // Set up the Tilt motor
acodd 3:4deb86f4ccfe 132 Tilt_Motor_PWM.period_us(50); // Set PWM to 20 kHz
acodd 3:4deb86f4ccfe 133 Tilt_Motor_PWM = 1; // Start with motor static
acodd 3:4deb86f4ccfe 134 Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();
acodd 3:4deb86f4ccfe 135 Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;
acodd 3:4deb86f4ccfe 136
acodd 3:4deb86f4ccfe 137 Tilt.Motor_position = 0;
acodd 3:4deb86f4ccfe 138 Tilt.MotorDemandPos = 0;
acodd 3:4deb86f4ccfe 139 Tilt.System_position = TiltPos.ReadPOSITION();
acodd 3:4deb86f4ccfe 140 Tilt.SysDemandPos = Tilt.System_position;
acodd 3:4deb86f4ccfe 141 Tilt.SysDemand_Prev = Tilt.SysDemandPos;
acodd 3:4deb86f4ccfe 142 Tilt.Sys_follow_error = Tilt.SysDemandPos - Tilt.System_position;
acodd 3:4deb86f4ccfe 143 //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
acodd 3:4deb86f4ccfe 144 }
acodd 3:4deb86f4ccfe 145
acodd 3:4deb86f4ccfe 146 void UpdatePanFeedback()
acodd 3:4deb86f4ccfe 147 {
acodd 3:4deb86f4ccfe 148 Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;
acodd 3:4deb86f4ccfe 149 Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();
acodd 3:4deb86f4ccfe 150
acodd 3:4deb86f4ccfe 151 Pan.Motor_Vel = (Pan.Motor_Inst_Pos - Pan.MotorLast_Inst_Pos);
acodd 3:4deb86f4ccfe 152
acodd 3:4deb86f4ccfe 153 // Check to see if we have gone past the index point
acodd 3:4deb86f4ccfe 154 if( (Pan.Motor_Inst_Pos < MyLower) && (Pan.MotorLast_Inst_Pos > MyUpper) ) { // We have gone over the index point in 1 direction
acodd 3:4deb86f4ccfe 155 Pan.Motor_Vel += OneTurnCts;
acodd 3:4deb86f4ccfe 156 } else if( (Pan.Motor_Inst_Pos > MyUpper) && (Pan.MotorLast_Inst_Pos < MyLower) ) { // We have gone over the index point in the other direction
acodd 3:4deb86f4ccfe 157 Pan.Motor_Vel -= OneTurnCts;
acodd 3:4deb86f4ccfe 158 }
acodd 3:4deb86f4ccfe 159
acodd 3:4deb86f4ccfe 160 Pan.Motor_position = Pan.Motor_position + Pan.Motor_Vel;
acodd 3:4deb86f4ccfe 161
acodd 3:4deb86f4ccfe 162 Pan.System_position_prev = Pan.System_position;
acodd 3:4deb86f4ccfe 163 Pan.System_position = PanPos.ReadPOSITION();
acodd 3:4deb86f4ccfe 164
acodd 3:4deb86f4ccfe 165 Pan.SysActualVel = Pan.System_position - Pan.System_position_prev;
acodd 3:4deb86f4ccfe 166
acodd 3:4deb86f4ccfe 167 }
acodd 3:4deb86f4ccfe 168 void UpdateTiltFeedback()
acodd 3:4deb86f4ccfe 169 {
acodd 3:4deb86f4ccfe 170 Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;
acodd 3:4deb86f4ccfe 171 Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();
acodd 3:4deb86f4ccfe 172
acodd 3:4deb86f4ccfe 173 Tilt.Motor_Vel = (Tilt.Motor_Inst_Pos - Tilt.MotorLast_Inst_Pos);
acodd 3:4deb86f4ccfe 174
acodd 3:4deb86f4ccfe 175 // Check to see if we have gone past the index point
acodd 3:4deb86f4ccfe 176 if( (Tilt.Motor_Inst_Pos < MyLower) && (Tilt.MotorLast_Inst_Pos > MyUpper) ) { // We have gone over the index point in 1 direction
acodd 3:4deb86f4ccfe 177 Tilt.Motor_Vel += OneTurnCts;
acodd 3:4deb86f4ccfe 178 } else if( (Tilt.Motor_Inst_Pos > MyUpper) && (Tilt.MotorLast_Inst_Pos < MyLower) ) { // We have gone over the index point in the other direction
acodd 3:4deb86f4ccfe 179 Tilt.Motor_Vel -= OneTurnCts;
acodd 3:4deb86f4ccfe 180 }
acodd 3:4deb86f4ccfe 181
acodd 3:4deb86f4ccfe 182 Tilt.Motor_position = Tilt.Motor_position + Tilt.Motor_Vel;
acodd 3:4deb86f4ccfe 183
acodd 3:4deb86f4ccfe 184 // Tilt.System_position_prev = Tilt.System_position;
acodd 3:4deb86f4ccfe 185 Tilt.System_position = TiltPos.ReadPOSITION();
acodd 3:4deb86f4ccfe 186
acodd 3:4deb86f4ccfe 187 // Tilt.SysActualVel = Tilt.System_position - Tilt.System_position_prev;
acodd 3:4deb86f4ccfe 188
acodd 3:4deb86f4ccfe 189 }
acodd 3:4deb86f4ccfe 190
acodd 3:4deb86f4ccfe 191 void AxisControlLoop(MyAxis& Axis)
acodd 3:4deb86f4ccfe 192 {
acodd 3:4deb86f4ccfe 193 if (Axis.Number == 1) {
acodd 3:4deb86f4ccfe 194 UpdatePanFeedback();
acodd 3:4deb86f4ccfe 195 } else if (Axis.Number == 2) {
acodd 3:4deb86f4ccfe 196 UpdateTiltFeedback();
acodd 3:4deb86f4ccfe 197 }
acodd 3:4deb86f4ccfe 198
acodd 3:4deb86f4ccfe 199
acodd 3:4deb86f4ccfe 200 // ***********************************************************
acodd 3:4deb86f4ccfe 201 // Sinusoid test
acodd 3:4deb86f4ccfe 202
acodd 3:4deb86f4ccfe 203 if ((Axis.sinusoidrun) && (Axis.jog > 0)) {
acodd 3:4deb86f4ccfe 204 //pc.printf("\n\r%d,%d,%f,%f",run_count,(Motor_Inst_Pos-262144),outprint,(b*1000));
acodd 3:4deb86f4ccfe 205 //Time (ms),Motor,System,SysDemandPos,frequency (Hz),Demand")
acodd 3:4deb86f4ccfe 206 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));
acodd 3:4deb86f4ccfe 207
acodd 3:4deb86f4ccfe 208 if(Axis.run_count < (run_time * 1000)) {
acodd 3:4deb86f4ccfe 209 // demandpos = 262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count)));
acodd 3:4deb86f4ccfe 210 Hz = (start_Hz + (a * Axis.run_count)); // This is the demand frequency
acodd 3:4deb86f4ccfe 211 //SysDemandPos = SysDemandPos + (amplitude * sin(2 * 3.14159 * Hz * run_count / 1000));
acodd 3:4deb86f4ccfe 212 Axis.SysDemandPos = Axis.start_SysPos + (amplitude * sin(2 * 3.14159 * Hz * Axis.run_count / 1000));
acodd 3:4deb86f4ccfe 213 // PanVelocityPID.setSetPoint(demandpos);
acodd 3:4deb86f4ccfe 214 Axis.run_count++; // Inc run_count
acodd 3:4deb86f4ccfe 215 } else {
acodd 3:4deb86f4ccfe 216 Axis.sinusoidrun = 0; // Stop the test
acodd 3:4deb86f4ccfe 217 Axis.run_count = 0;
acodd 3:4deb86f4ccfe 218 }
acodd 3:4deb86f4ccfe 219 }
acodd 3:4deb86f4ccfe 220
acodd 3:4deb86f4ccfe 221 // ***********************************************************
acodd 3:4deb86f4ccfe 222 // Calculate the effective ratio on the fly
acodd 3:4deb86f4ccfe 223
acodd 3:4deb86f4ccfe 224 if ((Axis.calcmyratio) && (fabs(Axis.SysActualVel) > 0)) {
acodd 3:4deb86f4ccfe 225
acodd 3:4deb86f4ccfe 226 Axis.dynamicratio = fabs((Axis.Motor_Vel) / (Axis.SysActualVel));
acodd 3:4deb86f4ccfe 227
acodd 3:4deb86f4ccfe 228
acodd 3:4deb86f4ccfe 229 }
acodd 3:4deb86f4ccfe 230
acodd 3:4deb86f4ccfe 231 // ***********************************************************
acodd 3:4deb86f4ccfe 232 // Go ahead and move at a set speed
acodd 3:4deb86f4ccfe 233
acodd 3:4deb86f4ccfe 234 if (Axis.moveme) {
acodd 3:4deb86f4ccfe 235 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.jog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 236 }
acodd 3:4deb86f4ccfe 237
acodd 3:4deb86f4ccfe 238 // ***********************************************************
acodd 3:4deb86f4ccfe 239 // Create set ramps to follow
acodd 3:4deb86f4ccfe 240 // sysjog is System speed demand in counts/ms at the System encoder (output)
acodd 3:4deb86f4ccfe 241 if (Axis.setramp) {
acodd 3:4deb86f4ccfe 242 if (Axis.run_count_jog < jogaccel_time) {
acodd 3:4deb86f4ccfe 243 if (primed) {
acodd 3:4deb86f4ccfe 244 scoping = true;
acodd 3:4deb86f4ccfe 245 primed = false;
acodd 3:4deb86f4ccfe 246 }
acodd 3:4deb86f4ccfe 247 Axis.sysjogdir = 1;
acodd 3:4deb86f4ccfe 248 Axis.sysdojog = Axis.sysdojog + Axis.ramp;
acodd 3:4deb86f4ccfe 249 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 250 Axis.SysDemandPos = Axis.SysDemandPos + Axis.sysdojog;
acodd 3:4deb86f4ccfe 251 Axis.SysDemandPos = Axis.SysDemandPos * Axis.sysjogdir;
acodd 3:4deb86f4ccfe 252 } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
acodd 3:4deb86f4ccfe 253 Axis.sysdojog = Axis.jog;
acodd 3:4deb86f4ccfe 254 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 255 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 256 } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
acodd 3:4deb86f4ccfe 257 Axis.sysdojog = Axis.sysdojog - Axis.ramp;
acodd 3:4deb86f4ccfe 258 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 259 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 260 } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3 * jogaccel_time) + dwell))) {
acodd 3:4deb86f4ccfe 261 Axis.sysdojog = Axis.sysdojog + Axis.ramp;
acodd 3:4deb86f4ccfe 262 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 263 Axis.sysjogdir = -1;
acodd 3:4deb86f4ccfe 264 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 265 } else if ((Axis.run_count_jog >= ((3* jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3* jogaccel_time) + dwell + dwell))) {
acodd 3:4deb86f4ccfe 266 Axis.sysdojog = 0;
acodd 3:4deb86f4ccfe 267 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 268 } else if (Axis.run_count_jog >= ((3* jogaccel_time) + dwell + dwell)) {
acodd 3:4deb86f4ccfe 269 Axis.setramp = false;
acodd 3:4deb86f4ccfe 270 Axis.run_count_jog = 0;
acodd 3:4deb86f4ccfe 271 Axis.sysjogdir = 1;
acodd 3:4deb86f4ccfe 272 scoping = false;
acodd 3:4deb86f4ccfe 273 }
acodd 3:4deb86f4ccfe 274 }
acodd 3:4deb86f4ccfe 275
acodd 3:4deb86f4ccfe 276 if (Axis.setramp_x) {
acodd 3:4deb86f4ccfe 277 if (Axis.run_count_jog < jogaccel_time) {
acodd 3:4deb86f4ccfe 278 if (primed) {
acodd 3:4deb86f4ccfe 279 scoping = true;
acodd 3:4deb86f4ccfe 280 primed = false;
acodd 3:4deb86f4ccfe 281 }
acodd 3:4deb86f4ccfe 282 Axis.sysdojog = Axis.sysdojog + Axis.ramp;
acodd 3:4deb86f4ccfe 283 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 284 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 285 } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
acodd 3:4deb86f4ccfe 286 Axis.sysdojog = Axis.jog;
acodd 3:4deb86f4ccfe 287 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 288 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 289 } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
acodd 3:4deb86f4ccfe 290 Axis.sysdojog = Axis.sysdojog - Axis.ramp;
acodd 3:4deb86f4ccfe 291 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 292 Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
acodd 3:4deb86f4ccfe 293 } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell + dwell))) {
acodd 3:4deb86f4ccfe 294 Axis.sysdojog = 0;
acodd 3:4deb86f4ccfe 295 Axis.run_count_jog++;
acodd 3:4deb86f4ccfe 296 } else if (Axis.run_count_jog >= ((2* jogaccel_time) + dwell + dwell)) {
acodd 3:4deb86f4ccfe 297 Axis.setramp_x = false;
acodd 3:4deb86f4ccfe 298 Axis.run_count_jog = 0;
acodd 3:4deb86f4ccfe 299 scoping = false;
acodd 3:4deb86f4ccfe 300 }
acodd 3:4deb86f4ccfe 301 }
acodd 3:4deb86f4ccfe 302
acodd 3:4deb86f4ccfe 303
acodd 3:4deb86f4ccfe 304 // **********************************************************
acodd 3:4deb86f4ccfe 305 // Generate perfect velocity demand for the System:
acodd 3:4deb86f4ccfe 306 Axis.SysProfilerVel = Axis.SysDemandPos - Axis.SysDemand_Prev;
acodd 3:4deb86f4ccfe 307 Axis.SysDemand_Prev = Axis.SysDemandPos;
acodd 3:4deb86f4ccfe 308
acodd 3:4deb86f4ccfe 309 // **********************************************************
acodd 3:4deb86f4ccfe 310 // Apply proportional gain to the System Velocity demand if sufficient
acodd 3:4deb86f4ccfe 311 Axis.Sys_follow_error = Axis.System_position - Axis.SysDemandPos;
acodd 3:4deb86f4ccfe 312 Axis.SysDerivedVel = Axis.SysProfilerVel - (Axis.Sys_follow_error * Axis.Kp_Sys);
acodd 3:4deb86f4ccfe 313
acodd 3:4deb86f4ccfe 314 // ^ LOAD SIDE
acodd 3:4deb86f4ccfe 315 // *************************************************
acodd 3:4deb86f4ccfe 316 // MOTOR CONTROL LOOP
acodd 3:4deb86f4ccfe 317 // *************************************************
acodd 3:4deb86f4ccfe 318 // V MOTOR SIDE
acodd 3:4deb86f4ccfe 319
acodd 3:4deb86f4ccfe 320 // Apply the system demand to the motor via the gearbox ratio:
acodd 3:4deb86f4ccfe 321 Axis.dojog = Axis.SysDerivedVel * Axis.ratio;
acodd 3:4deb86f4ccfe 322
acodd 3:4deb86f4ccfe 323 //Derive a motor postion demand
acodd 3:4deb86f4ccfe 324 Axis.MotorDemandPos = Axis.MotorDemandPos + Axis.dojog;
acodd 3:4deb86f4ccfe 325
acodd 3:4deb86f4ccfe 326 if (Axis.Motor_position <= -Motor_wrap) {
acodd 3:4deb86f4ccfe 327 // Motor is moving to wrap in a negative direction so bulk values to the mid point of the encoder
acodd 3:4deb86f4ccfe 328 Axis.Motor_position = Axis.Motor_position + (Motor_wrap);
acodd 3:4deb86f4ccfe 329 Axis.MotorDemandPos = Axis.MotorDemandPos + (Motor_wrap);
acodd 3:4deb86f4ccfe 330 //pc.printf("Wrap neg");
acodd 3:4deb86f4ccfe 331 } else if (Axis.Motor_position >= Motor_wrap) {
acodd 3:4deb86f4ccfe 332 // Motor is moving to wrap in a positive direction so bulk values back towards the mid point
acodd 3:4deb86f4ccfe 333 Axis.Motor_position = Axis.Motor_position - (Motor_wrap);
acodd 3:4deb86f4ccfe 334 Axis.MotorDemandPos = Axis.MotorDemandPos - (Motor_wrap);
acodd 3:4deb86f4ccfe 335 //pc.printf("Wrap pos");
acodd 3:4deb86f4ccfe 336 }
acodd 3:4deb86f4ccfe 337
acodd 3:4deb86f4ccfe 338 // *********************************************************
acodd 3:4deb86f4ccfe 339 // Apply proportional position loop gain
acodd 3:4deb86f4ccfe 340 Axis.follow_error = Axis.MotorDemandPos - Axis.Motor_position;
acodd 3:4deb86f4ccfe 341 Axis.Motor_final = (Axis.Kp * Axis.follow_error);
acodd 3:4deb86f4ccfe 342
acodd 3:4deb86f4ccfe 343 // *********************************************************
acodd 3:4deb86f4ccfe 344 // Apply feed forward profiling:
acodd 3:4deb86f4ccfe 345 // Gain Schedule according to system demand - this prevent stationary oscillation
acodd 3:4deb86f4ccfe 346 if (fabs(Axis.SysProfilerVel) > 0) {
acodd 3:4deb86f4ccfe 347 c = Axis.c_moving;
acodd 3:4deb86f4ccfe 348 } else {
acodd 3:4deb86f4ccfe 349 c = Axis.c_stationary;
acodd 3:4deb86f4ccfe 350 }
acodd 3:4deb86f4ccfe 351 m = (Axis.Jog_2 - c)/Axis.Jog_2;
acodd 3:4deb86f4ccfe 352 // Calculate the feed forward value
acodd 3:4deb86f4ccfe 353 if ((fabs(Axis.dojog) > (Axis.Jog_1 + 0)) && (fabs(Axis.dojog) <= Axis.Jog_2)) {
acodd 3:4deb86f4ccfe 354 Axis.Feed_Forward = ((m*fabs(Axis.dojog)) + c);
acodd 3:4deb86f4ccfe 355 if (Axis.dojog <= 0) {
acodd 3:4deb86f4ccfe 356 Axis.Feed_Forward = -Axis.Feed_Forward;
acodd 3:4deb86f4ccfe 357 }
acodd 3:4deb86f4ccfe 358 } else if (fabs(Axis.dojog) > Axis.Jog_2) {
acodd 3:4deb86f4ccfe 359 Axis.Feed_Forward = (Axis.Kff * Axis.dojog);
acodd 3:4deb86f4ccfe 360 } else {
acodd 3:4deb86f4ccfe 361 Axis.Feed_Forward = 0;
acodd 3:4deb86f4ccfe 362 }
acodd 3:4deb86f4ccfe 363 // Add the feedforward term to the demand already calculated:
acodd 3:4deb86f4ccfe 364 Axis.Motor_final = Axis.Motor_final + Axis.Feed_Forward;
acodd 3:4deb86f4ccfe 365
acodd 3:4deb86f4ccfe 366 // *****************************************************
acodd 3:4deb86f4ccfe 367 // NOW PREPARE AND SEND DEMAND TO THE MOTOR
acodd 3:4deb86f4ccfe 368 // Apply scale factor from user units to motor duty cycle.
acodd 3:4deb86f4ccfe 369 Duty_Cycle = Axis.Motor_final / Axis.Motor_scale;
acodd 3:4deb86f4ccfe 370
acodd 3:4deb86f4ccfe 371 if(Duty_Cycle < 0) {
acodd 3:4deb86f4ccfe 372 Motor_Direction = 1;
acodd 3:4deb86f4ccfe 373 Motor_PWM = 1 - ((Duty_Cycle - DeadBand) * -1.0);
acodd 3:4deb86f4ccfe 374 } else {
acodd 3:4deb86f4ccfe 375 Motor_Direction = 0;
acodd 3:4deb86f4ccfe 376 Motor_PWM = 1 - Duty_Cycle - DeadBand;
acodd 3:4deb86f4ccfe 377 }
acodd 3:4deb86f4ccfe 378
acodd 3:4deb86f4ccfe 379 // Send demand to the (correct) motor:
acodd 3:4deb86f4ccfe 380 if (Axis.Number == 1) {
acodd 3:4deb86f4ccfe 381 Pan_Motor_Direction = Motor_Direction;
acodd 3:4deb86f4ccfe 382 Pan_Motor_PWM = Motor_PWM;
acodd 3:4deb86f4ccfe 383 } else if (Axis.Number == 2) {
acodd 3:4deb86f4ccfe 384 Tilt_Motor_Direction = Motor_Direction;
acodd 3:4deb86f4ccfe 385 Tilt_Motor_PWM = Motor_PWM;
acodd 3:4deb86f4ccfe 386 }
acodd 3:4deb86f4ccfe 387
acodd 3:4deb86f4ccfe 388 // ***************************************************
acodd 3:4deb86f4ccfe 389 // CHOOSE VALUES AND SCOPE THEM
acodd 3:4deb86f4ccfe 390 if ((scoping) && (Axis.Number == 2)) {
acodd 3:4deb86f4ccfe 391 if (scope_time < 1) {
acodd 3:4deb86f4ccfe 392 // pc.printf("\n\r \n\rTime,Motor_position,follow_error,Sys_follow_error,dojog,SysProfilerVel,sysdojog,Feed_Forward");
acodd 3:4deb86f4ccfe 393 pc.printf("\n\r \n\rscope_time,Pan Sys Error,Pan Motor Error,Tilt Sys error,Tilt Motor Error, Pan Virtual, Tilt Virtual");
acodd 3:4deb86f4ccfe 394 }
acodd 3:4deb86f4ccfe 395 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));
acodd 3:4deb86f4ccfe 396 scope_time = scope_time + 1;
acodd 3:4deb86f4ccfe 397 }
acodd 3:4deb86f4ccfe 398
acodd 3:4deb86f4ccfe 399
acodd 3:4deb86f4ccfe 400
acodd 3:4deb86f4ccfe 401 }