Carbon Fibre / Mbed 2 deprecated Motor_test_harness

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

TiltVelocityLoop.cpp@4:4dafa4113982, 2015-06-25 (annotated)

Committer:
acodd
Date:
Thu Jun 25 09:41:26 2015 +0000
Revision:
4:4dafa4113982
Parent:
3:f8a5c1cee1fa 
Test Harness for a single axis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ms523 0:7ce0bc67f60f 1 #include "mbed.h"
ms523 0:7ce0bc67f60f 2 #include "iC_MU.h"
ms523 0:7ce0bc67f60f 3 #include "rtos.h"
ms523 0:7ce0bc67f60f 4 #include "Classic_PID.h"
ms523 0:7ce0bc67f60f 5
ms523 0:7ce0bc67f60f 6 // Define limits for zero crossing
ms523 0:7ce0bc67f60f 7 // These values should allow operation upto 3750 RPM
ms523 0:7ce0bc67f60f 8 #define bits 18 // The number of bits we want to use
ms523 0:7ce0bc67f60f 9 #define OneTurn (1<<bits) // 262144 counts per rev
ms523 0:7ce0bc67f60f 10 #define Lower (1<<(bits-5)) // 8192 counts = 11.25 degrees
ms523 0:7ce0bc67f60f 11 #define Upper OneTurn - Lower // 262144 - 8192 = 253952
ms523 0:7ce0bc67f60f 12
acodd 2:dc684c402296 13 extern Serial pc;
ms523 0:7ce0bc67f60f 14 extern iC_MU tilt_ic_mu;
acodd 2:dc684c402296 15 extern iC_MU TiltPos;
ms523 0:7ce0bc67f60f 16 extern PwmOut Tilt_Motor_PWM;
ms523 0:7ce0bc67f60f 17 extern DigitalOut Tilt_Motor_Direction;
ms523 0:7ce0bc67f60f 18 extern Classic_PID TiltVelocityPID;
acodd 2:dc684c402296 19 extern float Demand_Count_Rate;
acodd 2:dc684c402296 20 extern float Tilt_motor_max_count_rate;
acodd 2:dc684c402296 21 extern float Actual_Motor_Speed;
acodd 2:dc684c402296 22
acodd 2:dc684c402296 23 extern float T_Position; // True Tilt Position (Degrees)
acodd 3:f8a5c1cee1fa 24 extern float T_Encoder_sf;
acodd 2:dc684c402296 25
ms523 0:7ce0bc67f60f 26
acodd 3:f8a5c1cee1fa 27 int Last_M_Position = 0;
acodd 3:f8a5c1cee1fa 28 int M_Position = 0;
acodd 2:dc684c402296 29 int Velocity = 0;
acodd 2:dc684c402296 30 float Duty_Cycle = 0.0;
acodd 2:dc684c402296 31
acodd 3:f8a5c1cee1fa 32 // System Configuration:
acodd 2:dc684c402296 33 float MaxSpeed = 60.00; // 5 Deg/s, 1250 RPM
acodd 4:4dafa4113982 34 float Tilt_motor_max_count_rate = 6000; //encoder counts / ms
acodd 2:dc684c402296 35 float T_Position = 0; // True Tilt Position (Degrees)
acodd 3:f8a5c1cee1fa 36 float T_Encoder_sf = 1456.355; // counts per degree
acodd 3:f8a5c1cee1fa 37 float T_Gear_ratio = 125; // 125:1
acodd 3:f8a5c1cee1fa 38 float M_Encoder = 262144; // counts/rev
acodd 3:f8a5c1cee1fa 39 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
acodd 3:f8a5c1cee1fa 40 bool M_Encoder_front = true; // or 0 depending on the encoder configuration on the motor (facing to motor, or facing away from motor)
acodd 2:dc684c402296 41
acodd 4:4dafa4113982 42 double drive = 0; // this is a const speed demand straight into the loop (divided by 100)
acodd 4:4dafa4113982 43
acodd 2:dc684c402296 44 // Main servo loop
acodd 2:dc684c402296 45 int DoMove = 0;
acodd 2:dc684c402296 46 const float LOOPs = 0.001;
acodd 3:f8a5c1cee1fa 47 // S_Ramp Fade values
acodd 2:dc684c402296 48 float D = 10; // Fade distance
acodd 2:dc684c402296 49 float T = 15; // Fade time
acodd 2:dc684c402296 50 float dir = 1; //direction flag
acodd 2:dc684c402296 51 float ta; // The actual value used after sanity checks
acodd 2:dc684c402296 52 float ts; // The actual value used after sanity checks(S ramping)
acodd 2:dc684c402296 53 float tsfade = 0.5; // segment time for S ramped fade
acodd 4:4dafa4113982 54 float tscut = 0.3; // segment time for S ramped cut
acodd 2:dc684c402296 55 float j; // jerk value for fade
acodd 2:dc684c402296 56 float aj; // accel value when S ramping
acodd 2:dc684c402296 57 float Vp; // Top speed for the move Deg/s @ load (125:1 Ratio to motor)
acodd 2:dc684c402296 58 float Vs; // Speed step increment
acodd 2:dc684c402296 59 float Da; // Accel distance
acodd 2:dc684c402296 60 float Ds; // Distance convered at steady speed
acodd 2:dc684c402296 61 float s; // Profiler internal demand speed (always positive)
acodd 2:dc684c402296 62 float sout; // Demand as applied by the Vff term
acodd 2:dc684c402296 63 float s_profile; // output demand speed to postion loop + or -)
acodd 4:4dafa4113982 64 double P; // Profiler Demand postion
acodd 2:dc684c402296 65 float fadetime; // this will retain the current fade time
acodd 2:dc684c402296 66 float Error; // Current position vs the profiler position
acodd 2:dc684c402296 67 float Vff = 1; // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
acodd 4:4dafa4113982 68 float T_Kp = 6; // This is is multiplied by the position error and added to the motor demand
acodd 2:dc684c402296 69 float Prop; // The demand created by the Kp and error calculation
acodd 2:dc684c402296 70 float demand = 0; // The value sento to the motor to make it move
acodd 2:dc684c402296 71 float P_vel;
acodd 2:dc684c402296 72 float Va; // mid speed point
acodd 2:dc684c402296 73 float as; // acceleration value during linear accel stage
acodd 2:dc684c402296 74 float Vj; // Speed at bottom intersection
acodd 2:dc684c402296 75 float Vjp; // Speed at top intersection
acodd 2:dc684c402296 76 float c; // constant for up ramp y=mx+c
acodd 2:dc684c402296 77 float b; // constant for down ramp y = mx+b
acodd 2:dc684c402296 78 float real_time;
acodd 2:dc684c402296 79 float fade_tilt;
acodd 2:dc684c402296 80 float joy_tilt;
acodd 2:dc684c402296 81 float fade_time=10;
acodd 2:dc684c402296 82 extern bool joystick;
acodd 2:dc684c402296 83 extern float Time;
acodd 2:dc684c402296 84
acodd 2:dc684c402296 85 float T_Joy = 0.0;
acodd 2:dc684c402296 86
ms523 0:7ce0bc67f60f 87
ms523 0:7ce0bc67f60f 88 void TiltVelocityLoop(void const *args)
ms523 0:7ce0bc67f60f 89 {
acodd 3:f8a5c1cee1fa 90 T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_sf); // the 3D printed unit counts the opposite way to the aluminium unit.
acodd 3:f8a5c1cee1fa 91
acodd 3:f8a5c1cee1fa 92 if (joystick) {
acodd 2:dc684c402296 93 P = T_Position;
acodd 3:f8a5c1cee1fa 94 }
acodd 3:f8a5c1cee1fa 95
acodd 3:f8a5c1cee1fa 96 M_Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
acodd 3:f8a5c1cee1fa 97 Velocity = M_Position - Last_M_Position; // Calculate change in position (i.e. Velocity)
acodd 2:dc684c402296 98 Actual_Motor_Speed = Velocity;
acodd 2:dc684c402296 99
ms523 0:7ce0bc67f60f 100
ms523 0:7ce0bc67f60f 101 // Check to see if we have gone past the index point
acodd 3:f8a5c1cee1fa 102 if(M_Position < Lower & Last_M_Position > Upper) { // We have gone over the index point in 1 direction
ms523 0:7ce0bc67f60f 103 Velocity += OneTurn;
acodd 3:f8a5c1cee1fa 104 } else if(M_Position > Upper & Last_M_Position < Lower) {// We have gone over the index point in the other direction
ms523 0:7ce0bc67f60f 105 Velocity -= OneTurn;
acodd 2:dc684c402296 106 }
acodd 3:f8a5c1cee1fa 107 Last_M_Position = M_Position; // Update new position from next time
ms523 0:7ce0bc67f60f 108
acodd 2:dc684c402296 109 TiltVelocityPID.setProcessValue(Velocity);
acodd 2:dc684c402296 110
acodd 3:f8a5c1cee1fa 111 if(!joystick) {
acodd 3:f8a5c1cee1fa 112 if (DoMove == 1) {
acodd 3:f8a5c1cee1fa 113 if ((fadetime < ts) & (s < Vp)) {
acodd 3:f8a5c1cee1fa 114 //led2 = 0;
acodd 3:f8a5c1cee1fa 115 s = (j/2)*fadetime*fadetime; //bottom parabola
acodd 3:f8a5c1cee1fa 116 fadetime = fadetime + LOOPs; // This provides the base time for the fade sequence
acodd 3:f8a5c1cee1fa 117 } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
acodd 3:f8a5c1cee1fa 118 s = (as*fadetime)+c; //steady accel stage
acodd 3:f8a5c1cee1fa 119 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 120 } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
acodd 3:f8a5c1cee1fa 121 s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
acodd 3:f8a5c1cee1fa 122 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 123 } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
acodd 3:f8a5c1cee1fa 124 s = Vp; // Steady Speed Stage
acodd 3:f8a5c1cee1fa 125 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 126 } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
acodd 3:f8a5c1cee1fa 127 s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
acodd 3:f8a5c1cee1fa 128 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 129 } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
acodd 3:f8a5c1cee1fa 130 s = -as*(fadetime - T) + c; //steady decel stage
acodd 3:f8a5c1cee1fa 131 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 132 } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
acodd 3:f8a5c1cee1fa 133 //led2 = 1;
acodd 3:f8a5c1cee1fa 134 s = (j/2)*(T-fadetime)*(T-fadetime); //bottom parabola to end
acodd 3:f8a5c1cee1fa 135 fadetime = fadetime + LOOPs;
acodd 3:f8a5c1cee1fa 136 } else if (fadetime >= T) {
acodd 3:f8a5c1cee1fa 137 s=0;
acodd 3:f8a5c1cee1fa 138 //led2 = 0;
acodd 3:f8a5c1cee1fa 139 DoMove = 0;
acodd 3:f8a5c1cee1fa 140 TiltVelocityPID.setSetPoint(0);
acodd 3:f8a5c1cee1fa 141 } else {
acodd 3:f8a5c1cee1fa 142 fadetime = fadetime + LOOPs; // for TBC reason this is needed!
acodd 3:f8a5c1cee1fa 143 }
acodd 2:dc684c402296 144 // compute the new position demand:
acodd 2:dc684c402296 145 s_profile = s * dir;
acodd 4:4dafa4113982 146 P = P + ((s_profile * LOOPs));
acodd 2:dc684c402296 147 real_time = ((T - fadetime) * 1000);
acodd 3:f8a5c1cee1fa 148 } else { // If not fading then respect joystick profiler instead
acodd 2:dc684c402296 149
acodd 3:f8a5c1cee1fa 150 P = P + (T_Joy * LOOPs); // Sum total of Joystick profiler right now.
acodd 4:4dafa4113982 151 P = P + (drive / 100000); // Apply const speed drive
acodd 3:f8a5c1cee1fa 152
acodd 3:f8a5c1cee1fa 153 } // End of profilers
acodd 2:dc684c402296 154
acodd 3:f8a5c1cee1fa 155 sout = s_profile * Vff; //Apply velocity feedforward term
acodd 3:f8a5c1cee1fa 156 Error = (P - T_Position); // Position Error
acodd 3:f8a5c1cee1fa 157 Prop = T_Kp * Error; // Calculate proportional gain element
acodd 3:f8a5c1cee1fa 158 demand = sout + Prop; // Sum the result of Vff and Kp to the demand
acodd 3:f8a5c1cee1fa 159 P_vel = demand * Motor_sf; // Convert from load Deg/s into motor encoder counts/ms
acodd 4:4dafa4113982 160 TiltVelocityPID.setSetPoint(-P_vel);
acodd 3:f8a5c1cee1fa 161 //.printf("\n\r %f, %f, %f, %f, %f",Time, s_profile, P_vel, T_Position, Error);
acodd 3:f8a5c1cee1fa 162 } // End of if !Joystick
acodd 2:dc684c402296 163
acodd 2:dc684c402296 164 Duty_Cycle = TiltVelocityPID.compute_ff()/Tilt_motor_max_count_rate;
ms523 0:7ce0bc67f60f 165
ms523 0:7ce0bc67f60f 166 if(Duty_Cycle < 0) {
acodd 3:f8a5c1cee1fa 167 if (M_Encoder_front) {
acodd 3:f8a5c1cee1fa 168 Tilt_Motor_Direction = 1;
acodd 3:f8a5c1cee1fa 169 } else {
acodd 3:f8a5c1cee1fa 170 Tilt_Motor_Direction = 0;
acodd 3:f8a5c1cee1fa 171 }
ms523 0:7ce0bc67f60f 172 Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
ms523 0:7ce0bc67f60f 173 } else {
acodd 3:f8a5c1cee1fa 174 if (M_Encoder_front) {
acodd 3:f8a5c1cee1fa 175 Tilt_Motor_Direction = 0;
acodd 3:f8a5c1cee1fa 176 } else {
acodd 3:f8a5c1cee1fa 177 Tilt_Motor_Direction = 1;
acodd 3:f8a5c1cee1fa 178 }
ms523 0:7ce0bc67f60f 179 Tilt_Motor_PWM = 1 - Duty_Cycle;
ms523 0:7ce0bc67f60f 180 }
acodd 2:dc684c402296 181 }
acodd 2:dc684c402296 182
acodd 3:f8a5c1cee1fa 183 void Profile() // For S ramped movement using Servo for S ramping. Calling here as all it's variables are needed in this arena
acodd 2:dc684c402296 184 {
acodd 2:dc684c402296 185 if ((fade_tilt >=0) & (fade_tilt <= 359)) {
acodd 2:dc684c402296 186 D = fade_tilt - T_Position; // Calculate distance to move
acodd 2:dc684c402296 187 } else {
acodd 2:dc684c402296 188 D = 0;
acodd 2:dc684c402296 189 abort(); // leave this function
acodd 2:dc684c402296 190 // add an error event handler here
acodd 2:dc684c402296 191 }
acodd 2:dc684c402296 192
acodd 2:dc684c402296 193 if (D <= 0) {
acodd 2:dc684c402296 194 dir = -1;
acodd 2:dc684c402296 195 D = abs(D);
acodd 2:dc684c402296 196 } else {
acodd 2:dc684c402296 197 dir = 1;
acodd 2:dc684c402296 198 }
acodd 2:dc684c402296 199
acodd 2:dc684c402296 200 if (fade_time <= (6*tsfade + 0.2)) {
acodd 2:dc684c402296 201 ts = tscut;
acodd 4:4dafa4113982 202 if (fade_time <= (6*tscut+0.2)) {
acodd 4:4dafa4113982 203 T = 6*tscut + 0.2; //min fade fime
acodd 4:4dafa4113982 204 }
acodd 4:4dafa4113982 205
acodd 2:dc684c402296 206 } else {
acodd 2:dc684c402296 207 ts = tsfade;
acodd 2:dc684c402296 208 T = fade_time;
acodd 2:dc684c402296 209 }
acodd 4:4dafa4113982 210
acodd 2:dc684c402296 211 Vp = D / (T-(3*ts)); // Equation 1
acodd 2:dc684c402296 212 if (Vp > MaxSpeed) { //Check for maximum speed condition
acodd 2:dc684c402296 213 Vp = MaxSpeed; //Do the fade as fast as possible
acodd 2:dc684c402296 214 T = (D + (Vp * (3*ts)))/Vp;
acodd 2:dc684c402296 215 }
acodd 2:dc684c402296 216
acodd 2:dc684c402296 217 // New version based on S-Ramping Doc - V2
acodd 2:dc684c402296 218
acodd 2:dc684c402296 219 j = Vp / (2*ts*ts);
acodd 2:dc684c402296 220 as = j * ts;
acodd 2:dc684c402296 221 c = -(Vp / 4);
acodd 2:dc684c402296 222 s = 0;
acodd 2:dc684c402296 223 fadetime = 0;
acodd 3:f8a5c1cee1fa 224 // Time = 0;
acodd 2:dc684c402296 225 P = T_Position;
acodd 2:dc684c402296 226
acodd 2:dc684c402296 227 }
acodd 2:dc684c402296 228
acodd 2:dc684c402296 229