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@29:948b0b14f6be, 2015-10-21 (annotated)

Committer:
Vigilance88
Date:
Wed Oct 21 09:19:23 2015 +0000
Revision:
29:948b0b14f6be
Parent:
28:743485bb51e4
Child:
30:a9fdd3202ca2
removed some comment errors

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 26:fe3a5469dd6b 7 #include <string>
Vigilance88 21:d6a46315d5f5 8
Vigilance88 21:d6a46315d5f5 9 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 10 -------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 11 --------------------------------------------------------------------------------------------------------------------*/
vsluiter 0:32bb76391d89 12
Vigilance88 18:44905b008f44 13 //Define important constants in memory
Vigilance88 21:d6a46315d5f5 14 #define PI 3.14159265
Vigilance88 18:44905b008f44 15 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
Vigilance88 18:44905b008f44 16 #define CONTROL_RATE 0.01 //100 Hz Control rate
Vigilance88 21:d6a46315d5f5 17 #define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
Vigilance88 21:d6a46315d5f5 18 #define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
Vigilance88 26:fe3a5469dd6b 19 #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
Vigilance88 21:d6a46315d5f5 20 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 21 ---- OBJECTS ---------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 22 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 23
Vigilance88 18:44905b008f44 24 MODSERIAL pc(USBTX,USBRX); //serial communication
Vigilance88 18:44905b008f44 25
Vigilance88 25:49ccdc98639a 26 //Debug legs
Vigilance88 25:49ccdc98639a 27 DigitalOut red(LED_RED);
Vigilance88 25:49ccdc98639a 28 DigitalOut green(LED_GREEN);
Vigilance88 25:49ccdc98639a 29 DigitalOut blue(LED_BLUE);
Vigilance88 25:49ccdc98639a 30
Vigilance88 21:d6a46315d5f5 31 //EMG shields
Vigilance88 18:44905b008f44 32 AnalogIn emg1(A0); //Analog input - Biceps EMG
Vigilance88 18:44905b008f44 33 AnalogIn emg2(A1); //Analog input - Triceps EMG
Vigilance88 18:44905b008f44 34 AnalogIn emg3(A2); //Analog input - Flexor EMG
Vigilance88 18:44905b008f44 35 AnalogIn emg4(A3); //Analog input - Extensor EMG
Vigilance88 18:44905b008f44 36
Vigilance88 18:44905b008f44 37 Ticker sample_timer; //Ticker for EMG sampling
Vigilance88 18:44905b008f44 38 Ticker control_timer; //Ticker for control loop
Vigilance88 18:44905b008f44 39 HIDScope scope(4); //Scope 4 channels
Vigilance88 18:44905b008f44 40
Vigilance88 18:44905b008f44 41 // AnalogIn potmeter(A4); //potmeters
Vigilance88 18:44905b008f44 42 // AnalogIn potmeter2(A5); //
Vigilance88 18:44905b008f44 43
Vigilance88 21:d6a46315d5f5 44 //Encoders
Vigilance88 18:44905b008f44 45 QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses();
Vigilance88 18:44905b008f44 46 QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder,
Vigilance88 21:d6a46315d5f5 47
Vigilance88 21:d6a46315d5f5 48 //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
Vigilance88 21:d6a46315d5f5 49 PwmOut pwm_motor1(D6); //PWM motor 1
Vigilance88 21:d6a46315d5f5 50 PwmOut pwm_motor2(D5); //PWM motor 2
Vigilance88 26:fe3a5469dd6b 51
Vigilance88 18:44905b008f44 52 DigitalOut dir_motor1(D7); //Direction motor 1
Vigilance88 18:44905b008f44 53 DigitalOut dir_motor2(D4); //Direction motor 2
Vigilance88 18:44905b008f44 54
Vigilance88 24:56db31267f10 55 //Limit Switches
Vigilance88 28:743485bb51e4 56 InterruptIn shoulder_limit(D2); //using FRDM buttons
Vigilance88 28:743485bb51e4 57 InterruptIn elbow_limit(D3); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 58
Vigilance88 26:fe3a5469dd6b 59 //Other buttons
Vigilance88 26:fe3a5469dd6b 60 DigitalIn button1(PTA4); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 61 DigitalIn button2(PTC6); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 62
Vigilance88 26:fe3a5469dd6b 63 /*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m
Vigilance88 26:fe3a5469dd6b 64
Vigilance88 26:fe3a5469dd6b 65 \ 0 3 3 - ESC
Vigilance88 26:fe3a5469dd6b 66 [ 3 0 m - black
Vigilance88 26:fe3a5469dd6b 67 [ 3 1 m - red
Vigilance88 26:fe3a5469dd6b 68 [ 3 2 m - green
Vigilance88 26:fe3a5469dd6b 69 [ 3 3 m - yellow
Vigilance88 26:fe3a5469dd6b 70 [ 3 4 m - blue
Vigilance88 26:fe3a5469dd6b 71 [ 3 5 m - magenta
Vigilance88 26:fe3a5469dd6b 72 [ 3 6 m - cyan
Vigilance88 26:fe3a5469dd6b 73 [ 3 7 m - white
Vigilance88 26:fe3a5469dd6b 74 [ 0 m - reset attributes
Vigilance88 26:fe3a5469dd6b 75
Vigilance88 26:fe3a5469dd6b 76 Put the text you want to color between GREEN_ and _GREEN
Vigilance88 26:fe3a5469dd6b 77 */
Vigilance88 26:fe3a5469dd6b 78 string GREEN_ = "\033[32m"; //esc - green
Vigilance88 26:fe3a5469dd6b 79 string _GREEN = "\033[0m"; //esc - reset
Vigilance88 24:56db31267f10 80
Vigilance88 21:d6a46315d5f5 81
Vigilance88 21:d6a46315d5f5 82 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 83 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 84 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 85
Vigilance88 26:fe3a5469dd6b 86 //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction
Vigilance88 24:56db31267f10 87 double emg_biceps; double biceps_power; double bicepsMVC = 0;
Vigilance88 24:56db31267f10 88 double emg_triceps; double triceps_power; double tricepsMVC = 0;
Vigilance88 24:56db31267f10 89 double emg_flexor; double flexor_power; double flexorMVC = 0;
Vigilance88 24:56db31267f10 90 double emg_extens; double extens_power; double extensMVC = 0;
Vigilance88 24:56db31267f10 91
Vigilance88 26:fe3a5469dd6b 92 int muscle; //Muscle selector for MVC measurement
Vigilance88 26:fe3a5469dd6b 93 double calibrate_time; //Elapsed time for each MVC measurement
Vigilance88 25:49ccdc98639a 94
Vigilance88 24:56db31267f10 95 //PID variables
Vigilance88 24:56db31267f10 96 double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2)
Vigilance88 24:56db31267f10 97 double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error
Vigilance88 29:948b0b14f6be 98 const double m1_kp=0.001; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains.
Vigilance88 24:56db31267f10 99
Vigilance88 26:fe3a5469dd6b 100 double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error
Vigilance88 29:948b0b14f6be 101 const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains.
Vigilance88 24:56db31267f10 102
Vigilance88 24:56db31267f10 103 //highpass filter 20 Hz
Vigilance88 24:56db31267f10 104 const double high_b0 = 0.956543225556877;
Vigilance88 24:56db31267f10 105 const double high_b1 = -1.91308645113754;
Vigilance88 24:56db31267f10 106 const double high_b2 = 0.956543225556877;
Vigilance88 24:56db31267f10 107 const double high_a1 = -1.91197067426073;
Vigilance88 24:56db31267f10 108 const double high_a2 = 0.9149758348014341;
Vigilance88 24:56db31267f10 109
Vigilance88 24:56db31267f10 110 //notchfilter 50Hz
Vigilance88 24:56db31267f10 111 /* ** Primary Filter (H1)**
Vigilance88 24:56db31267f10 112 Filter Arithmetic = Floating Point (Double Precision)
Vigilance88 24:56db31267f10 113 Architecture = IIR
Vigilance88 24:56db31267f10 114 Response = Bandstop
Vigilance88 24:56db31267f10 115 Method = Butterworth
Vigilance88 24:56db31267f10 116 Biquad = Yes
Vigilance88 24:56db31267f10 117 Stable = Yes
Vigilance88 24:56db31267f10 118 Sampling Frequency = 500Hz
Vigilance88 24:56db31267f10 119 Filter Order = 2
Vigilance88 24:56db31267f10 120
Vigilance88 24:56db31267f10 121 Band Frequencies (Hz) Att/Ripple (dB) Biquad #1 Biquad #2
Vigilance88 24:56db31267f10 122
Vigilance88 24:56db31267f10 123 1 0, 48 0.001 Gain = 1.000000 Gain = 0.973674
Vigilance88 24:56db31267f10 124 2 49, 51 -60.000 B = [ 1.00000000000, -1.61816176147, 1.00000000000] B = [ 1.00000000000, -1.61816176147, 1.00000000000]
Vigilance88 24:56db31267f10 125 3 52, 250 0.001 A = [ 1.00000000000, -1.58071559235, 0.97319685401] A = [ 1.00000000000, -1.61244708381, 0.97415116257]
Vigilance88 24:56db31267f10 126 */
Vigilance88 24:56db31267f10 127
Vigilance88 24:56db31267f10 128 //biquad 1
Vigilance88 24:56db31267f10 129 const double notch1gain = 1.000000;
Vigilance88 24:56db31267f10 130 const double notch1_b0 = 1;
Vigilance88 24:56db31267f10 131 const double notch1_b1 = -1.61816176147 * notch1gain;
Vigilance88 24:56db31267f10 132 const double notch1_b2 = 1.00000000000 * notch1gain;
Vigilance88 24:56db31267f10 133 const double notch1_a1 = -1.58071559235 * notch1gain;
Vigilance88 24:56db31267f10 134 const double notch1_a2 = 0.97319685401 * notch1gain;
Vigilance88 24:56db31267f10 135
Vigilance88 24:56db31267f10 136 //biquad 2
Vigilance88 24:56db31267f10 137 const double notch2gain = 0.973674;
Vigilance88 24:56db31267f10 138 const double notch2_b0 = 1 * notch2gain;
Vigilance88 24:56db31267f10 139 const double notch2_b1 = -1.61816176147 * notch2gain;
Vigilance88 24:56db31267f10 140 const double notch2_b2 = 1.00000000000 * notch2gain;
Vigilance88 24:56db31267f10 141 const double notch2_a1 = -1.61244708381 * notch2gain;
Vigilance88 24:56db31267f10 142 const double notch2_a2 = 0.97415116257 * notch2gain;
Vigilance88 24:56db31267f10 143
Vigilance88 26:fe3a5469dd6b 144 //lowpass filter 7 Hz - envelope
Vigilance88 24:56db31267f10 145 const double low_b0 = 0.000119046743110057;
Vigilance88 24:56db31267f10 146 const double low_b1 = 0.000238093486220118;
Vigilance88 24:56db31267f10 147 const double low_b2 = 0.000119046743110057;
Vigilance88 24:56db31267f10 148 const double low_a1 = -1.968902268531908;
Vigilance88 24:56db31267f10 149 const double low_a2 = 0.9693784555043481;
Vigilance88 21:d6a46315d5f5 150
Vigilance88 29:948b0b14f6be 151 //Forward and Inverse Kinematics
Vigilance88 28:743485bb51e4 152 const double l1 = 0.25; const double l2 = 0.25;
Vigilance88 28:743485bb51e4 153 double current_x; double current_y;
Vigilance88 28:743485bb51e4 154 double theta1; double theta2;
Vigilance88 28:743485bb51e4 155 double dtheta1; double dtheta2;
Vigilance88 28:743485bb51e4 156 double rpc;
Vigilance88 28:743485bb51e4 157 double x; double y;
Vigilance88 28:743485bb51e4 158 double x_error; double y_error;
Vigilance88 28:743485bb51e4 159 double costheta1; double sintheta1;
Vigilance88 28:743485bb51e4 160 double costheta2; double sintheta2;
Vigilance88 21:d6a46315d5f5 161
Vigilance88 21:d6a46315d5f5 162 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 163 ---- Filters ---------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 164 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 24:56db31267f10 165
Vigilance88 24:56db31267f10 166 //Using biquadFilter library
Vigilance88 24:56db31267f10 167 //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
Vigilance88 26:fe3a5469dd6b 168 //Biceps
Vigilance88 24:56db31267f10 169 biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88 24:56db31267f10 170 biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88 24:56db31267f10 171 biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88 24:56db31267f10 172 biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88 25:49ccdc98639a 173
Vigilance88 26:fe3a5469dd6b 174 //Triceps
Vigilance88 25:49ccdc98639a 175 biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88 26:fe3a5469dd6b 176 biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88 26:fe3a5469dd6b 177 biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88 25:49ccdc98639a 178 biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88 25:49ccdc98639a 179
Vigilance88 26:fe3a5469dd6b 180 //Flexor
Vigilance88 25:49ccdc98639a 181 biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88 26:fe3a5469dd6b 182 biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88 26:fe3a5469dd6b 183 biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88 25:49ccdc98639a 184 biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88 25:49ccdc98639a 185
Vigilance88 26:fe3a5469dd6b 186 //Extensor
Vigilance88 25:49ccdc98639a 187 biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88 26:fe3a5469dd6b 188 biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88 26:fe3a5469dd6b 189 biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88 25:49ccdc98639a 190 biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88 25:49ccdc98639a 191
Vigilance88 26:fe3a5469dd6b 192 //PID filter (lowpass ??? Hz)
Vigilance88 29:948b0b14f6be 193 biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter
Vigilance88 24:56db31267f10 194
Vigilance88 24:56db31267f10 195 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 196 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 197 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 26:fe3a5469dd6b 198
Vigilance88 26:fe3a5469dd6b 199 void sample_filter(void); //Sampling and filtering
Vigilance88 26:fe3a5469dd6b 200 void control(); //Control - reference -> error -> pid -> motor output
Vigilance88 26:fe3a5469dd6b 201 void calibrate_emg(); //Instructions + measurement of MVC of each muscle
Vigilance88 26:fe3a5469dd6b 202 void emg_mvc(); //Helper funcion for storing MVC value
Vigilance88 26:fe3a5469dd6b 203 void calibrate_arm(void); //Calibration of the arm with limit switches
Vigilance88 26:fe3a5469dd6b 204 void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker
Vigilance88 26:fe3a5469dd6b 205 void stop_sampling(void); //Stops sample_filter
Vigilance88 26:fe3a5469dd6b 206 void start_control(void); //Attaches the control function to a 100Hz ticker
Vigilance88 26:fe3a5469dd6b 207 void stop_control(void); //Stops control function
Vigilance88 26:fe3a5469dd6b 208
Vigilance88 26:fe3a5469dd6b 209 //Keeps the input between min and max value
Vigilance88 24:56db31267f10 210 void keep_in_range(double * in, double min, double max);
Vigilance88 26:fe3a5469dd6b 211
Vigilance88 26:fe3a5469dd6b 212 //Reusable PID controller, previous and integral error need to be passed by reference
Vigilance88 21:d6a46315d5f5 213 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88 18:44905b008f44 214
Vigilance88 26:fe3a5469dd6b 215 //Menu functions
Vigilance88 21:d6a46315d5f5 216 void mainMenu();
Vigilance88 21:d6a46315d5f5 217 void caliMenu();
Vigilance88 28:743485bb51e4 218 void controlMenu();
Vigilance88 29:948b0b14f6be 219 void controlButtons();
Vigilance88 26:fe3a5469dd6b 220 void clearTerminal();
Vigilance88 28:743485bb51e4 221 void emgInstructions();
Vigilance88 28:743485bb51e4 222 void titleBox();
Vigilance88 26:fe3a5469dd6b 223
Vigilance88 21:d6a46315d5f5 224
Vigilance88 21:d6a46315d5f5 225 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 226 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 227 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 228
Vigilance88 21:d6a46315d5f5 229 int main()
Vigilance88 21:d6a46315d5f5 230 {
Vigilance88 29:948b0b14f6be 231 pc.baud(115200); //serial baudrate
Vigilance88 26:fe3a5469dd6b 232 red=1; green=1; blue=1; //Make sure debug LEDS are off
Vigilance88 26:fe3a5469dd6b 233
Vigilance88 26:fe3a5469dd6b 234 //Set PwmOut frequency to 10k Hz
Vigilance88 28:743485bb51e4 235 //pwm_motor1.period(PWM_PERIOD);
Vigilance88 28:743485bb51e4 236 //pwm_motor2.period(PWM_PERIOD);
Vigilance88 26:fe3a5469dd6b 237
Vigilance88 26:fe3a5469dd6b 238 clearTerminal(); //Clear the putty window
Vigilance88 26:fe3a5469dd6b 239
Vigilance88 24:56db31267f10 240 // make a menu, user has to initiate next step
Vigilance88 28:743485bb51e4 241 titleBox();
Vigilance88 26:fe3a5469dd6b 242 mainMenu();
Vigilance88 25:49ccdc98639a 243 //caliMenu(); // Menu function
Vigilance88 25:49ccdc98639a 244 //calibrate_arm(); //Start Calibration
Vigilance88 25:49ccdc98639a 245
Vigilance88 27:d1814e271a95 246 //calibrate_emg();
Vigilance88 28:743485bb51e4 247
Vigilance88 28:743485bb51e4 248
Vigilance88 29:948b0b14f6be 249 x=0.5; y=0;
Vigilance88 28:743485bb51e4 250 //start_control(); //100 Hz control
Vigilance88 21:d6a46315d5f5 251
Vigilance88 21:d6a46315d5f5 252 //maybe some stop commands when a button is pressed: detach from timers.
Vigilance88 21:d6a46315d5f5 253 //stop_control();
Vigilance88 29:948b0b14f6be 254 start_sampling();
Vigilance88 29:948b0b14f6be 255 wait(60);
Vigilance88 28:743485bb51e4 256 //char c;
Vigilance88 28:743485bb51e4 257
Vigilance88 28:743485bb51e4 258
Vigilance88 28:743485bb51e4 259
Vigilance88 28:743485bb51e4 260 char command;
Vigilance88 27:d1814e271a95 261
Vigilance88 28:743485bb51e4 262 while(command != 'Q' && command != 'q')
Vigilance88 28:743485bb51e4 263 {
Vigilance88 28:743485bb51e4 264 if(pc.readable()){
Vigilance88 28:743485bb51e4 265 command = pc.getc();
Vigilance88 28:743485bb51e4 266
Vigilance88 28:743485bb51e4 267 switch(command){
Vigilance88 28:743485bb51e4 268
Vigilance88 28:743485bb51e4 269 case 'c':
Vigilance88 28:743485bb51e4 270 case 'C':
Vigilance88 28:743485bb51e4 271 pc.printf("\n\r => You chose calibration.\r\n\n");
Vigilance88 28:743485bb51e4 272 caliMenu();
Vigilance88 28:743485bb51e4 273
Vigilance88 28:743485bb51e4 274 char command2=0;
Vigilance88 28:743485bb51e4 275
Vigilance88 28:743485bb51e4 276 while(command2 != 'B' && command2 != 'b'){
Vigilance88 28:743485bb51e4 277 command2 = pc.getc();
Vigilance88 28:743485bb51e4 278 switch(command2){
Vigilance88 28:743485bb51e4 279 case 'a':
Vigilance88 28:743485bb51e4 280 case 'A':
Vigilance88 28:743485bb51e4 281 pc.printf("\n\r => Arm Calibration Starting... please wait \n\r");
Vigilance88 28:743485bb51e4 282 calibrate_arm();
Vigilance88 28:743485bb51e4 283 wait(1);
Vigilance88 28:743485bb51e4 284 caliMenu();
Vigilance88 28:743485bb51e4 285 break;
Vigilance88 28:743485bb51e4 286
Vigilance88 28:743485bb51e4 287 case 'e':
Vigilance88 28:743485bb51e4 288 case 'E':
Vigilance88 28:743485bb51e4 289 pc.printf("\n\r => EMG Calibration Starting... please wait \n\r");
Vigilance88 28:743485bb51e4 290 wait(1);
Vigilance88 28:743485bb51e4 291 emgInstructions();
Vigilance88 28:743485bb51e4 292 calibrate_emg();
Vigilance88 28:743485bb51e4 293 pc.printf("\n\r ------------------------ \n\r");
Vigilance88 28:743485bb51e4 294 pc.printf("\n\r EMG Calibration complete \n\r");
Vigilance88 28:743485bb51e4 295 pc.printf("\n\r ------------------------ \n\r");
Vigilance88 28:743485bb51e4 296 caliMenu();
Vigilance88 28:743485bb51e4 297 break;
Vigilance88 28:743485bb51e4 298
Vigilance88 28:743485bb51e4 299 case 'b':
Vigilance88 28:743485bb51e4 300 case 'B':
Vigilance88 28:743485bb51e4 301 pc.printf("\n\r => Going back to main menu.. \n\r");
Vigilance88 28:743485bb51e4 302 mainMenu();
Vigilance88 28:743485bb51e4 303 break;
Vigilance88 28:743485bb51e4 304 }//end switch
Vigilance88 28:743485bb51e4 305
Vigilance88 28:743485bb51e4 306 }//end while
Vigilance88 28:743485bb51e4 307 break;
Vigilance88 28:743485bb51e4 308 case 's':
Vigilance88 28:743485bb51e4 309 case 'S':
Vigilance88 28:743485bb51e4 310 pc.printf("=> You chose control \r\n\n");
Vigilance88 28:743485bb51e4 311 wait(1);
Vigilance88 28:743485bb51e4 312 start_sampling();
Vigilance88 28:743485bb51e4 313 wait(1);
Vigilance88 28:743485bb51e4 314 start_control();
Vigilance88 28:743485bb51e4 315 wait(1);
Vigilance88 29:948b0b14f6be 316 controlButtons();
Vigilance88 28:743485bb51e4 317 break;
Vigilance88 28:743485bb51e4 318 case 'R':
Vigilance88 28:743485bb51e4 319 red=!red;
Vigilance88 28:743485bb51e4 320 pc.printf("=> Red LED triggered \n\r");
Vigilance88 28:743485bb51e4 321 break;
Vigilance88 28:743485bb51e4 322 case 'G':
Vigilance88 28:743485bb51e4 323 green=!green;
Vigilance88 28:743485bb51e4 324 pc.printf("=> Green LED triggered \n\r");
Vigilance88 28:743485bb51e4 325 break;
Vigilance88 28:743485bb51e4 326 case 'B':
Vigilance88 28:743485bb51e4 327 blue=!blue;
Vigilance88 28:743485bb51e4 328 pc.printf("=> Blue LED triggered \n\r");
Vigilance88 28:743485bb51e4 329 break;
Vigilance88 28:743485bb51e4 330 case 'q':
Vigilance88 28:743485bb51e4 331 case 'Q':
Vigilance88 28:743485bb51e4 332
Vigilance88 28:743485bb51e4 333 break;
Vigilance88 28:743485bb51e4 334 default:
Vigilance88 28:743485bb51e4 335 pc.printf("=> Invalid Input \n\r");
Vigilance88 28:743485bb51e4 336 break;
Vigilance88 28:743485bb51e4 337 } //end switch
Vigilance88 28:743485bb51e4 338 } // end if pc readable
Vigilance88 28:743485bb51e4 339
Vigilance88 28:743485bb51e4 340 } // end while loop
Vigilance88 28:743485bb51e4 341
Vigilance88 28:743485bb51e4 342
Vigilance88 28:743485bb51e4 343
Vigilance88 28:743485bb51e4 344 //When end of while loop reached (user chose quit program) - detach all timers and stop motors.
Vigilance88 28:743485bb51e4 345
Vigilance88 28:743485bb51e4 346 pc.printf("\r\n------------------------------ \n\r");
Vigilance88 28:743485bb51e4 347 pc.printf("Program Offline \n\r");
Vigilance88 28:743485bb51e4 348 pc.printf("Reset to start\r\n");
Vigilance88 28:743485bb51e4 349 pc.printf("------------------------------ \n\r");
Vigilance88 28:743485bb51e4 350 }
Vigilance88 28:743485bb51e4 351 //end of main
Vigilance88 28:743485bb51e4 352
Vigilance88 28:743485bb51e4 353 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 28:743485bb51e4 354 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
Vigilance88 28:743485bb51e4 355 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 28:743485bb51e4 356
Vigilance88 29:948b0b14f6be 357 void controlButtons()
Vigilance88 28:743485bb51e4 358 {
Vigilance88 28:743485bb51e4 359 controlMenu();
Vigilance88 28:743485bb51e4 360 char c=0;
Vigilance88 28:743485bb51e4 361 while(c != 'Q' && c != 'q') {
Vigilance88 27:d1814e271a95 362
Vigilance88 27:d1814e271a95 363 if( pc.readable() ){
Vigilance88 27:d1814e271a95 364 c = pc.getc();
Vigilance88 27:d1814e271a95 365 switch (c)
Vigilance88 27:d1814e271a95 366 {
Vigilance88 27:d1814e271a95 367 case '1' :
Vigilance88 27:d1814e271a95 368 x = x + 0.01;
Vigilance88 27:d1814e271a95 369 //controlMenu();
Vigilance88 27:d1814e271a95 370 //running=false;
Vigilance88 27:d1814e271a95 371 break;
Vigilance88 27:d1814e271a95 372
Vigilance88 27:d1814e271a95 373 case '2' :
Vigilance88 27:d1814e271a95 374 x-=0.01;
Vigilance88 27:d1814e271a95 375 //controlMenu();
Vigilance88 27:d1814e271a95 376 //running=false;
Vigilance88 27:d1814e271a95 377 break;
Vigilance88 27:d1814e271a95 378
Vigilance88 27:d1814e271a95 379 case '3' :
Vigilance88 27:d1814e271a95 380 y+=0.01;
Vigilance88 27:d1814e271a95 381 //controlMenu();
Vigilance88 27:d1814e271a95 382 //running=false;
Vigilance88 27:d1814e271a95 383 break;
Vigilance88 27:d1814e271a95 384
Vigilance88 27:d1814e271a95 385
Vigilance88 27:d1814e271a95 386 case '4' :
Vigilance88 27:d1814e271a95 387 y-=0.01;
Vigilance88 27:d1814e271a95 388 //controlMenu();
Vigilance88 27:d1814e271a95 389 //running=false;
Vigilance88 27:d1814e271a95 390 break;
Vigilance88 27:d1814e271a95 391
Vigilance88 27:d1814e271a95 392 case 'q' :
Vigilance88 28:743485bb51e4 393 case 'Q' :
Vigilance88 28:743485bb51e4 394 pc.printf("=> Quitting control... \r\n"); wait(1);
Vigilance88 28:743485bb51e4 395 stop_sampling();
Vigilance88 28:743485bb51e4 396 stop_control();
Vigilance88 28:743485bb51e4 397 pwm_motor1=0; pwm_motor2=0;
Vigilance88 28:743485bb51e4 398 pc.printf("Sampling and Control detached \n\r"); wait(1);
Vigilance88 28:743485bb51e4 399 pc.printf("Returning to Main Menu \r\n\n"); wait(1);
Vigilance88 28:743485bb51e4 400 mainMenu();
Vigilance88 28:743485bb51e4 401
Vigilance88 27:d1814e271a95 402 //running = false;
Vigilance88 27:d1814e271a95 403 break;
Vigilance88 27:d1814e271a95 404 }//end switch
Vigilance88 28:743485bb51e4 405 if(c!='q' && c!='Q'){
Vigilance88 27:d1814e271a95 406 pc.printf("Reference position: %f and %f \r\n",x,y);
Vigilance88 27:d1814e271a95 407 pc.printf("Current position: %f and %f \r\n",current_x,current_y);
Vigilance88 27:d1814e271a95 408 pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
Vigilance88 27:d1814e271a95 409 pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
Vigilance88 27:d1814e271a95 410 pc.printf("PID output: %f and %f \r\n",u1,u2);
Vigilance88 27:d1814e271a95 411 pc.printf("----------------------------------------\r\n\n");
Vigilance88 27:d1814e271a95 412 }
Vigilance88 28:743485bb51e4 413 }
Vigilance88 27:d1814e271a95 414 //end if
Vigilance88 21:d6a46315d5f5 415 }
Vigilance88 21:d6a46315d5f5 416 //end of while loop
Vigilance88 28:743485bb51e4 417 }
Vigilance88 18:44905b008f44 418
Vigilance88 21:d6a46315d5f5 419 //Sample and Filter
Vigilance88 21:d6a46315d5f5 420 void sample_filter(void)
Vigilance88 18:44905b008f44 421 {
Vigilance88 21:d6a46315d5f5 422 double emg_biceps = emg1.read(); //Biceps
Vigilance88 21:d6a46315d5f5 423 double emg_triceps = emg2.read(); //Triceps
Vigilance88 21:d6a46315d5f5 424 double emg_flexor = emg3.read(); //Flexor
Vigilance88 21:d6a46315d5f5 425 double emg_extens = emg4.read(); //Extensor
Vigilance88 21:d6a46315d5f5 426
Vigilance88 21:d6a46315d5f5 427 //Filter: high-pass -> notch -> rectify -> lowpass or moving average
Vigilance88 22:1ba637601dc3 428 // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
Vigilance88 25:49ccdc98639a 429 biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens);
Vigilance88 25:49ccdc98639a 430 biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power);
Vigilance88 25:49ccdc98639a 431 biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power);
Vigilance88 21:d6a46315d5f5 432 biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
Vigilance88 25:49ccdc98639a 433 biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
Vigilance88 25:49ccdc98639a 434
Vigilance88 29:948b0b14f6be 435 scope.set(0,emg_biceps);
Vigilance88 29:948b0b14f6be 436 scope.set(1,biceps_power);
Vigilance88 29:948b0b14f6be 437 scope.set(2,biceps_power);
Vigilance88 29:948b0b14f6be 438 scope.set(3,biceps_power);
Vigilance88 29:948b0b14f6be 439 scope.send();
Vigilance88 21:d6a46315d5f5 440 /* alternative for lowpass filter: moving average
Vigilance88 21:d6a46315d5f5 441 window=30; //30 samples
Vigilance88 21:d6a46315d5f5 442 int i=0; //buffer index
Vigilance88 21:d6a46315d5f5 443 bicepsbuffer[i]=biceps_power //fill array
Vigilance88 21:d6a46315d5f5 444
Vigilance88 21:d6a46315d5f5 445 i++;
Vigilance88 21:d6a46315d5f5 446 if(i==window){
Vigilance88 21:d6a46315d5f5 447 i=0;
Vigilance88 21:d6a46315d5f5 448 }
Vigilance88 21:d6a46315d5f5 449
Vigilance88 24:56db31267f10 450 for(int x = 0; x < window; x++){
Vigilance88 24:56db31267f10 451 avg1 += bicepsbuffer[x];
Vigilance88 24:56db31267f10 452 }
Vigilance88 24:56db31267f10 453 avg1 = avg1/window;
Vigilance88 21:d6a46315d5f5 454 */
Vigilance88 21:d6a46315d5f5 455
Vigilance88 18:44905b008f44 456 }
Vigilance88 18:44905b008f44 457
Vigilance88 27:d1814e271a95 458 void controlMenu()
Vigilance88 27:d1814e271a95 459 {
Vigilance88 28:743485bb51e4 460 //Title Box
Vigilance88 28:743485bb51e4 461 pc.putc(201);
Vigilance88 28:743485bb51e4 462 for(int j=0;j<33;j++){
Vigilance88 28:743485bb51e4 463 pc.putc(205);
Vigilance88 28:743485bb51e4 464 }
Vigilance88 28:743485bb51e4 465 pc.putc(187);
Vigilance88 28:743485bb51e4 466 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 467 pc.putc(186); pc.printf(" Control Menu "); pc.putc(186);
Vigilance88 28:743485bb51e4 468 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 469 pc.putc(200);
Vigilance88 28:743485bb51e4 470 for(int k=0;k<33;k++){
Vigilance88 28:743485bb51e4 471 pc.putc(205);
Vigilance88 28:743485bb51e4 472 }
Vigilance88 28:743485bb51e4 473 pc.putc(188);
Vigilance88 28:743485bb51e4 474
Vigilance88 28:743485bb51e4 475 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 476 //endbox
Vigilance88 27:d1814e271a95 477 pc.printf("1) Move Arm Left\r\n");
Vigilance88 27:d1814e271a95 478 pc.printf("2) Move Arm Right\r\n");
Vigilance88 27:d1814e271a95 479 pc.printf("3) Move Arm Up\r\n");
Vigilance88 27:d1814e271a95 480 pc.printf("4) Move Arm Down\r\n");
Vigilance88 27:d1814e271a95 481 pc.printf("q) Exit \r\n");
Vigilance88 27:d1814e271a95 482 pc.printf("Please make a choice => \r\n");
Vigilance88 27:d1814e271a95 483 }
Vigilance88 27:d1814e271a95 484
Vigilance88 18:44905b008f44 485 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88 19:0a3ee31dcdb4 486 void calibrate_arm(void)
Vigilance88 19:0a3ee31dcdb4 487 {
Vigilance88 28:743485bb51e4 488 pc.printf("Calibrate_arm() entered\r\n");
Vigilance88 26:fe3a5469dd6b 489 bool calibrating = true;
Vigilance88 26:fe3a5469dd6b 490 bool done1 = false;
Vigilance88 26:fe3a5469dd6b 491 bool done2 = false;
Vigilance88 27:d1814e271a95 492 pc.printf("To start arm calibration, press any key =>");
Vigilance88 27:d1814e271a95 493 pc.getc();
Vigilance88 27:d1814e271a95 494 pc.printf("\r\n Calibrating... \r\n");
Vigilance88 26:fe3a5469dd6b 495 dir_motor1=1; //cw
Vigilance88 27:d1814e271a95 496 dir_motor2=0; //cw
Vigilance88 27:d1814e271a95 497 pwm_motor1.write(0.2); //move upper arm slowly cw
Vigilance88 27:d1814e271a95 498
Vigilance88 26:fe3a5469dd6b 499
Vigilance88 26:fe3a5469dd6b 500 while(calibrating){
Vigilance88 27:d1814e271a95 501 red=0; blue=0; //Debug light is purple during arm calibration
Vigilance88 27:d1814e271a95 502
Vigilance88 27:d1814e271a95 503 if(done1==true){
Vigilance88 27:d1814e271a95 504 pwm_motor2.write(0.2); //move forearm slowly cw
Vigilance88 27:d1814e271a95 505 }
Vigilance88 27:d1814e271a95 506
Vigilance88 26:fe3a5469dd6b 507 //when limit switches are hit, stop motor and reset encoder
Vigilance88 26:fe3a5469dd6b 508 if(shoulder_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 509 pwm_motor1.write(0);
Vigilance88 26:fe3a5469dd6b 510 Encoder1.reset();
Vigilance88 26:fe3a5469dd6b 511 done1 = true;
Vigilance88 27:d1814e271a95 512 pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
Vigilance88 26:fe3a5469dd6b 513 }
Vigilance88 26:fe3a5469dd6b 514 if(elbow_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 515 pwm_motor2.write(0);
Vigilance88 26:fe3a5469dd6b 516 Encoder2.reset();
Vigilance88 26:fe3a5469dd6b 517 done2 = true;
Vigilance88 27:d1814e271a95 518 pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
Vigilance88 26:fe3a5469dd6b 519 }
Vigilance88 26:fe3a5469dd6b 520 if(done1 && done2){
Vigilance88 26:fe3a5469dd6b 521 calibrating=false; //Leave while loop when both limits are reached
Vigilance88 26:fe3a5469dd6b 522 red=1; blue=1; //Turn debug light off when calibration complete
Vigilance88 26:fe3a5469dd6b 523 }
Vigilance88 27:d1814e271a95 524
Vigilance88 27:d1814e271a95 525 }//end while
Vigilance88 25:49ccdc98639a 526
Vigilance88 27:d1814e271a95 527 //TO DO:
Vigilance88 27:d1814e271a95 528 //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) )
Vigilance88 27:d1814e271a95 529 //Encoder1.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:d1814e271a95 530 //Encoder2.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:d1814e271a95 531 //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
Vigilance88 27:d1814e271a95 532 wait(1);
Vigilance88 28:743485bb51e4 533 pc.printf("\n\r ------------------------ \n\r");
Vigilance88 27:d1814e271a95 534 pc.printf("Arm Calibration Complete\r\n");
Vigilance88 28:743485bb51e4 535 pc.printf(" ------------------------ \n\r");
Vigilance88 26:fe3a5469dd6b 536
Vigilance88 19:0a3ee31dcdb4 537 }
Vigilance88 19:0a3ee31dcdb4 538
Vigilance88 21:d6a46315d5f5 539 //EMG Maximum Voluntary Contraction measurement
Vigilance88 25:49ccdc98639a 540 void emg_mvc()
Vigilance88 25:49ccdc98639a 541 {
Vigilance88 24:56db31267f10 542 //double sampletime=0;
Vigilance88 24:56db31267f10 543 //sampletime=+SAMPLE_RATE;
Vigilance88 24:56db31267f10 544 //
Vigilance88 24:56db31267f10 545 // if(sampletime<5)
Vigilance88 25:49ccdc98639a 546 //int muscle=1;
Vigilance88 25:49ccdc98639a 547 //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 25:49ccdc98639a 548
Vigilance88 24:56db31267f10 549 if(muscle==1){
Vigilance88 24:56db31267f10 550
Vigilance88 24:56db31267f10 551 if(biceps_power>bicepsMVC){
Vigilance88 26:fe3a5469dd6b 552 //printf("+ ");
Vigilance88 26:fe3a5469dd6b 553 printf("%s+ %s",GREEN_,_GREEN);
Vigilance88 21:d6a46315d5f5 554 bicepsMVC=biceps_power;
Vigilance88 24:56db31267f10 555 }
Vigilance88 25:49ccdc98639a 556 else
Vigilance88 25:49ccdc98639a 557 printf("- ");
Vigilance88 24:56db31267f10 558 }
Vigilance88 24:56db31267f10 559
Vigilance88 24:56db31267f10 560 if(muscle==2){
Vigilance88 24:56db31267f10 561
Vigilance88 24:56db31267f10 562 if(triceps_power>tricepsMVC){
Vigilance88 26:fe3a5469dd6b 563 printf("%s+ %s",GREEN_,_GREEN);
Vigilance88 24:56db31267f10 564 tricepsMVC=triceps_power;
Vigilance88 24:56db31267f10 565 }
Vigilance88 26:fe3a5469dd6b 566 else
Vigilance88 26:fe3a5469dd6b 567 printf("- ");
Vigilance88 24:56db31267f10 568 }
Vigilance88 24:56db31267f10 569
Vigilance88 24:56db31267f10 570 if(muscle==3){
Vigilance88 24:56db31267f10 571
Vigilance88 24:56db31267f10 572 if(flexor_power>flexorMVC){
Vigilance88 26:fe3a5469dd6b 573 printf("%s+ %s",GREEN_,_GREEN);
Vigilance88 24:56db31267f10 574 flexorMVC=flexor_power;
Vigilance88 24:56db31267f10 575 }
Vigilance88 26:fe3a5469dd6b 576 else
Vigilance88 26:fe3a5469dd6b 577 printf("- ");
Vigilance88 24:56db31267f10 578 }
Vigilance88 24:56db31267f10 579
Vigilance88 24:56db31267f10 580 if(muscle==4){
Vigilance88 24:56db31267f10 581
Vigilance88 24:56db31267f10 582 if(extens_power>extensMVC){
Vigilance88 26:fe3a5469dd6b 583 printf("%s+ %s",GREEN_,_GREEN);
Vigilance88 24:56db31267f10 584 extensMVC=extens_power;
Vigilance88 24:56db31267f10 585 }
Vigilance88 26:fe3a5469dd6b 586 else
Vigilance88 26:fe3a5469dd6b 587 printf("- ");
Vigilance88 24:56db31267f10 588 }
Vigilance88 25:49ccdc98639a 589
Vigilance88 25:49ccdc98639a 590 //}
Vigilance88 25:49ccdc98639a 591 calibrate_time = calibrate_time + 0.002;
Vigilance88 25:49ccdc98639a 592
Vigilance88 25:49ccdc98639a 593
Vigilance88 25:49ccdc98639a 594
Vigilance88 25:49ccdc98639a 595 }
Vigilance88 25:49ccdc98639a 596
Vigilance88 25:49ccdc98639a 597 //EMG calibration
Vigilance88 25:49ccdc98639a 598 void calibrate_emg()
Vigilance88 25:49ccdc98639a 599 {
Vigilance88 25:49ccdc98639a 600 Ticker timer;
Vigilance88 25:49ccdc98639a 601
Vigilance88 25:49ccdc98639a 602 pc.printf("Testcode calibration \r\n");
Vigilance88 25:49ccdc98639a 603 wait(1);
Vigilance88 25:49ccdc98639a 604 pc.printf("+ means current sample is higher than stored MVC\r\n");
Vigilance88 25:49ccdc98639a 605 pc.printf("- means current sample is lower than stored MVC\r\n");
Vigilance88 26:fe3a5469dd6b 606 wait(2);
Vigilance88 28:743485bb51e4 607 pc.printf("\r\n----------------\r\n ");
Vigilance88 28:743485bb51e4 608 pc.printf(" Biceps is first.\r\n ");
Vigilance88 28:743485bb51e4 609 pc.printf("----------------\r\n ");
Vigilance88 28:743485bb51e4 610 wait(1);
Vigilance88 25:49ccdc98639a 611 pc.printf(" Press any key to begin... "); wait(1);
Vigilance88 25:49ccdc98639a 612 char input;
Vigilance88 25:49ccdc98639a 613 input=pc.getc();
Vigilance88 25:49ccdc98639a 614 pc.putc(input);
Vigilance88 25:49ccdc98639a 615 pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 616 pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 617 pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 618
Vigilance88 25:49ccdc98639a 619 start_sampling();
Vigilance88 25:49ccdc98639a 620 muscle=1;
Vigilance88 27:d1814e271a95 621 timer.attach(&emg_mvc,SAMPLE_RATE);
Vigilance88 25:49ccdc98639a 622 wait(5);
Vigilance88 25:49ccdc98639a 623 timer.detach();
Vigilance88 26:fe3a5469dd6b 624
Vigilance88 26:fe3a5469dd6b 625 pc.printf("\r\n Measurement complete."); wait(1);
Vigilance88 26:fe3a5469dd6b 626 pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1);
Vigilance88 26:fe3a5469dd6b 627 pc.printf("Calibrate_emg() exited \r\n"); wait(1);
Vigilance88 26:fe3a5469dd6b 628 pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
Vigilance88 25:49ccdc98639a 629 calibrate_time=0;
Vigilance88 25:49ccdc98639a 630
Vigilance88 25:49ccdc98639a 631 // Triceps:
Vigilance88 26:fe3a5469dd6b 632 muscle=2;
Vigilance88 28:743485bb51e4 633 pc.printf("\r\n----------------\r\n ");
Vigilance88 28:743485bb51e4 634 pc.printf(" Triceps is next.\r\n ");
Vigilance88 28:743485bb51e4 635 pc.printf("----------------\r\n ");
Vigilance88 28:743485bb51e4 636 wait(1);
Vigilance88 28:743485bb51e4 637
Vigilance88 25:49ccdc98639a 638 pc.printf(" Press any key to begin... "); wait(1);
Vigilance88 25:49ccdc98639a 639 input=pc.getc();
Vigilance88 25:49ccdc98639a 640 pc.putc(input);
Vigilance88 25:49ccdc98639a 641 pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 642 pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 643 pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88 25:49ccdc98639a 644 start_sampling();
Vigilance88 25:49ccdc98639a 645 muscle=1;
Vigilance88 25:49ccdc98639a 646 timer.attach(&emg_mvc,0.002);
Vigilance88 25:49ccdc98639a 647 wait(5);
Vigilance88 25:49ccdc98639a 648 timer.detach();
Vigilance88 25:49ccdc98639a 649 pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
Vigilance88 25:49ccdc98639a 650
Vigilance88 25:49ccdc98639a 651 pc.printf("Calibrate_emg() exited \r\n");
Vigilance88 25:49ccdc98639a 652 pc.printf("Measured time: %f seconds \r\n",calibrate_time);
Vigilance88 25:49ccdc98639a 653 calibrate_time=0;
Vigilance88 25:49ccdc98639a 654
Vigilance88 25:49ccdc98639a 655 //Flexor:
Vigilance88 26:fe3a5469dd6b 656 muscle=3;
Vigilance88 25:49ccdc98639a 657 //Extensor:
Vigilance88 26:fe3a5469dd6b 658 muscle=4;
Vigilance88 25:49ccdc98639a 659
Vigilance88 26:fe3a5469dd6b 660 //Stop sampling, detach ticker
Vigilance88 25:49ccdc98639a 661 stop_sampling();
Vigilance88 24:56db31267f10 662
Vigilance88 18:44905b008f44 663 }
Vigilance88 18:44905b008f44 664
Vigilance88 18:44905b008f44 665
Vigilance88 18:44905b008f44 666 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 20:0ede3818e08e 667 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 668 {
Vigilance88 20:0ede3818e08e 669 // Derivative
Vigilance88 24:56db31267f10 670 double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88 21:d6a46315d5f5 671 e_der = derfilter.step(e_der);
Vigilance88 21:d6a46315d5f5 672 e_prev = error;
Vigilance88 20:0ede3818e08e 673 // Integral
Vigilance88 24:56db31267f10 674 e_int = e_int + CONTROL_RATE * error;
Vigilance88 20:0ede3818e08e 675 // PID
Vigilance88 21:d6a46315d5f5 676 return kp*error + ki*e_int + kd * e_der;
Vigilance88 20:0ede3818e08e 677
Vigilance88 18:44905b008f44 678 }
Vigilance88 18:44905b008f44 679
Vigilance88 20:0ede3818e08e 680 //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 681 void control()
Vigilance88 18:44905b008f44 682 {
Vigilance88 20:0ede3818e08e 683 //analyze emg (= velocity, averages?)
Vigilance88 18:44905b008f44 684
Vigilance88 18:44905b008f44 685 //calculate reference position based on the average emg (integrate)
Vigilance88 18:44905b008f44 686
Vigilance88 27:d1814e271a95 687 //Current position - Encoder counts -> current angle -> Forward Kinematics
Vigilance88 27:d1814e271a95 688 rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200
Vigilance88 27:d1814e271a95 689 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts
Vigilance88 27:d1814e271a95 690 theta2 = Encoder2.getPulses() * rpc;
Vigilance88 27:d1814e271a95 691 current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
Vigilance88 27:d1814e271a95 692 current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
Vigilance88 27:d1814e271a95 693
Vigilance88 27:d1814e271a95 694 //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y);
Vigilance88 27:d1814e271a95 695
Vigilance88 27:d1814e271a95 696
Vigilance88 27:d1814e271a95 697 //pc.printf("X is %f and Y is %f \r\n",x,y);
Vigilance88 27:d1814e271a95 698
Vigilance88 18:44905b008f44 699 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 27:d1814e271a95 700 x_error = x - current_x;
Vigilance88 27:d1814e271a95 701 y_error = y - current_y;
Vigilance88 27:d1814e271a95 702
Vigilance88 27:d1814e271a95 703 //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error);
Vigilance88 27:d1814e271a95 704
Vigilance88 27:d1814e271a95 705 //inverse kinematics (refpos to refangle)
Vigilance88 18:44905b008f44 706
Vigilance88 27:d1814e271a95 707 costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
Vigilance88 27:d1814e271a95 708 sintheta2 = sqrt( 1 - pow(costheta2,2) );
Vigilance88 27:d1814e271a95 709
Vigilance88 27:d1814e271a95 710 //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2);
Vigilance88 27:d1814e271a95 711
Vigilance88 27:d1814e271a95 712 dtheta2 = atan2(sintheta2,costheta2);
Vigilance88 27:d1814e271a95 713
Vigilance88 27:d1814e271a95 714 costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
Vigilance88 27:d1814e271a95 715 sintheta1 = sqrt( 1 - pow(costheta1,2) );
Vigilance88 27:d1814e271a95 716
Vigilance88 27:d1814e271a95 717 //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1);
Vigilance88 27:d1814e271a95 718
Vigilance88 27:d1814e271a95 719 dtheta1 = atan2(sintheta1,costheta1);
Vigilance88 27:d1814e271a95 720
Vigilance88 27:d1814e271a95 721
Vigilance88 27:d1814e271a95 722 //Angle error
Vigilance88 27:d1814e271a95 723
Vigilance88 27:d1814e271a95 724 m1_error = dtheta1-theta1;
Vigilance88 27:d1814e271a95 725 m2_error = dtheta2-theta2;
Vigilance88 27:d1814e271a95 726
Vigilance88 27:d1814e271a95 727 //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error);
Vigilance88 18:44905b008f44 728
Vigilance88 18:44905b008f44 729 //PID controller
Vigilance88 24:56db31267f10 730
Vigilance88 24:56db31267f10 731 u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1
Vigilance88 24:56db31267f10 732 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2
Vigilance88 21:d6a46315d5f5 733
Vigilance88 27:d1814e271a95 734 keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1
Vigilance88 27:d1814e271a95 735 keep_in_range(&u2,-0.6,0.6);
Vigilance88 21:d6a46315d5f5 736
Vigilance88 21:d6a46315d5f5 737 //send PWM and DIR to motor 1
Vigilance88 21:d6a46315d5f5 738 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88 21:d6a46315d5f5 739 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 740
Vigilance88 21:d6a46315d5f5 741 //send PWM and DIR to motor 2
Vigilance88 27:d1814e271a95 742 dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below
Vigilance88 25:49ccdc98639a 743 pwm_motor2.write(abs(u2));
Vigilance88 21:d6a46315d5f5 744
Vigilance88 21:d6a46315d5f5 745 /*if(u1 > 0)
Vigilance88 21:d6a46315d5f5 746 {
Vigilance88 21:d6a46315d5f5 747 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 748 else{
Vigilance88 21:d6a46315d5f5 749 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 750 }
Vigilance88 21:d6a46315d5f5 751 }
Vigilance88 21:d6a46315d5f5 752 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 753
Vigilance88 21:d6a46315d5f5 754
Vigilance88 21:d6a46315d5f5 755 if(u2 > 0)
Vigilance88 21:d6a46315d5f5 756 {
Vigilance88 21:d6a46315d5f5 757 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 758 else{
Vigilance88 21:d6a46315d5f5 759 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 760 }
Vigilance88 21:d6a46315d5f5 761 }
Vigilance88 21:d6a46315d5f5 762 pwm_motor1.write(abs(u2));*/
Vigilance88 21:d6a46315d5f5 763
Vigilance88 18:44905b008f44 764 }
Vigilance88 18:44905b008f44 765
Vigilance88 26:fe3a5469dd6b 766 void mainMenu()
Vigilance88 26:fe3a5469dd6b 767 {
Vigilance88 28:743485bb51e4 768 //Title Box
Vigilance88 26:fe3a5469dd6b 769 pc.putc(201);
Vigilance88 26:fe3a5469dd6b 770 for(int j=0;j<33;j++){
Vigilance88 26:fe3a5469dd6b 771 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 772 }
Vigilance88 26:fe3a5469dd6b 773 pc.putc(187);
Vigilance88 26:fe3a5469dd6b 774 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 775 pc.putc(186); pc.printf(" Main Menu "); pc.putc(186);
Vigilance88 26:fe3a5469dd6b 776 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 777 pc.putc(200);
Vigilance88 26:fe3a5469dd6b 778 for(int k=0;k<33;k++){
Vigilance88 26:fe3a5469dd6b 779 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 780 }
Vigilance88 26:fe3a5469dd6b 781 pc.putc(188);
Vigilance88 26:fe3a5469dd6b 782
Vigilance88 26:fe3a5469dd6b 783 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 784 //endbox
Vigilance88 28:743485bb51e4 785 wait(1);
Vigilance88 28:743485bb51e4 786 pc.printf("[C]alibration\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 787 pc.printf("[S]tart Control with buttons\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 788 pc.printf("[Q]uit Robot Program\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 789 pc.printf("[R]ed LED\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 790 pc.printf("[G]reen LED\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 791 pc.printf("[B]lue LED\r\n"); wait(0.2);
Vigilance88 28:743485bb51e4 792 pc.printf("Please make a choice => \r\n");
Vigilance88 26:fe3a5469dd6b 793 }
Vigilance88 24:56db31267f10 794
Vigilance88 24:56db31267f10 795 //Start sampling
Vigilance88 24:56db31267f10 796 void start_sampling(void)
Vigilance88 24:56db31267f10 797 {
Vigilance88 24:56db31267f10 798 sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
Vigilance88 26:fe3a5469dd6b 799
Vigilance88 26:fe3a5469dd6b 800 //Debug LED will be green when sampling is active
Vigilance88 26:fe3a5469dd6b 801 green=0;
Vigilance88 25:49ccdc98639a 802 pc.printf("||- started sampling -|| \r\n");
Vigilance88 24:56db31267f10 803 }
Vigilance88 24:56db31267f10 804
Vigilance88 24:56db31267f10 805 //stop sampling
Vigilance88 24:56db31267f10 806 void stop_sampling(void)
Vigilance88 24:56db31267f10 807 {
Vigilance88 24:56db31267f10 808 sample_timer.detach();
Vigilance88 26:fe3a5469dd6b 809
Vigilance88 26:fe3a5469dd6b 810 //Debug LED will be turned off when sampling stops
Vigilance88 26:fe3a5469dd6b 811 green=1;
Vigilance88 25:49ccdc98639a 812 pc.printf("||- stopped sampling -|| \r\n");
Vigilance88 24:56db31267f10 813 }
Vigilance88 24:56db31267f10 814
Vigilance88 18:44905b008f44 815 //Start control
Vigilance88 18:44905b008f44 816 void start_control(void)
Vigilance88 18:44905b008f44 817 {
Vigilance88 27:d1814e271a95 818 control_timer.attach(&control,CONTROL_RATE); //100 Hz control
Vigilance88 26:fe3a5469dd6b 819
Vigilance88 26:fe3a5469dd6b 820 //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
Vigilance88 26:fe3a5469dd6b 821 blue=0;
Vigilance88 26:fe3a5469dd6b 822 pc.printf("||- started control -|| \r\n");
Vigilance88 18:44905b008f44 823 }
Vigilance88 18:44905b008f44 824
Vigilance88 18:44905b008f44 825 //stop control
Vigilance88 18:44905b008f44 826 void stop_control(void)
Vigilance88 18:44905b008f44 827 {
Vigilance88 18:44905b008f44 828 control_timer.detach();
Vigilance88 26:fe3a5469dd6b 829
Vigilance88 26:fe3a5469dd6b 830 //Debug LED will be off when control is off
Vigilance88 26:fe3a5469dd6b 831 blue=1;
Vigilance88 26:fe3a5469dd6b 832 pc.printf("||- stopped control -|| \r\n");
vsluiter 2:e314bb3b2d99 833 }
vsluiter 0:32bb76391d89 834
Vigilance88 21:d6a46315d5f5 835
Vigilance88 21:d6a46315d5f5 836 void calibrate()
vsluiter 0:32bb76391d89 837 {
Vigilance88 21:d6a46315d5f5 838
Vigilance88 21:d6a46315d5f5 839 }
tomlankhorst 15:0da764eea774 840
Vigilance88 26:fe3a5469dd6b 841 //Clears the putty (or other terminal) window
Vigilance88 26:fe3a5469dd6b 842 void clearTerminal()
Vigilance88 26:fe3a5469dd6b 843 {
Vigilance88 26:fe3a5469dd6b 844 pc.putc(27);
Vigilance88 26:fe3a5469dd6b 845 pc.printf("[2J"); // clear screen
Vigilance88 26:fe3a5469dd6b 846 pc.putc(27); // ESC
Vigilance88 26:fe3a5469dd6b 847 pc.printf("[H"); // cursor to home
Vigilance88 26:fe3a5469dd6b 848 }
Vigilance88 21:d6a46315d5f5 849
Vigilance88 28:743485bb51e4 850 void caliMenu(){
Vigilance88 28:743485bb51e4 851
Vigilance88 28:743485bb51e4 852 //Title Box
Vigilance88 28:743485bb51e4 853 pc.putc(201);
Vigilance88 28:743485bb51e4 854 for(int j=0;j<33;j++){
Vigilance88 28:743485bb51e4 855 pc.putc(205);
Vigilance88 28:743485bb51e4 856 }
Vigilance88 28:743485bb51e4 857 pc.putc(187);
Vigilance88 28:743485bb51e4 858 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 859 pc.putc(186); pc.printf(" Calibration Menu "); pc.putc(186);
Vigilance88 28:743485bb51e4 860 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 861 pc.putc(200);
Vigilance88 28:743485bb51e4 862 for(int k=0;k<33;k++){
Vigilance88 28:743485bb51e4 863 pc.putc(205);
Vigilance88 28:743485bb51e4 864 }
Vigilance88 28:743485bb51e4 865 pc.putc(188);
Vigilance88 28:743485bb51e4 866
Vigilance88 28:743485bb51e4 867 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 868 //endbox
Vigilance88 28:743485bb51e4 869
Vigilance88 28:743485bb51e4 870 pc.printf("[A]rm Calibration\r\n");
Vigilance88 28:743485bb51e4 871 pc.printf("[E]MG Calibration\r\n");
Vigilance88 28:743485bb51e4 872 pc.printf("[B]ack to main menu\r\n");
Vigilance88 28:743485bb51e4 873 pc.printf("Please make a choice => \r\n");
Vigilance88 28:743485bb51e4 874
Vigilance88 28:743485bb51e4 875 }
Vigilance88 28:743485bb51e4 876
Vigilance88 28:743485bb51e4 877 void titleBox(){
Vigilance88 28:743485bb51e4 878
Vigilance88 28:743485bb51e4 879 //Title Box
Vigilance88 28:743485bb51e4 880 pc.putc(201);
Vigilance88 28:743485bb51e4 881 for(int j=0;j<33;j++){
Vigilance88 28:743485bb51e4 882 pc.putc(205);
Vigilance88 28:743485bb51e4 883 }
Vigilance88 28:743485bb51e4 884 pc.putc(187);
Vigilance88 28:743485bb51e4 885 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 886 pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186);
Vigilance88 28:743485bb51e4 887 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 888 pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186);
Vigilance88 28:743485bb51e4 889 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 890 pc.putc(200);
Vigilance88 28:743485bb51e4 891 for(int k=0;k<33;k++){
Vigilance88 28:743485bb51e4 892 pc.putc(205);
Vigilance88 28:743485bb51e4 893 }
Vigilance88 28:743485bb51e4 894 pc.putc(188);
Vigilance88 28:743485bb51e4 895
Vigilance88 28:743485bb51e4 896 pc.printf("\n\r");
Vigilance88 28:743485bb51e4 897 //endbox
Vigilance88 28:743485bb51e4 898 }
Vigilance88 28:743485bb51e4 899
Vigilance88 28:743485bb51e4 900 void emgInstructions(){
Vigilance88 28:743485bb51e4 901 pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); wait(1);
Vigilance88 28:743485bb51e4 902 pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);
Vigilance88 28:743485bb51e4 903 pc.printf("\n\r Relax for a few minutes after electrodes are placed and check if EMG signal looks normal \n\r "); wait(1);
Vigilance88 28:743485bb51e4 904 pc.printf("\n\r To calibrate the EMG signals we will measure the Maximum Voluntary Contraction of each muscle \n\r"); wait(1);
Vigilance88 28:743485bb51e4 905 pc.printf("You will need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1);
Vigilance88 28:743485bb51e4 906 pc.printf("The measurement will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1);
Vigilance88 28:743485bb51e4 907 }
Vigilance88 28:743485bb51e4 908
Vigilance88 21:d6a46315d5f5 909 //keeps input limited between min max
Vigilance88 24:56db31267f10 910 void keep_in_range(double * in, double min, double max)
Vigilance88 18:44905b008f44 911 {
Vigilance88 18:44905b008f44 912 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 913 }