TiltVelocityLoop.cpp

00001 #include "mbed.h"
00002 #include "iC_MU.h"
00003 #include "rtos.h"
00004 #include "Classic_PID.h"
00005 
00006 // Define limits for zero crossing
00007 // These values should allow operation upto 3750 RPM
00008 #define bits 18                 // The number of bits we want to use
00009 #define OneTurn (1<<bits)       // 262144 counts per rev
00010 #define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
00011 #define Upper OneTurn - Lower   // 262144 - 8192 = 253952
00012 
00013 extern Serial pc;
00014 extern iC_MU tilt_ic_mu;
00015 extern iC_MU TiltPos;
00016 extern PwmOut Tilt_Motor_PWM;
00017 extern DigitalOut Tilt_Motor_Direction;
00018 extern Classic_PID TiltVelocityPID;
00019 extern float Demand_Count_Rate;
00020 extern float Tilt_motor_max_count_rate;
00021 extern float Actual_Motor_Speed;
00022 
00023 extern float T_Position; // True Tilt Position (Degrees)
00024 extern float T_Encoder_sf;
00025 
00026 
00027 int Last_M_Position = 0;
00028 int M_Position = 0;
00029 int Velocity = 0;
00030 float Duty_Cycle = 0.0;
00031 
00032 // System Configuration:
00033 float MaxSpeed = 60.00; // 5 Deg/s, 1250 RPM
00034 float Tilt_motor_max_count_rate = 6000; //encoder counts / ms
00035 float T_Position = 0; // True Tilt Position (Degrees)
00036 float T_Encoder_sf = 1456.355;  // counts per degree
00037 float T_Gear_ratio = 125;  // 125:1
00038 float M_Encoder = 262144; // counts/rev
00039 float Motor_sf = ((1 * T_Gear_ratio) * (M_Encoder / 360))/1000 ;  // This is the scale factor from deg/s at payload to motor counts/ms
00040 bool M_Encoder_front = true; // or 0 depending on the encoder configuration on the motor (facing to motor, or facing away from motor)
00041 
00042 double drive = 0;  // this is a const speed demand straight into the loop (divided by 100)
00043 
00044 // Main servo loop
00045 int DoMove = 0;
00046 const float LOOPs = 0.001;
00047 // S_Ramp Fade values
00048 float D = 10; // Fade distance
00049 float T = 15; // Fade time
00050 float dir = 1; //direction flag
00051 float ta; // The actual value used after sanity checks
00052 float ts; // The actual value used after sanity checks(S ramping)
00053 float tsfade = 0.5; // segment time for S ramped fade
00054 float tscut = 0.3; // segment time for S ramped cut
00055 float j; // jerk value for fade
00056 float aj; // accel value when S ramping
00057 float Vp;  // Top speed for the move Deg/s @ load (125:1 Ratio to motor)
00058 float Vs;  // Speed step increment
00059 float Da;  // Accel distance
00060 float Ds; // Distance convered at steady speed
00061 float s;  // Profiler internal demand speed (always positive)
00062 float sout;  // Demand as applied by the Vff term
00063 float s_profile; // output demand speed to postion loop + or -)
00064 double P;  // Profiler Demand postion
00065 float fadetime; // this will retain the current fade time
00066 float Error;    // Current position vs the profiler position
00067 float Vff = 1;  // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
00068 float T_Kp = 6;  //  This is is multiplied by the position error and added to the motor demand
00069 float Prop;   // The demand created by the Kp and error calculation
00070 float demand = 0;  // The value sento to the motor to make it move
00071 float P_vel;
00072 float Va; // mid speed point
00073 float as; // acceleration value during linear accel stage
00074 float Vj; // Speed at bottom intersection
00075 float Vjp; // Speed at top intersection
00076 float c; // constant for up ramp y=mx+c
00077 float b; // constant for down ramp y = mx+b
00078 float real_time;
00079 float fade_tilt;
00080 float joy_tilt;
00081 float fade_time=10;
00082 extern bool joystick;
00083 extern float Time;
00084 
00085 float T_Joy = 0.0;
00086 
00087 
00088 void TiltVelocityLoop(void const *args)
00089 {
00090     T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_sf); // the 3D printed unit counts the opposite way to the aluminium unit.
00091 
00092     if (joystick) {
00093         P = T_Position;
00094     }
00095 
00096     M_Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
00097     Velocity = M_Position - Last_M_Position;             // Calculate change in position (i.e. Velocity)
00098     Actual_Motor_Speed = Velocity;
00099 
00100 
00101     // Check to see if we have gone past the index point
00102     if(M_Position < Lower & Last_M_Position > Upper) {       // We have gone over the index point in 1 direction
00103         Velocity += OneTurn;
00104     } else if(M_Position > Upper & Last_M_Position < Lower) {// We have gone over the index point in the other direction
00105         Velocity -= OneTurn;
00106     }
00107     Last_M_Position = M_Position;                            // Update new position from next time
00108 
00109     TiltVelocityPID.setProcessValue(Velocity);
00110 
00111     if(!joystick) {
00112         if (DoMove == 1) {
00113             if ((fadetime < ts) & (s < Vp)) {
00114                 //led2 = 0;
00115                 s = (j/2)*fadetime*fadetime;  //bottom parabola
00116                 fadetime = fadetime + LOOPs; // This provides the base time for the fade sequence
00117             } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
00118                 s = (as*fadetime)+c; //steady accel stage
00119                 fadetime = fadetime + LOOPs;
00120             } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
00121                 s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
00122                 fadetime = fadetime + LOOPs;
00123             } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
00124                 s = Vp;  // Steady Speed Stage
00125                 fadetime = fadetime + LOOPs;
00126             } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
00127                 s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
00128                 fadetime = fadetime + LOOPs;
00129             } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
00130                 s = -as*(fadetime - T) + c; //steady decel stage
00131                 fadetime = fadetime + LOOPs;
00132             } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
00133                 //led2 = 1;
00134                 s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
00135                 fadetime = fadetime + LOOPs;
00136             } else if (fadetime >= T) {
00137                 s=0;
00138                 //led2 = 0;
00139                 DoMove = 0;
00140                 TiltVelocityPID.setSetPoint(0);
00141             } else {
00142                 fadetime = fadetime + LOOPs; // for TBC reason this is needed!
00143             }
00144             // compute the new position demand:
00145             s_profile = s * dir;
00146             P = P + ((s_profile * LOOPs));
00147             real_time = ((T - fadetime) * 1000);
00148         } else {  // If not fading then respect joystick profiler instead
00149 
00150             P = P + (T_Joy * LOOPs);  // Sum total of Joystick profiler right now.
00151             P = P + (drive / 100000);  // Apply const speed drive
00152 
00153         }  // End of profilers
00154 
00155         sout = s_profile * Vff;  //Apply velocity feedforward term
00156         Error = (P - T_Position);    // Position Error
00157         Prop = T_Kp * Error;      // Calculate proportional gain element
00158         demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
00159         P_vel = demand * Motor_sf; // Convert from load Deg/s into motor encoder counts/ms
00160         TiltVelocityPID.setSetPoint(-P_vel);
00161         //.printf("\n\r %f,   %f,   %f,   %f, %f",Time, s_profile, P_vel, T_Position, Error);
00162     }  // End of if !Joystick
00163 
00164     Duty_Cycle = TiltVelocityPID.compute_ff()/Tilt_motor_max_count_rate;
00165 
00166     if(Duty_Cycle < 0) {
00167         if (M_Encoder_front) {
00168             Tilt_Motor_Direction = 1;
00169         } else {
00170             Tilt_Motor_Direction = 0;
00171         }
00172         Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
00173     } else {
00174         if (M_Encoder_front) {
00175             Tilt_Motor_Direction = 0;
00176         } else {
00177             Tilt_Motor_Direction = 1;
00178         }
00179         Tilt_Motor_PWM = 1 - Duty_Cycle;
00180     }
00181 }
00182 
00183 void Profile() // For S ramped movement using Servo for S ramping. Calling here as all it's variables are needed in this arena
00184 {
00185     if ((fade_tilt >=0) & (fade_tilt <= 359)) {
00186         D = fade_tilt - T_Position; // Calculate distance to move
00187     } else {
00188         D = 0;
00189         abort();  // leave this function
00190         // add an error event handler here
00191     }
00192 
00193     if (D <= 0) {
00194         dir = -1;
00195         D = abs(D);
00196     } else {
00197         dir = 1;
00198     }
00199 
00200     if (fade_time <= (6*tsfade + 0.2)) {
00201         ts = tscut;
00202         if (fade_time <= (6*tscut+0.2)) {
00203             T = 6*tscut + 0.2;  //min fade fime
00204         }
00205 
00206     } else {
00207         ts = tsfade;
00208         T = fade_time;
00209     }
00210    
00211     Vp = D / (T-(3*ts));  // Equation 1
00212     if (Vp > MaxSpeed) {         //Check for maximum speed condition
00213         Vp = MaxSpeed;           //Do the fade as fast as possible
00214         T = (D + (Vp * (3*ts)))/Vp;
00215     }
00216 
00217     // New version based on S-Ramping Doc - V2
00218 
00219     j = Vp / (2*ts*ts);
00220     as = j * ts;
00221     c = -(Vp / 4);
00222     s = 0;
00223     fadetime = 0;
00224     // Time = 0;
00225     P = T_Position;
00226 
00227 }
00228 
00229