group14 - k9 / Mbed 2 deprecated robot_2

2nd draft

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

Fork of robot_mockup by Martijn Kern

main.cpp@21:d6a46315d5f5, 2015-10-13 (annotated)

Committer:
Vigilance88
Date:
Tue Oct 13 10:02:48 2015 +0000
Revision:
21:d6a46315d5f5
Parent:
20:0ede3818e08e
Child:
22:1ba637601dc3
made: sample_filter and calibrate_emg; started with menu functions;

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 21:d6a46315d5f5 141 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 142 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 143 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 144 biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
Vigilance88 21:d6a46315d5f5 145 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 146
Vigilance88 21:d6a46315d5f5 147 /* alternative for lowpass filter: moving average
Vigilance88 21:d6a46315d5f5 148 window=30; //30 samples
Vigilance88 21:d6a46315d5f5 149 int i=0; //buffer index
Vigilance88 21:d6a46315d5f5 150 bicepsbuffer[i]=biceps_power //fill array
Vigilance88 21:d6a46315d5f5 151
Vigilance88 21:d6a46315d5f5 152 i++;
Vigilance88 21:d6a46315d5f5 153 if(i==window){
Vigilance88 21:d6a46315d5f5 154 i=0;
Vigilance88 21:d6a46315d5f5 155 }
Vigilance88 21:d6a46315d5f5 156
Vigilance88 21:d6a46315d5f5 157 */
Vigilance88 21:d6a46315d5f5 158
Vigilance88 21:d6a46315d5f5 159
Vigilance88 21:d6a46315d5f5 160
Vigilance88 18:44905b008f44 161 }
Vigilance88 18:44905b008f44 162
Vigilance88 18:44905b008f44 163 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88 19:0a3ee31dcdb4 164 void calibrate_arm(void)
Vigilance88 19:0a3ee31dcdb4 165 {
Vigilance88 19:0a3ee31dcdb4 166 }
Vigilance88 19:0a3ee31dcdb4 167
Vigilance88 21:d6a46315d5f5 168 //EMG Maximum Voluntary Contraction measurement
Vigilance88 21:d6a46315d5f5 169 void calibrate_emg(int muscle)
Vigilance88 18:44905b008f44 170 {
Vigilance88 21:d6a46315d5f5 171 if(muscle==1){
Vigilance88 21:d6a46315d5f5 172 start_sampling();
Vigilance88 21:d6a46315d5f5 173
Vigilance88 21:d6a46315d5f5 174 for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 21:d6a46315d5f5 175 if(biceps_power>bicepsMVC){
Vigilance88 21:d6a46315d5f5 176 bicepsMVC=biceps_power;
Vigilance88 21:d6a46315d5f5 177 }
Vigilance88 21:d6a46315d5f5 178 }
Vigilance88 21:d6a46315d5f5 179 stop_sampling();
Vigilance88 21:d6a46315d5f5 180 }
Vigilance88 21:d6a46315d5f5 181
Vigilance88 21:d6a46315d5f5 182 if(muscle==2){
Vigilance88 21:d6a46315d5f5 183 start_sampling();
Vigilance88 21:d6a46315d5f5 184
Vigilance88 21:d6a46315d5f5 185 for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 21:d6a46315d5f5 186 if(triceps_power>tricepsMVC){
Vigilance88 21:d6a46315d5f5 187 tricepsMVC=triceps_power;
Vigilance88 21:d6a46315d5f5 188 }
Vigilance88 21:d6a46315d5f5 189 }
Vigilance88 21:d6a46315d5f5 190 stop_sampling();
Vigilance88 21:d6a46315d5f5 191 }
Vigilance88 18:44905b008f44 192 }
Vigilance88 18:44905b008f44 193
Vigilance88 18:44905b008f44 194
Vigilance88 18:44905b008f44 195 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 20:0ede3818e08e 196 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 197 {
Vigilance88 20:0ede3818e08e 198 // Derivative
Vigilance88 21:d6a46315d5f5 199 double e_der = (error-e_prev)/CONTROL_RATE;
Vigilance88 21:d6a46315d5f5 200 e_der = derfilter.step(e_der);
Vigilance88 21:d6a46315d5f5 201 e_prev = error;
Vigilance88 20:0ede3818e08e 202 // Integral
Vigilance88 21:d6a46315d5f5 203 e_int = e_int+CONTROL_RATE*error;
Vigilance88 20:0ede3818e08e 204 // PID
Vigilance88 21:d6a46315d5f5 205 return kp*error + ki*e_int + kd * e_der;
Vigilance88 20:0ede3818e08e 206
Vigilance88 18:44905b008f44 207 }
Vigilance88 18:44905b008f44 208
Vigilance88 20:0ede3818e08e 209 //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 210 void control()
Vigilance88 18:44905b008f44 211 {
Vigilance88 20:0ede3818e08e 212 //analyze emg (= velocity, averages?)
Vigilance88 18:44905b008f44 213
Vigilance88 18:44905b008f44 214 //calculate reference position based on the average emg (integrate)
Vigilance88 18:44905b008f44 215
Vigilance88 18:44905b008f44 216 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 18:44905b008f44 217
Vigilance88 18:44905b008f44 218 //inverse kinematics (pos error to angle error)
Vigilance88 18:44905b008f44 219
Vigilance88 18:44905b008f44 220 //PID controller
Vigilance88 21:d6a46315d5f5 221 double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0;
Vigilance88 21:d6a46315d5f5 222 u1=pid(error,kp,ki,kd,e_int,e_prev); //motor 1
Vigilance88 21:d6a46315d5f5 223 u2=pid(error,kp,ki,kd,e_int,e_prev); //motor 2
Vigilance88 21:d6a46315d5f5 224
Vigilance88 21:d6a46315d5f5 225 keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1
Vigilance88 21:d6a46315d5f5 226 keep_in_range(&u2,-1,1);
Vigilance88 21:d6a46315d5f5 227
Vigilance88 21:d6a46315d5f5 228 //send PWM and DIR to motor 1
Vigilance88 21:d6a46315d5f5 229 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88 21:d6a46315d5f5 230 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 231
Vigilance88 21:d6a46315d5f5 232 //send PWM and DIR to motor 2
Vigilance88 21:d6a46315d5f5 233 dir_motor1 = u2>0 ? 1 : 0; //conditional statement, same as if else below
Vigilance88 21:d6a46315d5f5 234 pwm_motor1.write(abs(u2));
Vigilance88 21:d6a46315d5f5 235
Vigilance88 21:d6a46315d5f5 236 /*if(u1 > 0)
Vigilance88 21:d6a46315d5f5 237 {
Vigilance88 21:d6a46315d5f5 238 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 239 else{
Vigilance88 21:d6a46315d5f5 240 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 241 }
Vigilance88 21:d6a46315d5f5 242 }
Vigilance88 21:d6a46315d5f5 243 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 244
Vigilance88 21:d6a46315d5f5 245
Vigilance88 21:d6a46315d5f5 246 if(u2 > 0)
Vigilance88 21:d6a46315d5f5 247 {
Vigilance88 21:d6a46315d5f5 248 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 249 else{
Vigilance88 21:d6a46315d5f5 250 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 251 }
Vigilance88 21:d6a46315d5f5 252 }
Vigilance88 21:d6a46315d5f5 253 pwm_motor1.write(abs(u2));*/
Vigilance88 21:d6a46315d5f5 254
Vigilance88 18:44905b008f44 255 }
Vigilance88 18:44905b008f44 256
Vigilance88 18:44905b008f44 257 //Start control
Vigilance88 18:44905b008f44 258 void start_control(void)
Vigilance88 18:44905b008f44 259 {
Vigilance88 21:d6a46315d5f5 260 control_timer.attach(&control,SAMPLE_RATE); //100 Hz control
Vigilance88 18:44905b008f44 261 }
Vigilance88 18:44905b008f44 262
Vigilance88 18:44905b008f44 263 //stop control
Vigilance88 18:44905b008f44 264 void stop_control(void)
Vigilance88 18:44905b008f44 265 {
Vigilance88 18:44905b008f44 266 control_timer.detach();
vsluiter 2:e314bb3b2d99 267 }
vsluiter 0:32bb76391d89 268
Vigilance88 21:d6a46315d5f5 269
Vigilance88 21:d6a46315d5f5 270 void calibrate()
vsluiter 0:32bb76391d89 271 {
Vigilance88 21:d6a46315d5f5 272
Vigilance88 21:d6a46315d5f5 273 }
tomlankhorst 15:0da764eea774 274
Vigilance88 21:d6a46315d5f5 275
Vigilance88 21:d6a46315d5f5 276 //keeps input limited between min max
Vigilance88 18:44905b008f44 277 void keep_in_range(float * in, float min, float max)
Vigilance88 18:44905b008f44 278 {
Vigilance88 18:44905b008f44 279 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 280 }