Janet Huisman / Mbed 2 deprecated Motor_Control_EMG

Motor_Control_EMG

Dependencies:   HIDScope MODSERIAL QEI Servo biquadFilter mbed

Fork of Motor_Control_buttons by Janet Huisman

main.cpp@20:17a0c462caf0, 2016-11-02 (annotated)

Committer:
huismaja
Date:
Wed Nov 02 08:34:55 2016 +0000
Revision:
20:17a0c462caf0
Parent:
19:2a74dd8dc4fa 
Motor control with EMG (4 emg signals), with comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
huismaja 13:746240466172 1 #include "mbed.h" //Include the mbed library
huismaja 13:746240466172 2 #include "MODSERIAL.h" //Include the MODSERIAL library for communication with the pc
huismaja 13:746240466172 3 #include "Servo.h" //Include the Servo library for controlling the gripper
huismaja 13:746240466172 4 #include "QEI.h" //Include the QEI library for reading the encoder data of the DC-motors
huismaja 13:746240466172 5 //#include "HIDScope.h" //Include the HIDScope library for plotting the emg data
huismaja 13:746240466172 6 #include "BiQuad.h" //Include the BiQuad library for filtering the emg signal
huismaja 13:746240466172 7
huismaja 19:2a74dd8dc4fa 8 MODSERIAL pc(USBTX, USBRX); //Make a connection with the PC
huismaja 19:2a74dd8dc4fa 9 //HIDScope scope(4); //Create a 4-channel HIDScope object
huismaja 13:746240466172 10
huismaja 13:746240466172 11 const double pi = 3.1415926535897; //Declare the value of pi
huismaja 13:746240466172 12
huismaja 13:746240466172 13 double speed_rotation=pi/5; //Set the rotation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
huismaja 13:746240466172 14 double speed_translation=pi/5; //Set the translation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
huismaja 13:746240466172 15 double speedM1=speed_rotation/8.4; //Map the rotation speed from (0-8.4) to (0-1) by dividing by 8.4
huismaja 13:746240466172 16 double speedM2=speed_translation/8.4; //Map the translation speed from (0-8.4) to (0-1) by dividing by 8.4
huismaja 0:6c8444d06e97 17
huismaja 10:cf579c3eaf01 18 QEI encoder_M1 (D9, D10, NC, 8400); //Define an encoder for motor 1 called encoder_M1
huismaja 10:cf579c3eaf01 19 QEI encoder_M2 (D11, D12, NC, 8400); //Define an encoder for motor 2 called encoder_M2
huismaja 9:cca4d4084775 20
huismaja 10:cf579c3eaf01 21 Ticker encoder_M1_ticker; //Create a ticker for reading the encoder data of encoder_M1
huismaja 10:cf579c3eaf01 22 Ticker encoder_M2_ticker; //Create a ticker for reading the encoder data of encoder_M2
huismaja 6:98121d2d76a6 23
huismaja 12:35a81d6c6505 24 DigitalOut Direction_M2(D4); //To control the rotation direction of the arm
huismaja 12:35a81d6c6505 25 PwmOut Speed_M2(D5); //To control the rotation speed of the arm
huismaja 12:35a81d6c6505 26 PwmOut Speed_M1(D6); //To control the translation direction of the arm
huismaja 12:35a81d6c6505 27 DigitalOut Direction_M1(D7); //To control the translation speed of the arm
huismaja 10:cf579c3eaf01 28 Servo gripper_servo(D13); //To control the gripper
huismaja 0:6c8444d06e97 29
huismaja 19:2a74dd8dc4fa 30 InterruptIn Switch_1(SW3); //Switch 1 to control the rotation to the left
huismaja 19:2a74dd8dc4fa 31 InterruptIn Switch_2(SW2); //Switch 2 to control the rotation to the right
huismaja 19:2a74dd8dc4fa 32 InterruptIn Switch_3(D2); //Switch 3 to control the translation of the arm
huismaja 19:2a74dd8dc4fa 33 InterruptIn Switch_4(D3); //Switch 4 to control the gripper
huismaja 13:746240466172 34
huismaja 20:17a0c462caf0 35 DigitalOut red(LED_RED); //LED for calibration_biceps (red)
huismaja 20:17a0c462caf0 36 DigitalOut blue(LED_BLUE); //LED for calibration_triceps (blue)
huismaja 19:2a74dd8dc4fa 37
huismaja 19:2a74dd8dc4fa 38 AnalogIn emg_1(A0); //Analog of EMG 1
huismaja 19:2a74dd8dc4fa 39 AnalogIn emg_2(A1); //Analog of EMG 2
huismaja 19:2a74dd8dc4fa 40 AnalogIn emg_3(A2); //Analog of EMG 3
huismaja 20:17a0c462caf0 41 //AnalogIn emg_4(A3); //Analog of EMG 4
huismaja 18:f28d7ba60eb4 42
huismaja 19:2a74dd8dc4fa 43 double emg_1_value = 0; //Initially the emg_1 value is zero
huismaja 19:2a74dd8dc4fa 44 double emg_2_value = 0; //Initially the emg_2 value is zero
huismaja 19:2a74dd8dc4fa 45 double emg_3_value = 0; //Initially the emg_3 value is zero
huismaja 19:2a74dd8dc4fa 46 double emg_4_value = 0; //Initially the emg_4 value is zero
huismaja 13:746240466172 47
huismaja 19:2a74dd8dc4fa 48 double signalpart1=0; //Initially signalpart1 is zero
huismaja 19:2a74dd8dc4fa 49 double signalpart2=0; //Initially signalpart2 is zero
huismaja 19:2a74dd8dc4fa 50 double signalpart3=0; //Initially signalpart3 is zero
huismaja 19:2a74dd8dc4fa 51 double signalpart4=0; //Initially signalpart4 is zero
huismaja 19:2a74dd8dc4fa 52 double emg_1_filtered = 0; //Initially the emg_1_filtered signal is zero
huismaja 19:2a74dd8dc4fa 53 double emg_2_filtered = 0; //Initially the emg_2_filtered signal is zero
huismaja 19:2a74dd8dc4fa 54 double emg_3_filtered = 0; //Initially the emg_3_filtered signal is zero
huismaja 19:2a74dd8dc4fa 55 double emg_4_filtered = 0; //Initially the emg_4_filtered signal is zero
huismaja 19:2a74dd8dc4fa 56 double maximum_calibration_value_1=0; //Initially the maximum calibration value for emg 1 is zero
huismaja 19:2a74dd8dc4fa 57 double maximum_calibration_value_2=0; //Initially the maximum calibration value for emg 2 is zero
huismaja 19:2a74dd8dc4fa 58 double maximum_calibration_value_3=0; //Initially the maximum calibration value for emg 3 is zero
huismaja 19:2a74dd8dc4fa 59 double maximum_calibration_value_4=0; //Initially the maximum calibration value for emg 4 is zero
huismaja 19:2a74dd8dc4fa 60 bool calibration_biceps_done=0; //Initially the biceps calibration is not done
huismaja 19:2a74dd8dc4fa 61 bool calibration_triceps_done=0; //Initially the tricpes calibration is not done
huismaja 13:746240466172 62
huismaja 19:2a74dd8dc4fa 63 volatile double emg_1_threshold = 0.2; //Set the threshold for emg 1
huismaja 19:2a74dd8dc4fa 64 volatile double emg_2_threshold = 0.2; //Set the threshold for emg 2
huismaja 19:2a74dd8dc4fa 65 volatile double emg_3_threshold = 0.2; //Set the threshold for emg 3
huismaja 19:2a74dd8dc4fa 66 volatile double emg_4_threshold = 0.2; //Set the threshold for emg 4
huismaja 14:63f2a5165ffd 67
huismaja 19:2a74dd8dc4fa 68 Ticker filter_EMG_ticker; //Create a ticker for the filtering of all emg signals
huismaja 19:2a74dd8dc4fa 69 Ticker calibration_biceps_ticker; //Create a ticker for the biceps calibration
huismaja 19:2a74dd8dc4fa 70 Ticker calibration_triceps_ticker; //Create a ticker for the triceps calibration
huismaja 19:2a74dd8dc4fa 71 Ticker check_threshold_crossing_ticker; //Create a ticker for checking if the threshold is crossed
huismaja 19:2a74dd8dc4fa 72 Ticker check_goflags_ticker; //Create a ticker for checking if the go-flags are set true
huismaja 19:2a74dd8dc4fa 73
huismaja 19:2a74dd8dc4fa 74 //Create Biquad filters for the filtering of emg_1
huismaja 19:2a74dd8dc4fa 75 BiQuad highpass1(0.9565, -1.9131, 0.9565, -1.9112, 0.9150); //Create a highpass filter to remove low-frequent noise
huismaja 20:17a0c462caf0 76 BiQuad notch1(0.9938, -1.8902, 0.9938, -1.8902, 0.9875); //Create a notch-filter to remove 50Hz noise
huismaja 19:2a74dd8dc4fa 77 BiQuad lowpass1(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824); //Create a lowpass filter to envelope the signal
huismaja 13:746240466172 78
huismaja 19:2a74dd8dc4fa 79 //Create Biquad filters for the filtering of emg_2
huismaja 19:2a74dd8dc4fa 80 BiQuad highpass2(0.9565, -1.9131, 0.9565, -1.9112, 0.9150); //Create a highpass filter to remove low-frequent noise
huismaja 20:17a0c462caf0 81 BiQuad notch2(0.9938, -1.8902, 0.9938, -1.8902, 0.9875); //Create a notch-filter to remove 50Hz noise
huismaja 19:2a74dd8dc4fa 82 BiQuad lowpass2(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824); //Create a lowpass filter to envelope the signal
huismaja 18:f28d7ba60eb4 83
huismaja 19:2a74dd8dc4fa 84 //Create Biquad filters for the filtering of emg_3
huismaja 19:2a74dd8dc4fa 85 BiQuad highpass3(0.9565, -1.9131, 0.9565, -1.9112, 0.9150); //Create a highpass filter to remove low-frequent noise
huismaja 20:17a0c462caf0 86 BiQuad notch3(0.9938, -1.8902, 0.9938, -1.8902, 0.9875); //Create a notch-filter to remove 50Hz noise
huismaja 19:2a74dd8dc4fa 87 BiQuad lowpass3(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824); //Create a lowpass filter to envelope the signal
huismaja 14:63f2a5165ffd 88
huismaja 20:17a0c462caf0 89 ////Create Biquad filters for the filtering of emg_4
huismaja 19:2a74dd8dc4fa 90 //BiQuad highpass4(0.9565, -1.9131, 0.9565, -1.9112, 0.9150); //Create a highpass filter to remove low-frequent noise
huismaja 20:17a0c462caf0 91 //BiQuad notch4(0.9938, -1.8902, 0.9938, -1.8902, 0.9875); //Create a notch-filter to remove 50Hz noise
huismaja 19:2a74dd8dc4fa 92 //BiQuad lowpass4(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824); //Create a lowpass filter to envelope the signal
huismaja 2:b20570f160c6 93
huismaja 5:9b5edadc023b 94 int counter_rotation_left=0; //To count the number of times the rotation_left switch (switch_1) has been pushed
huismaja 5:9b5edadc023b 95 int counter_rotation_right=0; //To count the number of times the rotation_right switch (switch_2) has been pushed
huismaja 5:9b5edadc023b 96 int counter_translation=0; //To count the number of times the translation switch (switch_3) has been pushed
huismaja 5:9b5edadc023b 97 int counter_gripper=0; //To count the number of times the gripper switch (switch_4) has been pushed
huismaja 5:9b5edadc023b 98
huismaja 17:4cfa7951bfa2 99 bool emg_1_activated = 0; //Initially the emg_1 has not crossed the threshold
huismaja 17:4cfa7951bfa2 100 bool emg_2_activated = 0; //Initially the emg_2 has not crossed the threshold
huismaja 17:4cfa7951bfa2 101 bool emg_3_activated = 0; //Initially the emg_3 has not crossed the threshold
huismaja 17:4cfa7951bfa2 102 bool emg_4_activated = 0; //Initially the emg_4 has not crossed the threshold
huismaja 13:746240466172 103
huismaja 17:4cfa7951bfa2 104 volatile bool rotation_left_go = 0; //Create a go-flag for the rotation_left and set it to false
huismaja 17:4cfa7951bfa2 105 volatile bool rotation_right_go = 0; //Create a go-flag for the rotation_right and set it to false
huismaja 17:4cfa7951bfa2 106 volatile bool translation_go = 0; //Create a go-flag for the translation and set it to false
huismaja 17:4cfa7951bfa2 107 volatile bool gripper_go = 0; //Create a go-flag for the gripper and set it to false
huismaja 0:6c8444d06e97 108
huismaja 13:746240466172 109 float angle_M1=0; //The measured angle of motor 1 is initially zero
huismaja 13:746240466172 110 float angle_M2=0; //The measured angle of motor 2 is initially zero
huismaja 9:cca4d4084775 111
huismaja 19:2a74dd8dc4fa 112 void read_position_M1 (){ //Function to read the position of motor 1
huismaja 19:2a74dd8dc4fa 113 int pulses_M1 = -encoder_M1.getPulses(); //Read the encoder data and store it in pulses_M1
huismaja 19:2a74dd8dc4fa 114 angle_M1 = float(pulses_M1)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
huismaja 13:746240466172 115 // pc.printf("%i \t%f \t", pulses_M1, angle_M1);
huismaja 13:746240466172 116 }
huismaja 13:746240466172 117
huismaja 19:2a74dd8dc4fa 118 void read_position_M2 (){ //Function to read the position of motor 2
huismaja 19:2a74dd8dc4fa 119 int pulses_M2 = -encoder_M2.getPulses(); //Read the encoder data and store it in pulses_M2
huismaja 19:2a74dd8dc4fa 120 angle_M2 = float(pulses_M2)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
huismaja 13:746240466172 121 // pc.printf("%i \t%f \n", pulses_M2, angle_M2);
huismaja 9:cca4d4084775 122 }
huismaja 9:cca4d4084775 123
huismaja 19:2a74dd8dc4fa 124 void activate_rotation_left (){ //To activate the rotation_left
huismaja 19:2a74dd8dc4fa 125 counter_rotation_left++; //Increase the counter_rotation_left that counts the number of time switch 1 has been pressed
huismaja 19:2a74dd8dc4fa 126 if (counter_rotation_left > 2){ //Because there are only 2 cases in the switch statement, case 3 = case 1 etc.
huismaja 11:b1ad5267a6bd 127 counter_rotation_left=1;
huismaja 11:b1ad5267a6bd 128 }
huismaja 19:2a74dd8dc4fa 129 rotation_left_go = 1; //After increasing the counter, set the rotation_left go-flag to true
huismaja 11:b1ad5267a6bd 130 }
huismaja 11:b1ad5267a6bd 131
huismaja 19:2a74dd8dc4fa 132 void rotation_left (){ //Function to control the rotation to the left
huismaja 19:2a74dd8dc4fa 133 switch (counter_rotation_left){ //Create a switch statement
huismaja 19:2a74dd8dc4fa 134 case 1: //For activating the rotation to the left
huismaja 19:2a74dd8dc4fa 135 Direction_M1 = 1; //The arm will rotate to the left
huismaja 19:2a74dd8dc4fa 136 Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
huismaja 8:9c58ca13076e 137 pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1);
huismaja 12:35a81d6c6505 138 wait(0.1f);
huismaja 3:0a4bfcb3f339 139 break;
huismaja 19:2a74dd8dc4fa 140 case 2: //For stopping the rotation to the left
huismaja 19:2a74dd8dc4fa 141 Direction_M1 = 1; //The arm will rotate to the left
huismaja 19:2a74dd8dc4fa 142 Speed_M1 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 143 pc.printf("The arm will now stop rotating to the left \n");
huismaja 12:35a81d6c6505 144 wait(0.1f);
huismaja 3:0a4bfcb3f339 145 break;
huismaja 3:0a4bfcb3f339 146 }
huismaja 11:b1ad5267a6bd 147 }
huismaja 3:0a4bfcb3f339 148
huismaja 19:2a74dd8dc4fa 149 void activate_rotation_right (){ //To activate the rotation_right
huismaja 19:2a74dd8dc4fa 150 counter_rotation_right++; //Increase the counter_rotation_right that counts the number of time switch 2 has been pressed
huismaja 19:2a74dd8dc4fa 151 if (counter_rotation_right> 2){ //Because there are only 2 cases in the switch statement, case 3 = case 1
huismaja 11:b1ad5267a6bd 152 counter_rotation_right=1;
huismaja 3:0a4bfcb3f339 153 }
huismaja 19:2a74dd8dc4fa 154 rotation_right_go = 1; //After increasing the counter, set the rotation_right go-flag to true
huismaja 3:0a4bfcb3f339 155 }
huismaja 3:0a4bfcb3f339 156
huismaja 19:2a74dd8dc4fa 157 void rotation_right (){ //Function to control the rotation to the left
huismaja 19:2a74dd8dc4fa 158 switch (counter_rotation_right){ //Create a switch statement
huismaja 19:2a74dd8dc4fa 159 case 1: //For activation the rotation to the right
huismaja 19:2a74dd8dc4fa 160 Direction_M1 = 0; //The arm will rotate to the right
huismaja 19:2a74dd8dc4fa 161 Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
huismaja 8:9c58ca13076e 162 pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1);
huismaja 12:35a81d6c6505 163 wait(0.1f);
huismaja 3:0a4bfcb3f339 164 break;
huismaja 19:2a74dd8dc4fa 165 case 2: //For stopping the rotation to the right
huismaja 19:2a74dd8dc4fa 166 Direction_M1 = 0; //The arm will rotate to the right
huismaja 19:2a74dd8dc4fa 167 Speed_M1 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 168 pc.printf("The arm will now stop rotating to the right \n");
huismaja 12:35a81d6c6505 169 wait(0.1f);
huismaja 3:0a4bfcb3f339 170 break;
huismaja 3:0a4bfcb3f339 171 }
huismaja 3:0a4bfcb3f339 172 }
huismaja 3:0a4bfcb3f339 173
huismaja 19:2a74dd8dc4fa 174 void activate_translation (){ //To activate the translation
huismaja 19:2a74dd8dc4fa 175 counter_translation++; //Increase the counter_translation that counts the number of time switch 3 has been pressed
huismaja 19:2a74dd8dc4fa 176 if (counter_translation > 4){ //Because there are 4 cases in the switch statement, case 5 = case 1
huismaja 11:b1ad5267a6bd 177 counter_translation=1;
huismaja 3:0a4bfcb3f339 178 }
huismaja 19:2a74dd8dc4fa 179 translation_go = 1; //After increasing the counter, set the translation go-flag to true
huismaja 3:0a4bfcb3f339 180 }
huismaja 3:0a4bfcb3f339 181
huismaja 10:cf579c3eaf01 182 void translation (){ //Function to control the translation
huismaja 10:cf579c3eaf01 183 switch (counter_translation){ //Create a switch statement
huismaja 8:9c58ca13076e 184 case 1: //For activating the elongation of the arm
huismaja 8:9c58ca13076e 185 Direction_M2 = 1; //The arm will get longer
huismaja 10:cf579c3eaf01 186 Speed_M2 = speedM2; //The motor is turned on at speed_translation rad/sec
huismaja 5:9b5edadc023b 187 pc.printf("The arm will now get longer \n");
huismaja 12:35a81d6c6505 188 wait(0.1f);
huismaja 5:9b5edadc023b 189 break;
huismaja 8:9c58ca13076e 190 case 2: //For stopping the elongation of the arm
huismaja 8:9c58ca13076e 191 Direction_M2 = 1; //The arm will get longer
huismaja 8:9c58ca13076e 192 Speed_M2 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 193 pc.printf("The arm will now stop getting longer \n");
huismaja 12:35a81d6c6505 194 wait(0.1f);
huismaja 5:9b5edadc023b 195 break;
huismaja 8:9c58ca13076e 196 case 3: //For activating the shortening of the arm
huismaja 8:9c58ca13076e 197 Direction_M2 = 0; //The arm will get shorter
huismaja 10:cf579c3eaf01 198 Speed_M2 = speedM2; //The motor is turned on at speed_translation rad/sec
huismaja 5:9b5edadc023b 199 pc.printf("The arm will now get shorter \n");
huismaja 12:35a81d6c6505 200 wait(0.1f);
huismaja 5:9b5edadc023b 201 break;
huismaja 8:9c58ca13076e 202 case 4: //For stopping the shortening of the arm
huismaja 8:9c58ca13076e 203 Direction_M2 = 0; //The arm will get shorter
huismaja 8:9c58ca13076e 204 Speed_M2 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 205 pc.printf("The arm will now stop getting shorter \n");
huismaja 12:35a81d6c6505 206 wait(0.1f);
huismaja 5:9b5edadc023b 207 break;
huismaja 5:9b5edadc023b 208 }
huismaja 11:b1ad5267a6bd 209 }
huismaja 5:9b5edadc023b 210
huismaja 19:2a74dd8dc4fa 211 void activate_gripper (){ //To activate the gripper
huismaja 19:2a74dd8dc4fa 212 counter_gripper++; //Increase the couter_gripper that counts the number of time switch 4 has been pressed
huismaja 19:2a74dd8dc4fa 213 if (counter_gripper> 2){ //Because there are only 2 cases in the switch statement, case 3 = case 1
huismaja 11:b1ad5267a6bd 214 counter_gripper=1;
huismaja 5:9b5edadc023b 215 }
huismaja 19:2a74dd8dc4fa 216 gripper_go = 1; //After increasing the counter, set the gripper go-flag to true
huismaja 5:9b5edadc023b 217 }
huismaja 5:9b5edadc023b 218
huismaja 19:2a74dd8dc4fa 219 void gripper (){ //Function to control the gripper
huismaja 19:2a74dd8dc4fa 220 switch (counter_gripper){ //Create a switch statement
huismaja 19:2a74dd8dc4fa 221 case 1: //For closing the gripper
huismaja 19:2a74dd8dc4fa 222 gripper_servo = 0; //The gripper is now closed
huismaja 5:9b5edadc023b 223 pc.printf("The gripper will now close \n");
huismaja 12:35a81d6c6505 224 wait(0.1f);
huismaja 4:84bd5ead83f9 225 break;
huismaja 19:2a74dd8dc4fa 226 case 2: //For opening the gripper
huismaja 19:2a74dd8dc4fa 227 gripper_servo = 0.3; //The gripper is now open
huismaja 5:9b5edadc023b 228 pc.printf("The gripper will now open \n");
huismaja 12:35a81d6c6505 229 wait(0.1f);
huismaja 4:84bd5ead83f9 230 break;
huismaja 4:84bd5ead83f9 231 }
huismaja 4:84bd5ead83f9 232 }
huismaja 17:4cfa7951bfa2 233
huismaja 19:2a74dd8dc4fa 234 void calibration_biceps(){ //Function to calibrate the biceps
huismaja 19:2a74dd8dc4fa 235 if(Switch_1.read()== 0) { //If Switch_1 is pressed
huismaja 19:2a74dd8dc4fa 236 for(int n=0; n<3000; n++){ //For 3000 samples
huismaja 19:2a74dd8dc4fa 237 signalpart1 = highpass1.step(emg_1.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 238 signalpart2 = notch1.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 239 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 240 emg_1_filtered = lowpass1.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 241 if (emg_1_filtered > maximum_calibration_value_1) { //If the measured value is higher than the maximum value
huismaja 19:2a74dd8dc4fa 242 maximum_calibration_value_1 = emg_1_filtered; //The measured value is the new maximum value
huismaja 14:63f2a5165ffd 243 }
huismaja 19:2a74dd8dc4fa 244 emg_1_threshold = maximum_calibration_value_1*0.7; //Set the threshold for emg 1
huismaja 15:c43f0dfe7cdf 245
huismaja 19:2a74dd8dc4fa 246 signalpart1 = highpass2.step(emg_2.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 247 signalpart2 = notch2.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 248 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 249 emg_2_filtered = lowpass2.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 250 if (emg_2_filtered > maximum_calibration_value_2) { //If the measured value is higher than the maximum value
huismaja 19:2a74dd8dc4fa 251 maximum_calibration_value_2 = emg_2_filtered; //The measured value is the new maximum value
huismaja 18:f28d7ba60eb4 252 }
huismaja 19:2a74dd8dc4fa 253 emg_2_threshold = maximum_calibration_value_2*0.7; //Set the threshold for emg 2
huismaja 14:63f2a5165ffd 254 }
huismaja 19:2a74dd8dc4fa 255
huismaja 19:2a74dd8dc4fa 256 red = 0; //Switch the red light on to indicate the biceps calibration is done
huismaja 19:2a74dd8dc4fa 257 calibration_biceps_done=1; //Declare that the biceps calibration is done
huismaja 17:4cfa7951bfa2 258 }
huismaja 14:63f2a5165ffd 259 }
huismaja 14:63f2a5165ffd 260
huismaja 19:2a74dd8dc4fa 261 void calibration_triceps(){ //Function to calibrate the triceps
huismaja 19:2a74dd8dc4fa 262 if(Switch_2.read()== 0) { //If Switch_2 is pressed
huismaja 19:2a74dd8dc4fa 263 for(int n=0; n<3000; n++){ //For 3000 samples
huismaja 19:2a74dd8dc4fa 264 signalpart1 = highpass3.step(emg_3.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 265 signalpart2 = notch3.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 266 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 267 emg_3_filtered = lowpass3.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 268 if (emg_3_filtered > maximum_calibration_value_3) { //If the measured value is higher than the maximum value
huismaja 19:2a74dd8dc4fa 269 maximum_calibration_value_3 = emg_3_filtered; //The measured value is the new maximum value
huismaja 18:f28d7ba60eb4 270 }
huismaja 19:2a74dd8dc4fa 271 emg_3_threshold = maximum_calibration_value_3*0.4; //Set the threshold for emg 3
huismaja 18:f28d7ba60eb4 272
huismaja 19:2a74dd8dc4fa 273 // signalpart1 = highpass4.step(emg_4.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 274 // signalpart2 = notch4.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 275 // signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 276 // emg_4_filtered = lowpass4.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 277 // if (emg_4_filtered > maximum_calibration_value_4) { //If the measured value is higher than the maximum value
huismaja 19:2a74dd8dc4fa 278 // maximum_calibration_value_4 = emg_4_filtered; //The measured value is the new maximum value
huismaja 16:196abf318ea4 279 // }
huismaja 19:2a74dd8dc4fa 280 // emg_4_threshold = maximum_calibration_value_4*0.4; //Set the threshold for emg 4
huismaja 20:17a0c462caf0 281 }
huismaja 19:2a74dd8dc4fa 282
huismaja 19:2a74dd8dc4fa 283 blue = 0; //Switch the blue light on to indicate the triceps calibration is done
huismaja 19:2a74dd8dc4fa 284 calibration_triceps_done=1; //Declare that the triceps calibration is done
huismaja 19:2a74dd8dc4fa 285 }
huismaja 19:2a74dd8dc4fa 286 }
huismaja 19:2a74dd8dc4fa 287
huismaja 19:2a74dd8dc4fa 288 void filter_emg(){ //Function to filter the emg signals
huismaja 19:2a74dd8dc4fa 289 if(calibration_biceps_done==1 && calibration_triceps_done==1) { //If both calibrations are done
huismaja 19:2a74dd8dc4fa 290 signalpart1 = highpass1.step(emg_1.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 291 signalpart2 = notch1.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 292 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 293 emg_1_filtered = lowpass1.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 294 // pc.printf("%f \n", emg_1_filtered);
huismaja 19:2a74dd8dc4fa 295
huismaja 19:2a74dd8dc4fa 296 signalpart1 = highpass2.step(emg_2.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 297 signalpart2 = notch2.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 298 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 299 emg_2_filtered = lowpass2.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 300 // pc.printf("%f \n", emg_2_filtered);
huismaja 19:2a74dd8dc4fa 301
huismaja 19:2a74dd8dc4fa 302 signalpart1 = highpass3.step(emg_1.read()); //Filter the signal with the highpass filter
huismaja 20:17a0c462caf0 303 signalpart2 = notch3.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 304 signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 305 emg_3_filtered = lowpass3.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 306 // pc.printf("%f \n", emg_3_filtered);
huismaja 19:2a74dd8dc4fa 307
huismaja 19:2a74dd8dc4fa 308 // signalpart1 = highpass4.step(emg_1.read()); //Filter the signal with the highpass filter
huismaja 19:2a74dd8dc4fa 309 // signalpart2 = notch_low4.step(signalpart1); //Filter the signal with the notch filter
huismaja 20:17a0c462caf0 310 // signalpart3 = fabs(signalpart2); //Rectify the signal
huismaja 20:17a0c462caf0 311 // emg_4_filtered = lowpass4.step(signalpart3); //Filter the signal with the lowpass filter
huismaja 19:2a74dd8dc4fa 312 //// pc.printf("%f \n", emg_4_filtered);
huismaja 19:2a74dd8dc4fa 313
huismaja 19:2a74dd8dc4fa 314 // scope.set(0,emg_1_filtered); //Plot the emg_1_filtered in the first scope of HIDScope
huismaja 19:2a74dd8dc4fa 315 // scope.set(1,emg_2_filtered); //Plot the emg_2_filtered in the second scope of HIDScope
huismaja 19:2a74dd8dc4fa 316 // scope.set(2,emg_3_filtered); //Plot the emg_3_filtered in the third scope of HIDScope
huismaja 19:2a74dd8dc4fa 317 // scope.set(3,emg_4_filtered); //Plot the emg_4_filtered in the fourth scope of HIDScope
huismaja 19:2a74dd8dc4fa 318 // scope.send(); //Send all the data to HIDScope
huismaja 18:f28d7ba60eb4 319 }
huismaja 18:f28d7ba60eb4 320 }
huismaja 16:196abf318ea4 321
huismaja 19:2a74dd8dc4fa 322 void check_threshold_crossing (){ //Function to check if the emg thresholds are crossed
huismaja 20:17a0c462caf0 323 if(calibration_biceps_done==1 && calibration_triceps_done==1) { //If both calibrations are done
huismaja 19:2a74dd8dc4fa 324 if(emg_1_filtered >= emg_1_threshold && emg_1_activated == 0) { //If the filtered emg 1 signal is above the threshold value and if emg_1 is not activated yet
huismaja 19:2a74dd8dc4fa 325 emg_1_activated = 1; //Declare that emg_1 is activated
huismaja 19:2a74dd8dc4fa 326 activate_rotation_left(); //Execute the activate_rotation_left function
huismaja 19:2a74dd8dc4fa 327 wait(0.1f);
huismaja 19:2a74dd8dc4fa 328 } else if (emg_1_filtered <= emg_1_threshold) { //If the filtered emg 1 signal is below the threshold value
huismaja 19:2a74dd8dc4fa 329 emg_1_activated = 0; //Declare that emg_1 is deactivated
huismaja 19:2a74dd8dc4fa 330 }
huismaja 19:2a74dd8dc4fa 331 if(emg_2_filtered >= emg_2_threshold && emg_2_activated == 0) { //If the filtered emg 2 signal is above the threshold value and if emg_2 is not activated yet
huismaja 19:2a74dd8dc4fa 332 emg_2_activated = 1; //Declare that emg_2 is activated
huismaja 19:2a74dd8dc4fa 333 activate_rotation_right(); //Execute the activate_rotation_right function
huismaja 16:196abf318ea4 334 wait(0.1f);
huismaja 19:2a74dd8dc4fa 335 } else if (emg_2_filtered <= emg_2_threshold) { //If the filtered emg 2 signal is below the threshold value
huismaja 19:2a74dd8dc4fa 336 emg_2_activated = 0; //Declare that emg_2 is deactivated
huismaja 18:f28d7ba60eb4 337 }
huismaja 19:2a74dd8dc4fa 338 if(emg_3_filtered >= emg_3_threshold && emg_3_activated == 0) { //If the filtered emg 3 signal is above the threshold value and if emg_3 is not activated yet
huismaja 19:2a74dd8dc4fa 339 emg_3_activated = 1; //Declare that emg_3 is activated
huismaja 19:2a74dd8dc4fa 340 activate_translation(); //Execute the activate_translation function
huismaja 19:2a74dd8dc4fa 341 wait(0.1f);
huismaja 19:2a74dd8dc4fa 342 } else if (emg_3_filtered <= emg_3_threshold) { //If the filtered emg 3 signal is below the threshold value
huismaja 19:2a74dd8dc4fa 343 emg_3_activated = 0; //Declare that emg_3 is deactivated
huismaja 18:f28d7ba60eb4 344 }
huismaja 19:2a74dd8dc4fa 345 // if(emg_4_filtered >= emg_4_threshold && emg_4_activated == 0) { //If the filtered emg 4 signal is above the threshold value and if emg_4 is not activated yet
huismaja 19:2a74dd8dc4fa 346 // emg_4_activated = 1; //Declare that emg_4 is activated
huismaja 19:2a74dd8dc4fa 347 // activate_gripper(); //Execute the activate_gripper function
huismaja 19:2a74dd8dc4fa 348 // wait(0.1f);
huismaja 19:2a74dd8dc4fa 349 // } else if (emg_4_filtered <= emg_4_threshold) { //If the filtered emg 4 signal is below the threshold value
huismaja 19:2a74dd8dc4fa 350 // emg_4_activated = 0; //Declare that emg_4 is deactivated
huismaja 16:196abf318ea4 351 // }
huismaja 15:c43f0dfe7cdf 352 }
huismaja 13:746240466172 353 }
huismaja 13:746240466172 354
huismaja 19:2a74dd8dc4fa 355 void check_goflags (){ //Function to check if the go-flags are activated
huismaja 19:2a74dd8dc4fa 356 if (rotation_left_go == 1) { //If the rotation_left go-flag is true
huismaja 17:4cfa7951bfa2 357 rotation_left_go = 0; //Set the rotation_left go-flag to false
huismaja 19:2a74dd8dc4fa 358 rotation_left(); //Execute the rotation_left function
huismaja 13:746240466172 359 }
huismaja 19:2a74dd8dc4fa 360 if (rotation_right_go == 1) { //If the rotation_right go-flag is true
huismaja 17:4cfa7951bfa2 361 rotation_right_go = 0; //Set the rotation_right go-flag to false
huismaja 19:2a74dd8dc4fa 362 rotation_right(); //Execute the rotation_right function
huismaja 13:746240466172 363 }
huismaja 19:2a74dd8dc4fa 364 if (translation_go == 1) { //If the translation go-flag is true
huismaja 17:4cfa7951bfa2 365 translation_go = 0; //Set the translation go-flag to false
huismaja 19:2a74dd8dc4fa 366 translation(); //Execute the translation function
huismaja 13:746240466172 367 }
huismaja 19:2a74dd8dc4fa 368 if (gripper_go == 1) { //If the gripper go-flag is true
huismaja 17:4cfa7951bfa2 369 gripper_go = 0; //Set the gripper go-flag to false
huismaja 19:2a74dd8dc4fa 370 gripper(); //Execute the gripper function
huismaja 13:746240466172 371 }
huismaja 13:746240466172 372 }
huismaja 13:746240466172 373
huismaja 13:746240466172 374 int main (){
huismaja 10:cf579c3eaf01 375 pc.baud(115200); //Set the boud rate for serial communication
huismaja 10:cf579c3eaf01 376 pc.printf("RESET \n"); //Print "RESET"
huismaja 1:0d55a4bf2269 377
huismaja 19:2a74dd8dc4fa 378 red=1; //The red LED will initially be switched off
huismaja 19:2a74dd8dc4fa 379 blue=1; //The blue LED will initially be switched off
huismaja 5:9b5edadc023b 380 Direction_M1 = 1; //The arm will initially get longer
huismaja 5:9b5edadc023b 381 Speed_M1 = 0; //The first motor is initially turned off
huismaja 13:746240466172 382 Direction_M2 = 1; //The arm will initially turn left
huismaja 6:98121d2d76a6 383 Speed_M2 = 0; //The second motor is initially turned off
huismaja 13:746240466172 384 gripper_servo = 0.3; //The gripper is initially open
huismaja 10:cf579c3eaf01 385 encoder_M1.reset(); //Reset the encoder for motor 1
huismaja 10:cf579c3eaf01 386 encoder_M2.reset(); //Reset the encoder for motor 2
huismaja 1:0d55a4bf2269 387
huismaja 10:cf579c3eaf01 388 encoder_M1_ticker.attach(&read_position_M1,0.01); //Connect the encoder_M1_ticker to the read_position_M1 function and execute at 100Hz
huismaja 10:cf579c3eaf01 389 encoder_M2_ticker.attach(&read_position_M2,0.01); //Connect the encoder_M2_ticker to the read_position_M2 function and execute at 100Hz
huismaja 10:cf579c3eaf01 390
huismaja 13:746240466172 391 // Switch_1.rise(&activate_rotation_left); //Use switch_1 to activate the counter_rotation_left go-flag
huismaja 15:c43f0dfe7cdf 392 // Switch_2.rise(&activate_rotation_right); //Use switch_2 to activate the counter_rotation_right go-flag
huismaja 17:4cfa7951bfa2 393 // Switch_3.rise(&activate_translation); //Use switch_3 to activate the counter_translation go-flag
huismaja 17:4cfa7951bfa2 394 // Switch_4.rise(&activate_gripper); //Use switch_4 to activate the counter_gripper go-flag
huismaja 5:9b5edadc023b 395
huismaja 19:2a74dd8dc4fa 396 filter_EMG_ticker.attach(&filter_emg, 0.001); //Connect the filter_EMG_ticker to the filter_EMG funtion and execute at 1000Hz
huismaja 19:2a74dd8dc4fa 397 calibration_biceps_ticker.attach(&calibration_biceps, 0.001); //Connect the calibration_biceps_ticker to the calibration_biceps function and execute at 1000Hz
huismaja 19:2a74dd8dc4fa 398 calibration_triceps_ticker.attach(&calibration_triceps, 0.001); //Connect the calibration_triceps_ticker to the calibration_triceps function and execute at 1000Hz
huismaja 19:2a74dd8dc4fa 399 check_threshold_crossing_ticker.attach(&check_threshold_crossing, 0.01); //Connect the check_threshold_crossing_ticker to the check_threshold_crossing function and execute at 100Hz
huismaja 19:2a74dd8dc4fa 400 check_goflags_ticker.attach(&check_goflags, 0.01); //Connect the check_goflags_ticker to the check_goflags and execute at 100Hz
huismaja 13:746240466172 401
huismaja 17:4cfa7951bfa2 402 while (1){} //Create a while loop to let the main loop run indefinitly
huismaja 0:6c8444d06e97 403 }