Daniqe Kottelenberg / Mbed 2 deprecated another_try_from_scratch_on_emg

emg with text

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of emg_import by Daniqe Kottelenberg

main.cpp@43:7d0b2dc05b80, 2016-11-01 (annotated)

Committer:
daniQQue
Date:
Tue Nov 01 14:38:11 2016 +0000
Revision:
43:7d0b2dc05b80
Parent:
42:7164ccd2aa14 
niet werkend..

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 0:34c739fcc3e0 1 //libraries
daniQQue 43:7d0b2dc05b80 2 #include "mbed.h" //mbed library
daniQQue 43:7d0b2dc05b80 3 #include "HIDScope.h" //hidscope library
daniQQue 43:7d0b2dc05b80 4 #include "QEI.h" //library for encoder functions
daniQQue 43:7d0b2dc05b80 5 #include "BiQuad.h" //library for filtering with BiQuad
daniQQue 43:7d0b2dc05b80 6 #include "MODSERIAL.h" //library for connect pc with mbed
daniQQue 0:34c739fcc3e0 7
daniQQue 0:34c739fcc3e0 8 //Define objects
daniQQue 43:7d0b2dc05b80 9
daniQQue 43:7d0b2dc05b80 10 //EMG
daniQQue 43:7d0b2dc05b80 11 AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++
daniQQue 43:7d0b2dc05b80 12 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 43:7d0b2dc05b80 13 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 42:7164ccd2aa14 14
daniQQue 43:7d0b2dc05b80 15 //Encoder
daniQQue 43:7d0b2dc05b80 16 DigitalIn encoder1A(D13);
daniQQue 43:7d0b2dc05b80 17 DigitalIn encoder1B(D12);
daniQQue 43:7d0b2dc05b80 18 DigitalIn encoder2A(D11);
daniQQue 43:7d0b2dc05b80 19 DigitalIn encoder2B(D10);
daniQQue 43:7d0b2dc05b80 20
daniQQue 43:7d0b2dc05b80 21 //callibration button
daniQQue 43:7d0b2dc05b80 22 InterruptIn button_calibration_biceps (SW3); //button to start calibration biceps
daniQQue 43:7d0b2dc05b80 23 InterruptIn button_calibration_triceps (SW2); // button to start calibration tricps
daniQQue 42:7164ccd2aa14 24
daniQQue 43:7d0b2dc05b80 25 // reset button
daniQQue 43:7d0b2dc05b80 26 DigitalIn resetbutton(D9);
daniQQue 43:7d0b2dc05b80 27
daniQQue 43:7d0b2dc05b80 28 //tickers
daniQQue 43:7d0b2dc05b80 29
daniQQue 43:7d0b2dc05b80 30 Ticker sample_timer; //ticker for emg sampling
daniQQue 43:7d0b2dc05b80 31 Ticker switch_function; //ticker for switch
daniQQue 43:7d0b2dc05b80 32 Ticker speed_measuring; //ticker for speed measurment
daniQQue 43:7d0b2dc05b80 33
daniQQue 43:7d0b2dc05b80 34 //everything for monitoring
daniQQue 43:7d0b2dc05b80 35 HIDScope scope(5); //open 3 channels in hidscope
daniQQue 43:7d0b2dc05b80 36 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 43:7d0b2dc05b80 37 DigitalOut red(LED_RED);
daniQQue 43:7d0b2dc05b80 38 DigitalOut green(LED_GREEN);
daniQQue 43:7d0b2dc05b80 39 DigitalOut blue(LED_BLUE);
daniQQue 43:7d0b2dc05b80 40
daniQQue 43:7d0b2dc05b80 41 //motors
daniQQue 43:7d0b2dc05b80 42 DigitalOut direction_motor1(D4);
daniQQue 43:7d0b2dc05b80 43 PwmOut pwm_motor1(D5);
daniQQue 43:7d0b2dc05b80 44 DigitalOut direction_motor2(D7);
daniQQue 43:7d0b2dc05b80 45 PwmOut pwm_motor2(D6);
daniQQue 0:34c739fcc3e0 46
daniQQue 0:34c739fcc3e0 47 //define variables
daniQQue 42:7164ccd2aa14 48
daniQQue 43:7d0b2dc05b80 49 //for motorcontrol
daniQQue 43:7d0b2dc05b80 50 const int cw = 0; // motor should turn clockwise
daniQQue 43:7d0b2dc05b80 51 const int ccw =1; // motor should turn counterclockwise
daniQQue 43:7d0b2dc05b80 52 const float gearboxratio=131.25; // gearboxratio from encoder to motor
daniQQue 43:7d0b2dc05b80 53 const float rev_rond=64.0; // revolutions per round of encoder
daniQQue 43:7d0b2dc05b80 54 int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
daniQQue 43:7d0b2dc05b80 55 int switch_signal_triceps=0; // switching between motors.
daniQQue 43:7d0b2dc05b80 56
daniQQue 43:7d0b2dc05b80 57 volatile double cut_off_value_biceps_right = 0.04; //tested, normal values. Can be changed by calibration
daniQQue 43:7d0b2dc05b80 58 volatile double cut_off_value_biceps_left = -0.04; //volatiles becaused changen in interrupt
daniQQue 43:7d0b2dc05b80 59 volatile double cut_off_value_triceps=-0.03;
daniQQue 43:7d0b2dc05b80 60 double signal_biceps_sum;
daniQQue 43:7d0b2dc05b80 61 double bicepstriceps_rightarm;
daniQQue 40:187ef29de53d 62
daniQQue 43:7d0b2dc05b80 63 volatile double voltage_motor1=0.18; //pwm is de pulse with tussen geen ampere en wel ampere motor 1
daniQQue 43:7d0b2dc05b80 64 volatile double voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1
daniQQue 43:7d0b2dc05b80 65
daniQQue 43:7d0b2dc05b80 66 int motorswitch= 0; //start van de teller wordt op nul gesteld
daniQQue 40:187ef29de53d 67
daniQQue 43:7d0b2dc05b80 68 //variables and constants for calibration
daniQQue 43:7d0b2dc05b80 69
daniQQue 43:7d0b2dc05b80 70 const float percentage_max_triceps=0.3;
daniQQue 43:7d0b2dc05b80 71 const float percentage_max_biceps =0.3;
daniQQue 43:7d0b2dc05b80 72 double max_biceps; //calibration maximum biceps
daniQQue 43:7d0b2dc05b80 73 double max_triceps; //calibration maximum triceps
daniQQue 43:7d0b2dc05b80 74
daniQQue 43:7d0b2dc05b80 75 //variables for feedback loop, manual calibration
daniQQue 43:7d0b2dc05b80 76
daniQQue 43:7d0b2dc05b80 77 volatile double current_position_motor1 = 0; // at start, the position is 0. Manual calibration
daniQQue 43:7d0b2dc05b80 78 volatile double previous_position_motor1 = 0; // at start, the position is 0. Manual calibration
daniQQue 43:7d0b2dc05b80 79 volatile double current_position_motor2 = 0; // at start, the position is 0. Manual calibration
daniQQue 43:7d0b2dc05b80 80 volatile double previous_position_motor2 = 0; // at start, the position is 0. Manual calibration
daniQQue 43:7d0b2dc05b80 81 volatile double rev_counts_motor1=0;
daniQQue 43:7d0b2dc05b80 82 volatile double rev_counts_motor2=0;
daniQQue 43:7d0b2dc05b80 83 volatile double counts_encoder1;
daniQQue 43:7d0b2dc05b80 84 volatile double counts_encoder2;
daniQQue 43:7d0b2dc05b80 85
daniQQue 43:7d0b2dc05b80 86 volatile bool tickerflag; //tickerflag is true or false, implicated by bool.
daniQQue 43:7d0b2dc05b80 87 volatile double speed_motor1; // speed in rad/s
daniQQue 43:7d0b2dc05b80 88 volatile double speed_motor2; // speed in rad/s
daniQQue 43:7d0b2dc05b80 89
daniQQue 43:7d0b2dc05b80 90
daniQQue 43:7d0b2dc05b80 91 //speedmeasuring
daniQQue 43:7d0b2dc05b80 92 double ticker_TS=0.001; // time step to derivate position to speed, in seconds.
daniQQue 43:7d0b2dc05b80 93 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
daniQQue 43:7d0b2dc05b80 94
daniQQue 43:7d0b2dc05b80 95 //resetbuttons
daniQQue 43:7d0b2dc05b80 96 volatile double value1_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 97 volatile double value2_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 98
daniQQue 43:7d0b2dc05b80 99 //filter defining
daniQQue 43:7d0b2dc05b80 100
daniQQue 43:7d0b2dc05b80 101 //b1 = biceps right arm
daniQQue 43:7d0b2dc05b80 102 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 43:7d0b2dc05b80 103 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 43:7d0b2dc05b80 104
daniQQue 43:7d0b2dc05b80 105 //t1= triceps right arm
daniQQue 43:7d0b2dc05b80 106 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 43:7d0b2dc05b80 107 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 43:7d0b2dc05b80 108
daniQQue 43:7d0b2dc05b80 109 //b2= biceps left arm
daniQQue 43:7d0b2dc05b80 110 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 43:7d0b2dc05b80 111 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 43:7d0b2dc05b80 112
daniQQue 43:7d0b2dc05b80 113 //after abs filtering
daniQQue 43:7d0b2dc05b80 114 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 43:7d0b2dc05b80 115 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 43:7d0b2dc05b80 116 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 5:688b1b5530d8 117
daniQQue 43:7d0b2dc05b80 118 //voids
daniQQue 40:187ef29de53d 119
daniQQue 43:7d0b2dc05b80 120 //to make a tickerfunction for speedmeasurment
daniQQue 43:7d0b2dc05b80 121
daniQQue 43:7d0b2dc05b80 122 void speed_sampling() // maakt een ticker functie aan
daniQQue 40:187ef29de53d 123 {
daniQQue 43:7d0b2dc05b80 124 tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
daniQQue 41:9ea3d5921f07 125 }
daniQQue 40:187ef29de53d 126
daniQQue 43:7d0b2dc05b80 127 //void to make the switch between the motors with triceps
daniQQue 43:7d0b2dc05b80 128
daniQQue 43:7d0b2dc05b80 129 void SwitchN() {
daniQQue 43:7d0b2dc05b80 130 if(switch_signal_triceps==1)
daniQQue 43:7d0b2dc05b80 131 {
daniQQue 43:7d0b2dc05b80 132 motorswitch++;
daniQQue 42:7164ccd2aa14 133
daniQQue 43:7d0b2dc05b80 134 if (motorswitch%2==0)
daniQQue 43:7d0b2dc05b80 135 { pc.printf("If you contract the right arm, the robot will go right \r\n");
daniQQue 43:7d0b2dc05b80 136 pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
daniQQue 43:7d0b2dc05b80 137 pc.printf("\r\n");
daniQQue 43:7d0b2dc05b80 138 green=0;
daniQQue 43:7d0b2dc05b80 139 red=1;
daniQQue 43:7d0b2dc05b80 140 }
daniQQue 40:187ef29de53d 141
daniQQue 40:187ef29de53d 142 else
daniQQue 43:7d0b2dc05b80 143 {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
daniQQue 43:7d0b2dc05b80 144 pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
daniQQue 43:7d0b2dc05b80 145 pc.printf("\r\n");
daniQQue 43:7d0b2dc05b80 146 green=1;
daniQQue 43:7d0b2dc05b80 147 red=0;
daniQQue 43:7d0b2dc05b80 148 }
daniQQue 43:7d0b2dc05b80 149
daniQQue 43:7d0b2dc05b80 150 }
daniQQue 43:7d0b2dc05b80 151 }
daniQQue 40:187ef29de53d 152
daniQQue 43:7d0b2dc05b80 153 //callibration
daniQQue 43:7d0b2dc05b80 154 void calibration_biceps(){
daniQQue 43:7d0b2dc05b80 155 pc.printf("start of calibration biceps, contract maximal \n");
daniQQue 43:7d0b2dc05b80 156 red=1;
daniQQue 43:7d0b2dc05b80 157 green=1;
daniQQue 43:7d0b2dc05b80 158 blue=0;
daniQQue 43:7d0b2dc05b80 159
daniQQue 43:7d0b2dc05b80 160 for(int n =0; n<3000;n++) //read for 2000 samples as calibration
daniQQue 43:7d0b2dc05b80 161 {
daniQQue 43:7d0b2dc05b80 162 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 43:7d0b2dc05b80 163 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
daniQQue 43:7d0b2dc05b80 164 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
daniQQue 43:7d0b2dc05b80 165 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
daniQQue 43:7d0b2dc05b80 166 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
daniQQue 43:7d0b2dc05b80 167
daniQQue 43:7d0b2dc05b80 168 if (emg_filtered_biceps_right > max_biceps) //determine what the highest reachable emg signal is
daniQQue 43:7d0b2dc05b80 169 {
daniQQue 43:7d0b2dc05b80 170 max_biceps = emg_filtered_biceps_right;
daniQQue 43:7d0b2dc05b80 171
daniQQue 43:7d0b2dc05b80 172 }
daniQQue 43:7d0b2dc05b80 173 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 43:7d0b2dc05b80 174 }
daniQQue 43:7d0b2dc05b80 175 cut_off_value_biceps_right=percentage_max_biceps*max_biceps;
daniQQue 43:7d0b2dc05b80 176 cut_off_value_biceps_left=-cut_off_value_biceps_right;
daniQQue 43:7d0b2dc05b80 177 //toggle lights
daniQQue 43:7d0b2dc05b80 178 blue=!blue;
daniQQue 43:7d0b2dc05b80 179
daniQQue 43:7d0b2dc05b80 180 pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );
daniQQue 43:7d0b2dc05b80 181 pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
daniQQue 43:7d0b2dc05b80 182
daniQQue 43:7d0b2dc05b80 183 wait(0.5f);
daniQQue 43:7d0b2dc05b80 184
daniQQue 43:7d0b2dc05b80 185 if (motorswitch%2==0)
daniQQue 43:7d0b2dc05b80 186 {green=0;
daniQQue 43:7d0b2dc05b80 187 red=1;}
daniQQue 43:7d0b2dc05b80 188
daniQQue 43:7d0b2dc05b80 189 else {green=1;
daniQQue 43:7d0b2dc05b80 190 red=0;}
daniQQue 43:7d0b2dc05b80 191
daniQQue 43:7d0b2dc05b80 192 }
daniQQue 43:7d0b2dc05b80 193
daniQQue 43:7d0b2dc05b80 194 void calibration_triceps(){
daniQQue 43:7d0b2dc05b80 195 red=1;
daniQQue 43:7d0b2dc05b80 196 green=1;
daniQQue 43:7d0b2dc05b80 197 blue=0;
daniQQue 43:7d0b2dc05b80 198
daniQQue 43:7d0b2dc05b80 199 pc.printf("start of calibration triceps\r\n");
daniQQue 43:7d0b2dc05b80 200
daniQQue 43:7d0b2dc05b80 201 for(int n =0; n<3000;n++) //read for 2000 samples as calibration
daniQQue 43:7d0b2dc05b80 202 {
daniQQue 43:7d0b2dc05b80 203 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 43:7d0b2dc05b80 204 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
daniQQue 43:7d0b2dc05b80 205 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
daniQQue 43:7d0b2dc05b80 206 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
daniQQue 43:7d0b2dc05b80 207 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
daniQQue 43:7d0b2dc05b80 208
daniQQue 43:7d0b2dc05b80 209 if (emg_filtered_triceps_right > max_triceps) //determine what the highest reachable emg signal is
daniQQue 43:7d0b2dc05b80 210 {
daniQQue 43:7d0b2dc05b80 211 max_triceps = emg_filtered_triceps_right;
daniQQue 43:7d0b2dc05b80 212
daniQQue 43:7d0b2dc05b80 213 }
daniQQue 43:7d0b2dc05b80 214 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 43:7d0b2dc05b80 215 }
daniQQue 43:7d0b2dc05b80 216 cut_off_value_triceps=-percentage_max_triceps*max_triceps;
daniQQue 43:7d0b2dc05b80 217 pc.printf(" end of calibration\r\n");
daniQQue 43:7d0b2dc05b80 218 pc.printf(" change of cv triceps: %f ",cut_off_value_triceps );
daniQQue 43:7d0b2dc05b80 219 blue=!blue;
daniQQue 43:7d0b2dc05b80 220 wait(0.5f);
daniQQue 43:7d0b2dc05b80 221 if (motorswitch%2==0)
daniQQue 43:7d0b2dc05b80 222 {green=0;
daniQQue 43:7d0b2dc05b80 223 red=1;}
daniQQue 43:7d0b2dc05b80 224
daniQQue 43:7d0b2dc05b80 225 else {green=1;
daniQQue 43:7d0b2dc05b80 226 red=0;}
daniQQue 43:7d0b2dc05b80 227
daniQQue 43:7d0b2dc05b80 228 }
daniQQue 43:7d0b2dc05b80 229
daniQQue 43:7d0b2dc05b80 230
daniQQue 43:7d0b2dc05b80 231 //sampling emg with filters as defined before
daniQQue 43:7d0b2dc05b80 232 void filter(){
daniQQue 43:7d0b2dc05b80 233 //biceps right arm read+filtering
daniQQue 43:7d0b2dc05b80 234 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 43:7d0b2dc05b80 235 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
daniQQue 43:7d0b2dc05b80 236 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
daniQQue 43:7d0b2dc05b80 237 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
daniQQue 43:7d0b2dc05b80 238 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
daniQQue 43:7d0b2dc05b80 239
daniQQue 43:7d0b2dc05b80 240 //triceps right arm read+filtering
daniQQue 43:7d0b2dc05b80 241 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 43:7d0b2dc05b80 242 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
daniQQue 43:7d0b2dc05b80 243 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
daniQQue 43:7d0b2dc05b80 244 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
daniQQue 43:7d0b2dc05b80 245 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
daniQQue 43:7d0b2dc05b80 246
daniQQue 43:7d0b2dc05b80 247 //biceps left arm read+filtering
daniQQue 43:7d0b2dc05b80 248 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 43:7d0b2dc05b80 249 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
daniQQue 43:7d0b2dc05b80 250 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
daniQQue 43:7d0b2dc05b80 251 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
daniQQue 43:7d0b2dc05b80 252 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
daniQQue 43:7d0b2dc05b80 253
daniQQue 43:7d0b2dc05b80 254 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 43:7d0b2dc05b80 255 signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 43:7d0b2dc05b80 256 bicepstriceps_rightarm= emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 43:7d0b2dc05b80 257
daniQQue 43:7d0b2dc05b80 258 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 43:7d0b2dc05b80 259 if (signal_biceps_sum>cut_off_value_biceps_right)
daniQQue 43:7d0b2dc05b80 260 {onoffsignal_biceps=1;}
daniQQue 43:7d0b2dc05b80 261
daniQQue 43:7d0b2dc05b80 262 else if (signal_biceps_sum<cut_off_value_biceps_left)
daniQQue 43:7d0b2dc05b80 263 {
daniQQue 43:7d0b2dc05b80 264 onoffsignal_biceps=-1;
daniQQue 43:7d0b2dc05b80 265 }
daniQQue 43:7d0b2dc05b80 266
daniQQue 43:7d0b2dc05b80 267 else
daniQQue 43:7d0b2dc05b80 268 {onoffsignal_biceps=0;}
daniQQue 43:7d0b2dc05b80 269
daniQQue 43:7d0b2dc05b80 270 //creating on/off signal for switch (left arm)
daniQQue 43:7d0b2dc05b80 271
daniQQue 43:7d0b2dc05b80 272 if (bicepstriceps_rightarm<cut_off_value_triceps)
daniQQue 43:7d0b2dc05b80 273 {
daniQQue 43:7d0b2dc05b80 274 switch_signal_triceps=1;
daniQQue 43:7d0b2dc05b80 275 }
daniQQue 43:7d0b2dc05b80 276
daniQQue 43:7d0b2dc05b80 277 else
daniQQue 43:7d0b2dc05b80 278 {
daniQQue 43:7d0b2dc05b80 279 switch_signal_triceps=0;
daniQQue 43:7d0b2dc05b80 280 }
daniQQue 43:7d0b2dc05b80 281
daniQQue 43:7d0b2dc05b80 282 //send signals to scope
daniQQue 43:7d0b2dc05b80 283 scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0
daniQQue 43:7d0b2dc05b80 284 scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1
daniQQue 43:7d0b2dc05b80 285 scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2
daniQQue 43:7d0b2dc05b80 286
daniQQue 43:7d0b2dc05b80 287
daniQQue 43:7d0b2dc05b80 288 scope.send(); //send all the signals to the scope
daniQQue 43:7d0b2dc05b80 289 }
daniQQue 0:34c739fcc3e0 290
daniQQue 0:34c739fcc3e0 291 //program
daniQQue 0:34c739fcc3e0 292
daniQQue 0:34c739fcc3e0 293 int main()
daniQQue 0:34c739fcc3e0 294 {
daniQQue 43:7d0b2dc05b80 295 pc.baud(115200); //connect with pc with baudrate 115200
daniQQue 43:7d0b2dc05b80 296 QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING); // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen.
daniQQue 43:7d0b2dc05b80 297 QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
daniQQue 40:187ef29de53d 298
daniQQue 43:7d0b2dc05b80 299 //attach voids to tickers and interrupts
daniQQue 43:7d0b2dc05b80 300 speed_measuring.attach(&speed_sampling,ticker_TS); //sampling function, for speed measurement
daniQQue 43:7d0b2dc05b80 301 sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
daniQQue 43:7d0b2dc05b80 302 switch_function.attach(&SwitchN,1.0); //switch is every second available
daniQQue 43:7d0b2dc05b80 303 button_calibration_biceps.fall (&calibration_biceps); //to call calibration biceps, stop everything else
daniQQue 43:7d0b2dc05b80 304 button_calibration_triceps.fall(&calibration_triceps); //to call calibration triceps, stop everything else
daniQQue 43:7d0b2dc05b80 305
daniQQue 43:7d0b2dc05b80 306 //print which motor will be controlled by text and light. Red: up/down=motor 2. Green: left/right=motor 1.
daniQQue 43:7d0b2dc05b80 307 if (motorswitch%2==0)
daniQQue 43:7d0b2dc05b80 308 { pc.printf("If you contract the right arm, the robot will go right \r\n");
daniQQue 43:7d0b2dc05b80 309 pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
daniQQue 43:7d0b2dc05b80 310 pc.printf("\r\n");
daniQQue 43:7d0b2dc05b80 311 green=0;
daniQQue 43:7d0b2dc05b80 312 red=1;
daniQQue 43:7d0b2dc05b80 313 blue=1;
daniQQue 43:7d0b2dc05b80 314 } //if loop closed
daniQQue 42:7164ccd2aa14 315
daniQQue 42:7164ccd2aa14 316 else
daniQQue 43:7d0b2dc05b80 317 {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
daniQQue 43:7d0b2dc05b80 318 pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
daniQQue 43:7d0b2dc05b80 319 pc.printf("\r\n");
daniQQue 43:7d0b2dc05b80 320 green=1;
daniQQue 43:7d0b2dc05b80 321 red=0;
daniQQue 43:7d0b2dc05b80 322 blue=1;
daniQQue 43:7d0b2dc05b80 323 } //else loop closed
daniQQue 43:7d0b2dc05b80 324
daniQQue 43:7d0b2dc05b80 325
daniQQue 0:34c739fcc3e0 326 //endless loop
daniQQue 0:34c739fcc3e0 327
daniQQue 40:187ef29de53d 328 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
daniQQue 43:7d0b2dc05b80 329
daniQQue 43:7d0b2dc05b80 330 //encoder and velocity measurement
daniQQue 43:7d0b2dc05b80 331 if (tickerflag ==1)
daniQQue 43:7d0b2dc05b80 332 {
daniQQue 43:7d0b2dc05b80 333 //position change, 'memory function'
daniQQue 43:7d0b2dc05b80 334 previous_position_motor1 = current_position_motor1; // zorgt er voor dat de huidige positie wordt gedefineerd als de vorige positie is
daniQQue 43:7d0b2dc05b80 335 current_position_motor1 = rev_counts_motor1; // zorgt dat de huidige positie wordt gedefineerd als het huidige aantal rondejs dat gedraaid is
daniQQue 43:7d0b2dc05b80 336 previous_position_motor2 = current_position_motor2; // zelfde maar dan voor motor2
daniQQue 43:7d0b2dc05b80 337 current_position_motor2 = rev_counts_motor2;
daniQQue 43:7d0b2dc05b80 338
daniQQue 43:7d0b2dc05b80 339 //speed calculation
daniQQue 43:7d0b2dc05b80 340 speed_motor1=((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
daniQQue 43:7d0b2dc05b80 341 speed_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
daniQQue 43:7d0b2dc05b80 342
daniQQue 43:7d0b2dc05b80 343 tickerflag = 0; // reset de tickerflag weer op 0 zodat het loopje niet wordt doorlopen tot de volgende tick zo kan de tijd tussen het lopen van ieder loopje gecontroleerd worden
daniQQue 43:7d0b2dc05b80 344 } //if tickerflag==1 closed
daniQQue 43:7d0b2dc05b80 345
daniQQue 43:7d0b2dc05b80 346 //control and monitor motor with EMG signal, with build in restrictions.
daniQQue 43:7d0b2dc05b80 347
daniQQue 43:7d0b2dc05b80 348 //left biceps contracted:
daniQQue 43:7d0b2dc05b80 349 if (onoffsignal_biceps==-1) //left biceps contracted
daniQQue 43:7d0b2dc05b80 350 { //open if loop for left biceps
daniQQue 8:cd0cb71b69f2 351
daniQQue 43:7d0b2dc05b80 352 if (motorswitch%2==0) //-3.4 is limitationpoint, when motor turns clockwise
daniQQue 43:7d0b2dc05b80 353 {
daniQQue 43:7d0b2dc05b80 354 direction_motor1 = cw;
daniQQue 43:7d0b2dc05b80 355 pwm_motor1 = voltage_motor1;
daniQQue 43:7d0b2dc05b80 356 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
daniQQue 43:7d0b2dc05b80 357 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 358 value1_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 359 } //if loop closed
daniQQue 43:7d0b2dc05b80 360
daniQQue 43:7d0b2dc05b80 361 else if (motorswitch%2!=0 && rev_counts_motor2<2.0) //2.0 is limitation for motor 2 when clockwise
daniQQue 43:7d0b2dc05b80 362 {
daniQQue 43:7d0b2dc05b80 363 direction_motor2 = cw;
daniQQue 43:7d0b2dc05b80 364 pwm_motor2 = voltage_motor2;
daniQQue 43:7d0b2dc05b80 365 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
daniQQue 43:7d0b2dc05b80 366 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
daniQQue 43:7d0b2dc05b80 367 value2_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 368 } //else if loop closed
daniQQue 43:7d0b2dc05b80 369
daniQQue 43:7d0b2dc05b80 370 //speed control motor 1
daniQQue 43:7d0b2dc05b80 371 if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
daniQQue 43:7d0b2dc05b80 372 voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
daniQQue 43:7d0b2dc05b80 373
daniQQue 43:7d0b2dc05b80 374 else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
daniQQue 43:7d0b2dc05b80 375 { voltage_motor1 = voltage_motor1+0.005; }
daniQQue 43:7d0b2dc05b80 376
daniQQue 43:7d0b2dc05b80 377 //speed control motor 2
daniQQue 43:7d0b2dc05b80 378 if (fabs(speed_motor2) > 5.0)
daniQQue 43:7d0b2dc05b80 379 { voltage_motor2 = voltage_motor2-0.005; }
daniQQue 43:7d0b2dc05b80 380
daniQQue 43:7d0b2dc05b80 381 else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
daniQQue 43:7d0b2dc05b80 382 { voltage_motor2 = voltage_motor2+0.005; }
daniQQue 43:7d0b2dc05b80 383
daniQQue 43:7d0b2dc05b80 384 } //if left biceps is contracted closed
daniQQue 43:7d0b2dc05b80 385
daniQQue 43:7d0b2dc05b80 386
daniQQue 43:7d0b2dc05b80 387 //right biceps contract (else if case)
daniQQue 43:7d0b2dc05b80 388 else if (onoffsignal_biceps==1) //right biceps contracted
daniQQue 43:7d0b2dc05b80 389 {
daniQQue 43:7d0b2dc05b80 390 if (motorswitch%2==0) //limitation of motor turning right
daniQQue 43:7d0b2dc05b80 391 {
daniQQue 43:7d0b2dc05b80 392 direction_motor1 = ccw; //turning right
daniQQue 43:7d0b2dc05b80 393 pwm_motor1 = voltage_motor1;
daniQQue 43:7d0b2dc05b80 394 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
daniQQue 43:7d0b2dc05b80 395 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
daniQQue 43:7d0b2dc05b80 396 value1_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 397 } //if loop; motor turning right closed
daniQQue 43:7d0b2dc05b80 398
daniQQue 43:7d0b2dc05b80 399 else if (motorswitch%2!=0 && rev_counts_motor2>-2.0) //limitation of motor turning down
daniQQue 43:7d0b2dc05b80 400 {
daniQQue 43:7d0b2dc05b80 401 direction_motor2 = ccw;
daniQQue 43:7d0b2dc05b80 402 pwm_motor2 = voltage_motor2;
daniQQue 43:7d0b2dc05b80 403 counts_encoder2 = Encoder2.getPulses();
daniQQue 43:7d0b2dc05b80 404 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
daniQQue 43:7d0b2dc05b80 405 value2_resetbutton = 0;
daniQQue 43:7d0b2dc05b80 406 } //if loop closed
daniQQue 43:7d0b2dc05b80 407
daniQQue 43:7d0b2dc05b80 408 //speed control motor 1
daniQQue 43:7d0b2dc05b80 409 if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
daniQQue 43:7d0b2dc05b80 410 voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
daniQQue 43:7d0b2dc05b80 411
daniQQue 43:7d0b2dc05b80 412 else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
daniQQue 43:7d0b2dc05b80 413 { voltage_motor1 = voltage_motor1+0.005; }
daniQQue 43:7d0b2dc05b80 414
daniQQue 43:7d0b2dc05b80 415 //speed control motor 2
daniQQue 43:7d0b2dc05b80 416 if (fabs(speed_motor2) > 5.0)
daniQQue 43:7d0b2dc05b80 417 { voltage_motor2 = voltage_motor2-0.005; }
daniQQue 40:187ef29de53d 418
daniQQue 43:7d0b2dc05b80 419 else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
daniQQue 43:7d0b2dc05b80 420 { voltage_motor2 = voltage_motor2+0.005; }
daniQQue 43:7d0b2dc05b80 421
daniQQue 43:7d0b2dc05b80 422 } //else if loop closed; right biceps contracted
daniQQue 43:7d0b2dc05b80 423
daniQQue 43:7d0b2dc05b80 424 else{ //no signal of both biceps!
daniQQue 43:7d0b2dc05b80 425 //encoders because even when signal off, motor can turn for a while.
daniQQue 43:7d0b2dc05b80 426
daniQQue 43:7d0b2dc05b80 427 counts_encoder1 = Encoder1.getPulses();
daniQQue 43:7d0b2dc05b80 428 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 429 counts_encoder2 = Encoder2.getPulses();
daniQQue 43:7d0b2dc05b80 430 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
daniQQue 40:187ef29de53d 431 pwm_motor2=0;
daniQQue 40:187ef29de53d 432 pwm_motor1=0;
daniQQue 43:7d0b2dc05b80 433 }
daniQQue 43:7d0b2dc05b80 434
daniQQue 43:7d0b2dc05b80 435 // all lopes are closed, except while (true)!
daniQQue 43:7d0b2dc05b80 436 //back to beginposition by button !
daniQQue 43:7d0b2dc05b80 437
daniQQue 43:7d0b2dc05b80 438 // motor 1
daniQQue 43:7d0b2dc05b80 439 while(resetbutton==0 && rev_counts_motor1<-0.1 && value1_resetbutton >= 0){
daniQQue 43:7d0b2dc05b80 440 direction_motor1 = ccw;
daniQQue 43:7d0b2dc05b80 441 pwm_motor1 = 0.1;
daniQQue 43:7d0b2dc05b80 442
daniQQue 43:7d0b2dc05b80 443 counts_encoder1 = Encoder1.getPulses();
daniQQue 43:7d0b2dc05b80 444 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 445 value1_resetbutton = 1;
daniQQue 43:7d0b2dc05b80 446 } //while loop closed
daniQQue 43:7d0b2dc05b80 447
daniQQue 43:7d0b2dc05b80 448 while (resetbutton==0 && rev_counts_motor1>0.1 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting.
daniQQue 43:7d0b2dc05b80 449
daniQQue 43:7d0b2dc05b80 450 direction_motor1 = cw;
daniQQue 43:7d0b2dc05b80 451 pwm_motor1 = 0.1;
daniQQue 43:7d0b2dc05b80 452
daniQQue 43:7d0b2dc05b80 453 counts_encoder1 = Encoder1.getPulses();
daniQQue 43:7d0b2dc05b80 454 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 455
daniQQue 43:7d0b2dc05b80 456 value1_resetbutton = -1;
daniQQue 43:7d0b2dc05b80 457 } //while loop closed
daniQQue 43:7d0b2dc05b80 458
daniQQue 43:7d0b2dc05b80 459 //motor 2
daniQQue 43:7d0b2dc05b80 460 while(resetbutton==0 && rev_counts_motor2<-0.1 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2
daniQQue 43:7d0b2dc05b80 461
daniQQue 43:7d0b2dc05b80 462 direction_motor2 = cw;
daniQQue 43:7d0b2dc05b80 463 pwm_motor2 = 0.1;
daniQQue 43:7d0b2dc05b80 464 pwm_motor1 = 0;
daniQQue 43:7d0b2dc05b80 465
daniQQue 43:7d0b2dc05b80 466 counts_encoder2 = Encoder2.getPulses();
daniQQue 43:7d0b2dc05b80 467 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 468 value2_resetbutton = 1;
daniQQue 43:7d0b2dc05b80 469 } //while loop closed
daniQQue 43:7d0b2dc05b80 470
daniQQue 43:7d0b2dc05b80 471 while (resetbutton==0 && rev_counts_motor2>0.1 && value2_resetbutton <=0){
daniQQue 43:7d0b2dc05b80 472
daniQQue 43:7d0b2dc05b80 473 direction_motor2 = ccw;
daniQQue 43:7d0b2dc05b80 474 pwm_motor2 = 0.1;
daniQQue 43:7d0b2dc05b80 475 pwm_motor1=0;
daniQQue 43:7d0b2dc05b80 476
daniQQue 43:7d0b2dc05b80 477 counts_encoder2 = Encoder2.getPulses();
daniQQue 43:7d0b2dc05b80 478 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
daniQQue 43:7d0b2dc05b80 479
daniQQue 43:7d0b2dc05b80 480 value2_resetbutton = -1;
daniQQue 43:7d0b2dc05b80 481 }//while loop closed
daniQQue 43:7d0b2dc05b80 482 pc.printf("rev count motor 1 is %f \r\n",rev_counts_motor1);
daniQQue 43:7d0b2dc05b80 483 pc.printf("speed motor 1: %f\r\n", speed_motor2);
daniQQue 43:7d0b2dc05b80 484 }//while true closed
daniQQue 0:34c739fcc3e0 485
daniQQue 43:7d0b2dc05b80 486 }//int main closed