group14 - k9 / Mbed 2 deprecated testPID

testcode pid

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of testPID by Martijn Kern

main.cpp@22:1ba637601dc3, 2015-10-13 (annotated)

Committer:
Vigilance88
Date:
Tue Oct 13 12:55:33 2015 +0000
Revision:
22:1ba637601dc3
Parent:
21:d6a46315d5f5
Child:
23:e9b1b426cde6
calibrate_arm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Vigilance88 18:44905b008f44 3 #include "MODSERIAL.h"
Vigilance88 18:44905b008f44 4 #include "biquadFilter.h"
Vigilance88 18:44905b008f44 5 #include "QEI.h"
Vigilance88 21:d6a46315d5f5 6 #include "math.h"
Vigilance88 21:d6a46315d5f5 7
Vigilance88 21:d6a46315d5f5 8 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 9 -------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 10 --------------------------------------------------------------------------------------------------------------------*/
vsluiter 0:32bb76391d89 11
Vigilance88 18:44905b008f44 12 //Define important constants in memory
Vigilance88 21:d6a46315d5f5 13 #define PI 3.14159265
Vigilance88 18:44905b008f44 14 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
Vigilance88 18:44905b008f44 15 #define CONTROL_RATE 0.01 //100 Hz Control rate
Vigilance88 21:d6a46315d5f5 16 #define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
Vigilance88 21:d6a46315d5f5 17 #define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
Vigilance88 18:44905b008f44 18
Vigilance88 21:d6a46315d5f5 19 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 20 ---- OBJECTS ---------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 21 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 22
Vigilance88 18:44905b008f44 23 MODSERIAL pc(USBTX,USBRX); //serial communication
Vigilance88 18:44905b008f44 24 DigitalIn button(PTA1); //buttons
Vigilance88 18:44905b008f44 25 DigitalIn button1(PTB9); //
Vigilance88 18:44905b008f44 26
Vigilance88 21:d6a46315d5f5 27 //EMG shields
Vigilance88 18:44905b008f44 28 AnalogIn emg1(A0); //Analog input - Biceps EMG
Vigilance88 18:44905b008f44 29 AnalogIn emg2(A1); //Analog input - Triceps EMG
Vigilance88 18:44905b008f44 30 AnalogIn emg3(A2); //Analog input - Flexor EMG
Vigilance88 18:44905b008f44 31 AnalogIn emg4(A3); //Analog input - Extensor EMG
Vigilance88 18:44905b008f44 32
Vigilance88 18:44905b008f44 33 Ticker sample_timer; //Ticker for EMG sampling
Vigilance88 18:44905b008f44 34 Ticker control_timer; //Ticker for control loop
Vigilance88 18:44905b008f44 35 HIDScope scope(4); //Scope 4 channels
Vigilance88 18:44905b008f44 36
Vigilance88 18:44905b008f44 37 // AnalogIn potmeter(A4); //potmeters
Vigilance88 18:44905b008f44 38 // AnalogIn potmeter2(A5); //
Vigilance88 18:44905b008f44 39
Vigilance88 21:d6a46315d5f5 40 //Encoders
Vigilance88 18:44905b008f44 41 QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses();
Vigilance88 18:44905b008f44 42 QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder,
Vigilance88 21:d6a46315d5f5 43
Vigilance88 21:d6a46315d5f5 44 //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
Vigilance88 21:d6a46315d5f5 45 PwmOut pwm_motor1(D6); //PWM motor 1
Vigilance88 21:d6a46315d5f5 46 PwmOut pwm_motor2(D5); //PWM motor 2
Vigilance88 18:44905b008f44 47 DigitalOut dir_motor1(D7); //Direction motor 1
Vigilance88 18:44905b008f44 48 DigitalOut dir_motor2(D4); //Direction motor 2
Vigilance88 18:44905b008f44 49
Vigilance88 18:44905b008f44 50 //Filters
Vigilance88 21:d6a46315d5f5 51 //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
Vigilance88 21:d6a46315d5f5 52 biquadFilter highpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes DC and movement artefacts
Vigilance88 21:d6a46315d5f5 53 biquadFilter notch1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes 49-51 Hz power interference
Vigilance88 21:d6a46315d5f5 54 biquadFilter notch2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //
Vigilance88 21:d6a46315d5f5 55 biquadFilter lowpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // EMG envelope
Vigilance88 21:d6a46315d5f5 56 biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter
Vigilance88 21:d6a46315d5f5 57
Vigilance88 21:d6a46315d5f5 58 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 59 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 60 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 61
Vigilance88 21:d6a46315d5f5 62 //EMG variables
Vigilance88 21:d6a46315d5f5 63 double biceps_power; double bicepsMVC = 0;
Vigilance88 21:d6a46315d5f5 64 double triceps_power; double tricepsMVC = 0;
Vigilance88 21:d6a46315d5f5 65 double flexor_power; double flexorMVC = 0;
Vigilance88 21:d6a46315d5f5 66 double extens_power; double extensorMVC = 0;
Vigilance88 21:d6a46315d5f5 67
Vigilance88 21:d6a46315d5f5 68
Vigilance88 21:d6a46315d5f5 69
Vigilance88 21:d6a46315d5f5 70 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 71 ---- DECLARE FUNCTION NAMES-------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 72 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 73 void keep_in_range(float * in, float min, float max);
Vigilance88 21:d6a46315d5f5 74 void sample_filter(void);
Vigilance88 21:d6a46315d5f5 75 void control();
Vigilance88 21:d6a46315d5f5 76 void calibrate_emg(int muscle);
Vigilance88 21:d6a46315d5f5 77 void calibrate_arm(void);
Vigilance88 21:d6a46315d5f5 78 void start_sampling(void);
Vigilance88 21:d6a46315d5f5 79 void stop_sampling(void);
Vigilance88 21:d6a46315d5f5 80 void start_control(void);
Vigilance88 21:d6a46315d5f5 81 void stop_control(void);
Vigilance88 21:d6a46315d5f5 82 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88 18:44905b008f44 83
Vigilance88 21:d6a46315d5f5 84 void mainMenu();
Vigilance88 21:d6a46315d5f5 85 void caliMenu();
Vigilance88 21:d6a46315d5f5 86
Vigilance88 21:d6a46315d5f5 87 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 88 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 89 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 90
Vigilance88 21:d6a46315d5f5 91 int main()
Vigilance88 21:d6a46315d5f5 92 {
Vigilance88 21:d6a46315d5f5 93 // make a menu, user has to initiated next step
Vigilance88 21:d6a46315d5f5 94 mainMenu();
Vigilance88 21:d6a46315d5f5 95 caliMenu(); // Menu function
Vigilance88 21:d6a46315d5f5 96 calibrate_arm(); //Start Calibration
Vigilance88 21:d6a46315d5f5 97 start_sampling(); //500 Hz EMG
Vigilance88 21:d6a46315d5f5 98 calibrate_emg(1);
Vigilance88 21:d6a46315d5f5 99 start_control(); //100 Hz control
Vigilance88 21:d6a46315d5f5 100
Vigilance88 21:d6a46315d5f5 101 //maybe some stop commands when a button is pressed: detach from timers.
Vigilance88 21:d6a46315d5f5 102 //stop_control();
Vigilance88 21:d6a46315d5f5 103 //stop_sampling();
Vigilance88 21:d6a46315d5f5 104
Vigilance88 21:d6a46315d5f5 105 while(1) {
Vigilance88 21:d6a46315d5f5 106 scope.set(0,emg_biceps);
Vigilance88 21:d6a46315d5f5 107 scope.set(1,emg_triceps);
Vigilance88 21:d6a46315d5f5 108 scope.set(2,emg_flexor);
Vigilance88 21:d6a46315d5f5 109 scope.set(3,emg_extens);
Vigilance88 21:d6a46315d5f5 110 scope.send();
Vigilance88 21:d6a46315d5f5 111 }
Vigilance88 21:d6a46315d5f5 112 //end of while loop
Vigilance88 21:d6a46315d5f5 113 }
Vigilance88 21:d6a46315d5f5 114 //end of main
Vigilance88 21:d6a46315d5f5 115
Vigilance88 21:d6a46315d5f5 116 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 117 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 118 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 18:44905b008f44 119
Vigilance88 18:44905b008f44 120 //Start sampling
Vigilance88 18:44905b008f44 121 void start_sampling(void)
Vigilance88 18:44905b008f44 122 {
Vigilance88 21:d6a46315d5f5 123 sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
Vigilance88 18:44905b008f44 124 }
Vigilance88 18:44905b008f44 125
Vigilance88 18:44905b008f44 126 //stop sampling
Vigilance88 18:44905b008f44 127 void stop_sampling(void)
Vigilance88 18:44905b008f44 128 {
Vigilance88 18:44905b008f44 129 sample_timer.detach();
Vigilance88 18:44905b008f44 130 }
vsluiter 2:e314bb3b2d99 131
Vigilance88 21:d6a46315d5f5 132 //Sample and Filter
Vigilance88 21:d6a46315d5f5 133 void sample_filter(void)
Vigilance88 18:44905b008f44 134 {
Vigilance88 21:d6a46315d5f5 135 double emg_biceps = emg1.read(); //Biceps
Vigilance88 21:d6a46315d5f5 136 double emg_triceps = emg2.read(); //Triceps
Vigilance88 21:d6a46315d5f5 137 double emg_flexor = emg3.read(); //Flexor
Vigilance88 21:d6a46315d5f5 138 double emg_extens = emg4.read(); //Extensor
Vigilance88 21:d6a46315d5f5 139
Vigilance88 21:d6a46315d5f5 140 //Filter: high-pass -> notch -> rectify -> lowpass or moving average
Vigilance88 22:1ba637601dc3 141 // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
Vigilance88 21:d6a46315d5f5 142 biceps_power = highpass.step(emg_biceps); triceps_power = highpass.step(emg_triceps); flexor_power = highpass.step(emg_flexor); extens_power = highpass.step(emg_extens);
Vigilance88 21:d6a46315d5f5 143 biceps_power = notch1.step(biceps_power); triceps_power = notch1.step(triceps_power); flexor_power = notch1.step(flexor_power); extens_power = notch1.step(extens_power);
Vigilance88 21:d6a46315d5f5 144 biceps_power = notch2.step(biceps_power); triceps_power = notch2.step(triceps_power); flexor_power = notch2.step(flexor_power); extens_power = notch2.step(extens_power);
Vigilance88 21:d6a46315d5f5 145 biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
Vigilance88 21:d6a46315d5f5 146 biceps_power = lowpass.step(biceps_power); triceps_power = lowpass.step(triceps_power); flexor_power = lowpass.step(flexor_power); extens_power = lowpass.step(extens_power);
Vigilance88 21:d6a46315d5f5 147
Vigilance88 21:d6a46315d5f5 148 /* alternative for lowpass filter: moving average
Vigilance88 21:d6a46315d5f5 149 window=30; //30 samples
Vigilance88 21:d6a46315d5f5 150 int i=0; //buffer index
Vigilance88 21:d6a46315d5f5 151 bicepsbuffer[i]=biceps_power //fill array
Vigilance88 21:d6a46315d5f5 152
Vigilance88 21:d6a46315d5f5 153 i++;
Vigilance88 21:d6a46315d5f5 154 if(i==window){
Vigilance88 21:d6a46315d5f5 155 i=0;
Vigilance88 21:d6a46315d5f5 156 }
Vigilance88 21:d6a46315d5f5 157
Vigilance88 21:d6a46315d5f5 158 */
Vigilance88 21:d6a46315d5f5 159
Vigilance88 21:d6a46315d5f5 160
Vigilance88 21:d6a46315d5f5 161
Vigilance88 18:44905b008f44 162 }
Vigilance88 18:44905b008f44 163
Vigilance88 18:44905b008f44 164 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88 19:0a3ee31dcdb4 165 void calibrate_arm(void)
Vigilance88 19:0a3ee31dcdb4 166 {
Vigilance88 22:1ba637601dc3 167 dir_motor1=1; //ccw
Vigilance88 22:1ba637601dc3 168 dir_motor2=1; //ccw
Vigilance88 22:1ba637601dc3 169 while(limitswitch != hit){
Vigilance88 22:1ba637601dc3 170 pwm_motor1.write(0.4);
Vigilance88 22:1ba637601dc3 171 pwm_motor2.write(0.4);
Vigilance88 22:1ba637601dc3 172 }
Vigilance88 19:0a3ee31dcdb4 173 }
Vigilance88 19:0a3ee31dcdb4 174
Vigilance88 21:d6a46315d5f5 175 //EMG Maximum Voluntary Contraction measurement
Vigilance88 21:d6a46315d5f5 176 void calibrate_emg(int muscle)
Vigilance88 18:44905b008f44 177 {
Vigilance88 21:d6a46315d5f5 178 if(muscle==1){
Vigilance88 21:d6a46315d5f5 179 start_sampling();
Vigilance88 22:1ba637601dc3 180 wait(1);
Vigilance88 21:d6a46315d5f5 181
Vigilance88 21:d6a46315d5f5 182 for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 21:d6a46315d5f5 183 if(biceps_power>bicepsMVC){
Vigilance88 21:d6a46315d5f5 184 bicepsMVC=biceps_power;
Vigilance88 21:d6a46315d5f5 185 }
Vigilance88 21:d6a46315d5f5 186 }
Vigilance88 21:d6a46315d5f5 187 stop_sampling();
Vigilance88 21:d6a46315d5f5 188 }
Vigilance88 21:d6a46315d5f5 189
Vigilance88 21:d6a46315d5f5 190 if(muscle==2){
Vigilance88 21:d6a46315d5f5 191 start_sampling();
Vigilance88 22:1ba637601dc3 192 wait(1);
Vigilance88 21:d6a46315d5f5 193
Vigilance88 21:d6a46315d5f5 194 for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 21:d6a46315d5f5 195 if(triceps_power>tricepsMVC){
Vigilance88 21:d6a46315d5f5 196 tricepsMVC=triceps_power;
Vigilance88 21:d6a46315d5f5 197 }
Vigilance88 21:d6a46315d5f5 198 }
Vigilance88 21:d6a46315d5f5 199 stop_sampling();
Vigilance88 21:d6a46315d5f5 200 }
Vigilance88 18:44905b008f44 201 }
Vigilance88 18:44905b008f44 202
Vigilance88 18:44905b008f44 203
Vigilance88 18:44905b008f44 204 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 20:0ede3818e08e 205 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 206 {
Vigilance88 20:0ede3818e08e 207 // Derivative
Vigilance88 21:d6a46315d5f5 208 double e_der = (error-e_prev)/CONTROL_RATE;
Vigilance88 21:d6a46315d5f5 209 e_der = derfilter.step(e_der);
Vigilance88 21:d6a46315d5f5 210 e_prev = error;
Vigilance88 20:0ede3818e08e 211 // Integral
Vigilance88 21:d6a46315d5f5 212 e_int = e_int+CONTROL_RATE*error;
Vigilance88 20:0ede3818e08e 213 // PID
Vigilance88 21:d6a46315d5f5 214 return kp*error + ki*e_int + kd * e_der;
Vigilance88 20:0ede3818e08e 215
Vigilance88 18:44905b008f44 216 }
Vigilance88 18:44905b008f44 217
Vigilance88 20:0ede3818e08e 218 //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors
Vigilance88 18:44905b008f44 219 void control()
Vigilance88 18:44905b008f44 220 {
Vigilance88 20:0ede3818e08e 221 //analyze emg (= velocity, averages?)
Vigilance88 18:44905b008f44 222
Vigilance88 18:44905b008f44 223 //calculate reference position based on the average emg (integrate)
Vigilance88 18:44905b008f44 224
Vigilance88 18:44905b008f44 225 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 18:44905b008f44 226
Vigilance88 18:44905b008f44 227 //inverse kinematics (pos error to angle error)
Vigilance88 18:44905b008f44 228
Vigilance88 18:44905b008f44 229 //PID controller
Vigilance88 21:d6a46315d5f5 230 double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0;
Vigilance88 21:d6a46315d5f5 231 u1=pid(error,kp,ki,kd,e_int,e_prev); //motor 1
Vigilance88 21:d6a46315d5f5 232 u2=pid(error,kp,ki,kd,e_int,e_prev); //motor 2
Vigilance88 21:d6a46315d5f5 233
Vigilance88 21:d6a46315d5f5 234 keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1
Vigilance88 21:d6a46315d5f5 235 keep_in_range(&u2,-1,1);
Vigilance88 21:d6a46315d5f5 236
Vigilance88 21:d6a46315d5f5 237 //send PWM and DIR to motor 1
Vigilance88 21:d6a46315d5f5 238 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88 21:d6a46315d5f5 239 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 240
Vigilance88 21:d6a46315d5f5 241 //send PWM and DIR to motor 2
Vigilance88 21:d6a46315d5f5 242 dir_motor1 = u2>0 ? 1 : 0; //conditional statement, same as if else below
Vigilance88 21:d6a46315d5f5 243 pwm_motor1.write(abs(u2));
Vigilance88 21:d6a46315d5f5 244
Vigilance88 21:d6a46315d5f5 245 /*if(u1 > 0)
Vigilance88 21:d6a46315d5f5 246 {
Vigilance88 21:d6a46315d5f5 247 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 248 else{
Vigilance88 21:d6a46315d5f5 249 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 250 }
Vigilance88 21:d6a46315d5f5 251 }
Vigilance88 21:d6a46315d5f5 252 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 253
Vigilance88 21:d6a46315d5f5 254
Vigilance88 21:d6a46315d5f5 255 if(u2 > 0)
Vigilance88 21:d6a46315d5f5 256 {
Vigilance88 21:d6a46315d5f5 257 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 258 else{
Vigilance88 21:d6a46315d5f5 259 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 260 }
Vigilance88 21:d6a46315d5f5 261 }
Vigilance88 21:d6a46315d5f5 262 pwm_motor1.write(abs(u2));*/
Vigilance88 21:d6a46315d5f5 263
Vigilance88 18:44905b008f44 264 }
Vigilance88 18:44905b008f44 265
Vigilance88 18:44905b008f44 266 //Start control
Vigilance88 18:44905b008f44 267 void start_control(void)
Vigilance88 18:44905b008f44 268 {
Vigilance88 21:d6a46315d5f5 269 control_timer.attach(&control,SAMPLE_RATE); //100 Hz control
Vigilance88 18:44905b008f44 270 }
Vigilance88 18:44905b008f44 271
Vigilance88 18:44905b008f44 272 //stop control
Vigilance88 18:44905b008f44 273 void stop_control(void)
Vigilance88 18:44905b008f44 274 {
Vigilance88 18:44905b008f44 275 control_timer.detach();
vsluiter 2:e314bb3b2d99 276 }
vsluiter 0:32bb76391d89 277
Vigilance88 21:d6a46315d5f5 278
Vigilance88 21:d6a46315d5f5 279 void calibrate()
vsluiter 0:32bb76391d89 280 {
Vigilance88 21:d6a46315d5f5 281
Vigilance88 21:d6a46315d5f5 282 }
tomlankhorst 15:0da764eea774 283
Vigilance88 21:d6a46315d5f5 284
Vigilance88 21:d6a46315d5f5 285 //keeps input limited between min max
Vigilance88 18:44905b008f44 286 void keep_in_range(float * in, float min, float max)
Vigilance88 18:44905b008f44 287 {
Vigilance88 18:44905b008f44 288 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 289 }