Daniqe Kottelenberg / Mbed 2 deprecated a_pid_kal_end

Wil je hier nog je PID controler kort uitleggen? Is sneller denk ik. Rest is gedaan volgens mij. Hier zit kall in.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of another_try_from_scratch_on_emg by Floor Couwenberg

main.cpp@57:c546edf67c5c, 2016-11-04 (annotated)

Committer:
daniQQue
Date:
Fri Nov 04 15:12:52 2016 +0000
Revision:
57:c546edf67c5c
Parent:
56:a38412383477 
kallibratie, motors werken, pid controler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 57:c546edf67c5c 1 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 2 //libraries
daniQQue 49:818a0e90ed9c 3 #include "mbed.h" //mbed revision 113
daniQQue 49:818a0e90ed9c 4 #include "HIDScope.h" //Hidscope by Tom Lankhorst
daniQQue 49:818a0e90ed9c 5 #include "BiQuad.h" //BiQuad by Tom Lankhorst
daniQQue 49:818a0e90ed9c 6 #include "MODSERIAL.h" //Modserial
sivuu 50:2c03357de7cc 7 #include "QEI.h" //QEI library for the encoders
daniQQue 0:34c739fcc3e0 8
daniQQue 46:4a8889f9dc9f 9
daniQQue 57:c546edf67c5c 10 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 11 //Define objects
daniQQue 49:818a0e90ed9c 12
daniQQue 49:818a0e90ed9c 13 //EMG
sivuu 50:2c03357de7cc 14 AnalogIn emg_biceps_right_in (A0); //analog in to get EMG biceps (r) in to c++
daniQQue 40:187ef29de53d 15 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 40:187ef29de53d 16 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 49:818a0e90ed9c 17
daniQQue 49:818a0e90ed9c 18 //Tickers
daniQQue 49:818a0e90ed9c 19 Ticker sample_timer; //ticker for EMG signal sampling, analog becomes digital
daniQQue 49:818a0e90ed9c 20 Ticker ticker_switch; //ticker for switch, every second it is possible to switch
sivuu 50:2c03357de7cc 21 Ticker ticker_referenceangle; //ticker for the reference angle
sivuu 50:2c03357de7cc 22 Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1
daniQQue 57:c546edf67c5c 23 Ticker ticker_encoder; //ticker for encoderfunction motor 1
daniQQue 57:c546edf67c5c 24 Ticker ticker_calibration_biceps; //ticker for calibration biceps
daniQQue 57:c546edf67c5c 25 Ticker ticker_calibration_triceps; //ticker for calibation triceps
FloorC 55:d742332ced11 26
FloorC 55:d742332ced11 27 //Timer
FloorC 55:d742332ced11 28 Timer timer;
daniQQue 49:818a0e90ed9c 29
daniQQue 49:818a0e90ed9c 30 //Monitoring
daniQQue 46:4a8889f9dc9f 31 HIDScope scope(5); //open 5 channels in hidscope
daniQQue 46:4a8889f9dc9f 32 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 49:818a0e90ed9c 33 DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
daniQQue 49:818a0e90ed9c 34 DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
daniQQue 57:c546edf67c5c 35 DigitalOut blue(LED_BLUE); //LED on K64f board, 1 is out; o is on
daniQQue 57:c546edf67c5c 36
daniQQue 57:c546edf67c5c 37 //buttons
daniQQue 57:c546edf67c5c 38 DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
daniQQue 57:c546edf67c5c 39 DigitalIn button_calibration_triceps (SW2); // button to start calibration triceps
daniQQue 40:187ef29de53d 40
daniQQue 40:187ef29de53d 41 //motors
sivuu 50:2c03357de7cc 42 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 43 PwmOut pwm_motor1(D6);
sivuu 50:2c03357de7cc 44 DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator
daniQQue 47:ddaa59d48aca 45 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 46
sivuu 50:2c03357de7cc 47 //encoders
sivuu 50:2c03357de7cc 48 DigitalIn encoder1A(D13);
sivuu 50:2c03357de7cc 49 DigitalIn encoder1B(D12);
sivuu 50:2c03357de7cc 50
sivuu 50:2c03357de7cc 51 //controller
sivuu 50:2c03357de7cc 52 BiQuad PID_controller;
sivuu 50:2c03357de7cc 53
daniQQue 57:c546edf67c5c 54 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 55 //define variables
daniQQue 47:ddaa59d48aca 56
sivuu 50:2c03357de7cc 57 //thresholds
sivuu 50:2c03357de7cc 58 double treshold_biceps_right = 0.04; //common values that work.
sivuu 52:0135deb3b07f 59 double treshold_biceps_left = -0.04; // tested on multiple persons
sivuu 52:0135deb3b07f 60 double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold
daniQQue 49:818a0e90ed9c 61
daniQQue 57:c546edf67c5c 62
daniQQue 57:c546edf67c5c 63 //calibration variables
daniQQue 57:c546edf67c5c 64 const float percentage_max_triceps=0.25; //percentage from max to calculate new treshold
daniQQue 57:c546edf67c5c 65 const float percentage_max_biceps =0.3; //percentage from max to calculate new treshold
daniQQue 57:c546edf67c5c 66 double max_biceps; //calibration maximum biceps
daniQQue 57:c546edf67c5c 67 double max_triceps; //calibration maximum triceps
daniQQue 57:c546edf67c5c 68
daniQQue 49:818a0e90ed9c 69 //on/off and switch signals
daniQQue 57:c546edf67c5c 70 int switch_signal = 0; //start of counter, switch made by even and odd numbers
daniQQue 57:c546edf67c5c 71 int onoffsignal_biceps; //on/off signal created by the bicepssignal. (-1: left biceps contract, 0: nothing contracted, 1: right biceps contracted)
daniQQue 45:08bddea67bd8 72 int switch_signal_triceps;
daniQQue 46:4a8889f9dc9f 73
daniQQue 49:818a0e90ed9c 74 //motorvariables
daniQQue 49:818a0e90ed9c 75 float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
daniQQue 49:818a0e90ed9c 76 float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
daniQQue 46:4a8889f9dc9f 77
daniQQue 49:818a0e90ed9c 78 int cw=0; //clockwise direction
daniQQue 49:818a0e90ed9c 79 int ccw=1; //counterclockwise direction
sivuu 50:2c03357de7cc 80
sivuu 50:2c03357de7cc 81 //encoder
daniQQue 57:c546edf67c5c 82 int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1.
daniQQue 57:c546edf67c5c 83 float rev_counts_motor1; //Calculated revolutions
daniQQue 57:c546edf67c5c 84 float rev_counts_motor1_rad; //calculated revolutions in rad!
daniQQue 57:c546edf67c5c 85 const float gearboxratio=131.25; // gearboxratio from encoder to motor
daniQQue 57:c546edf67c5c 86 const float rev_rond=64.0; // number of revolutions per rotation
daniQQue 57:c546edf67c5c 87
daniQQue 57:c546edf67c5c 88 QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the Encoder.
sivuu 50:2c03357de7cc 89
sivuu 50:2c03357de7cc 90 //reference
daniQQue 57:c546edf67c5c 91 volatile float d_ref = 0;
daniQQue 57:c546edf67c5c 92 const float w_ref = 1.5;
daniQQue 57:c546edf67c5c 93 volatile double t_start;
daniQQue 57:c546edf67c5c 94 volatile double w_var;
daniQQue 57:c546edf67c5c 95 const double Ts = 0.001; //Time for diverse tickers. It is comparable to a frequency of 1000Hz.
sivuu 50:2c03357de7cc 96
sivuu 50:2c03357de7cc 97 //controller
daniQQue 57:c546edf67c5c 98 const double Kp = 0.3823;
daniQQue 57:c546edf67c5c 99 const double Ki = 0.1279;
daniQQue 57:c546edf67c5c 100 const double Kd = 0.2519;
daniQQue 57:c546edf67c5c 101 const double N = 100;
daniQQue 57:c546edf67c5c 102 volatile double error1;
daniQQue 57:c546edf67c5c 103 volatile double controlOutput;
daniQQue 57:c546edf67c5c 104 bool start_motor = true;
daniQQue 57:c546edf67c5c 105 volatile double starttime;
daniQQue 57:c546edf67c5c 106
daniQQue 57:c546edf67c5c 107 //=======================================================================================================================================================
daniQQue 44:c79e5944ac91 108 //filter coefficients
daniQQue 40:187ef29de53d 109
daniQQue 40:187ef29de53d 110 //b1 = biceps right arm
daniQQue 57:c546edf67c5c 111 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
daniQQue 57:c546edf67c5c 112 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
daniQQue 40:187ef29de53d 113
daniQQue 40:187ef29de53d 114 //t1= triceps right arm
daniQQue 57:c546edf67c5c 115 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
daniQQue 57:c546edf67c5c 116 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
daniQQue 5:688b1b5530d8 117
daniQQue 40:187ef29de53d 118 //b2= biceps left arm
daniQQue 57:c546edf67c5c 119 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
daniQQue 57:c546edf67c5c 120 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
daniQQue 40:187ef29de53d 121
daniQQue 40:187ef29de53d 122 //after abs filtering
daniQQue 57:c546edf67c5c 123 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
daniQQue 57:c546edf67c5c 124 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
daniQQue 57:c546edf67c5c 125 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
daniQQue 10:7255b59224cc 126
daniQQue 57:c546edf67c5c 127 //=======================================================================================================================================================
daniQQue 45:08bddea67bd8 128 //voids
daniQQue 57:c546edf67c5c 129 //=======================================================================================================================================================
daniQQue 45:08bddea67bd8 130
daniQQue 40:187ef29de53d 131 //function teller
daniQQue 49:818a0e90ed9c 132 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
daniQQue 45:08bddea67bd8 133 if(switch_signal_triceps==1)
daniQQue 40:187ef29de53d 134 {
daniQQue 45:08bddea67bd8 135 switch_signal++;
daniQQue 49:818a0e90ed9c 136
daniQQue 49:818a0e90ed9c 137 // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
daniQQue 49:818a0e90ed9c 138
daniQQue 40:187ef29de53d 139 green=!green;
daniQQue 40:187ef29de53d 140 red=!red;
daniQQue 47:ddaa59d48aca 141
daniQQue 45:08bddea67bd8 142 if (switch_signal%2==0)
daniQQue 49:818a0e90ed9c 143 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 144 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 145 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 146 }
daniQQue 47:ddaa59d48aca 147
daniQQue 47:ddaa59d48aca 148
daniQQue 47:ddaa59d48aca 149 else
daniQQue 40:187ef29de53d 150 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 151 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 152 pc.printf("\r\n");
daniQQue 40:187ef29de53d 153 }
daniQQue 47:ddaa59d48aca 154
daniQQue 40:187ef29de53d 155 }
daniQQue 40:187ef29de53d 156 }
daniQQue 49:818a0e90ed9c 157
daniQQue 57:c546edf67c5c 158 //=======================================================================================================================================================
daniQQue 49:818a0e90ed9c 159 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 40:187ef29de53d 160
daniQQue 57:c546edf67c5c 161 //Filter void :// funciton which is called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 0:34c739fcc3e0 162 void filter(){
daniQQue 40:187ef29de53d 163 //biceps right arm read+filtering
daniQQue 49:818a0e90ed9c 164 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 165 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 166 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 167 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 168 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 169
daniQQue 40:187ef29de53d 170 //triceps right arm read+filtering
daniQQue 49:818a0e90ed9c 171 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 172 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 173 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 174 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 175 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 7:42d0e38196f1 176
daniQQue 40:187ef29de53d 177 //biceps left arm read+filtering
daniQQue 49:818a0e90ed9c 178 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 179 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 180 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 181 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 182 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 183
daniQQue 40:187ef29de53d 184 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 185 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 186 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 187 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 188
daniQQue 44:c79e5944ac91 189 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 49:818a0e90ed9c 190 if (signal_biceps_sum>treshold_biceps_right)
daniQQue 49:818a0e90ed9c 191 {onoffsignal_biceps=1;}
daniQQue 7:42d0e38196f1 192
daniQQue 49:818a0e90ed9c 193 else if (signal_biceps_sum<treshold_biceps_left)
daniQQue 49:818a0e90ed9c 194 { onoffsignal_biceps=-1; }
daniQQue 40:187ef29de53d 195
daniQQue 49:818a0e90ed9c 196 else
daniQQue 49:818a0e90ed9c 197 {onoffsignal_biceps=0;}
daniQQue 7:42d0e38196f1 198
daniQQue 40:187ef29de53d 199 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 200
daniQQue 49:818a0e90ed9c 201 if (bicepstriceps_rightarm<treshold_triceps)
daniQQue 49:818a0e90ed9c 202 { switch_signal_triceps=1; }
daniQQue 40:187ef29de53d 203
daniQQue 40:187ef29de53d 204 else
daniQQue 49:818a0e90ed9c 205 { switch_signal_triceps=0; }
daniQQue 49:818a0e90ed9c 206
daniQQue 49:818a0e90ed9c 207 //send signals to scope to monitor the EMG signals
daniQQue 49:818a0e90ed9c 208 scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
daniQQue 49:818a0e90ed9c 209 scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
daniQQue 49:818a0e90ed9c 210 scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
daniQQue 49:818a0e90ed9c 211 scope.set(3, onoffsignal_biceps); // set on/off signal for the motors to scope in channel 3
daniQQue 49:818a0e90ed9c 212 scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
daniQQue 40:187ef29de53d 213
daniQQue 57:c546edf67c5c 214 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 215 }
daniQQue 57:c546edf67c5c 216 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 217
daniQQue 57:c546edf67c5c 218 //reference void makes the reference that the controllor should follow. There is only a controller for motor 1.
sivuu 50:2c03357de7cc 219 void reference(){
daniQQue 57:c546edf67c5c 220 if (start_motor == true){
FloorC 55:d742332ced11 221 timer.start();
sivuu 50:2c03357de7cc 222 }
FloorC 55:d742332ced11 223 if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even
FloorC 55:d742332ced11 224 t_start = timer.read_ms();
daniQQue 57:c546edf67c5c 225 start_motor = false; //It means that motor 2 is running and therefore the PID controllor should not be working. Therefore the bool is set on false.
FloorC 55:d742332ced11 226
FloorC 55:d742332ced11 227 if (t_start < 1.0){
FloorC 55:d742332ced11 228 w_var = t_start*1.5;
FloorC 55:d742332ced11 229 }
FloorC 55:d742332ced11 230
FloorC 55:d742332ced11 231 else {
FloorC 55:d742332ced11 232 w_var = 1.5;
FloorC 55:d742332ced11 233 }
FloorC 55:d742332ced11 234
FloorC 55:d742332ced11 235 d_ref = d_ref + w_var * Ts;
FloorC 55:d742332ced11 236
FloorC 55:d742332ced11 237 }
FloorC 55:d742332ced11 238 if (d_ref > 12){
FloorC 55:d742332ced11 239 d_ref = 12;
FloorC 55:d742332ced11 240 start_motor = true;
sivuu 50:2c03357de7cc 241 //d_ref_const_cw = 1;
FloorC 55:d742332ced11 242 }
sivuu 50:2c03357de7cc 243 else{
sivuu 50:2c03357de7cc 244 d_ref = d_ref;
sivuu 50:2c03357de7cc 245 }
sivuu 50:2c03357de7cc 246
sivuu 52:0135deb3b07f 247 if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even //left biceps contracted{
FloorC 55:d742332ced11 248 t_start = timer.read_ms();
FloorC 55:d742332ced11 249 start_motor = false;
FloorC 55:d742332ced11 250
FloorC 55:d742332ced11 251 if (t_start < 1.0){
FloorC 55:d742332ced11 252 w_var = t_start*1.5;
FloorC 55:d742332ced11 253 }
FloorC 55:d742332ced11 254 else {
FloorC 55:d742332ced11 255 w_var = 1.5;
FloorC 55:d742332ced11 256 }
FloorC 55:d742332ced11 257 d_ref = d_ref - w_var * Ts;
sivuu 50:2c03357de7cc 258
sivuu 50:2c03357de7cc 259 }
FloorC 55:d742332ced11 260 if (d_ref < -12){
FloorC 55:d742332ced11 261 d_ref = -12;
FloorC 55:d742332ced11 262 start_motor = true;
FloorC 55:d742332ced11 263 }
sivuu 50:2c03357de7cc 264 else{
sivuu 50:2c03357de7cc 265 d_ref = d_ref;
sivuu 50:2c03357de7cc 266 }
sivuu 50:2c03357de7cc 267
sivuu 50:2c03357de7cc 268 }
daniQQue 57:c546edf67c5c 269 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 270 //This void calculates the error and makes the control output.
sivuu 50:2c03357de7cc 271 void m1_controller(){
sivuu 50:2c03357de7cc 272 error1 = d_ref-rev_counts_motor1_rad;
sivuu 51:b344a92b6a5f 273 controlOutput = PID_controller.step(error1);
sivuu 50:2c03357de7cc 274 }
daniQQue 57:c546edf67c5c 275 //=======================================================================================================================================================
sivuu 50:2c03357de7cc 276
daniQQue 57:c546edf67c5c 277 //This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously.
FloorC 55:d742332ced11 278 void encoders(){
FloorC 55:d742332ced11 279 counts_encoder1 = Encoder1.getPulses();
FloorC 55:d742332ced11 280 rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
FloorC 55:d742332ced11 281 rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
FloorC 55:d742332ced11 282
FloorC 55:d742332ced11 283 }
FloorC 55:d742332ced11 284
daniQQue 57:c546edf67c5c 285 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 286
daniQQue 57:c546edf67c5c 287 //The calibration of the Biceps threshold is started by a button.
daniQQue 57:c546edf67c5c 288 //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
daniQQue 57:c546edf67c5c 289 void calibration_biceps(){
daniQQue 57:c546edf67c5c 290 if (button_calibration_biceps==0){ //only runs when button is pressed
daniQQue 57:c546edf67c5c 291
daniQQue 57:c546edf67c5c 292 //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
daniQQue 57:c546edf67c5c 293 ticker_switch.detach();
daniQQue 57:c546edf67c5c 294 sample_timer.detach();
daniQQue 57:c546edf67c5c 295
daniQQue 57:c546edf67c5c 296 //let the user know what is happening, blue led on: calibration is going.
daniQQue 57:c546edf67c5c 297 pc.printf("start of calibration biceps, contract maximal \r\n");
daniQQue 57:c546edf67c5c 298 pc.printf("\r\n");
daniQQue 57:c546edf67c5c 299 red=1;
daniQQue 57:c546edf67c5c 300 green=1;
daniQQue 57:c546edf67c5c 301 blue=0;
daniQQue 57:c546edf67c5c 302
daniQQue 57:c546edf67c5c 303 //start callibration of biceps
daniQQue 57:c546edf67c5c 304 for(int n =0; n<1500;n++) //read for 1500 samples as calibration
daniQQue 57:c546edf67c5c 305 {
daniQQue 57:c546edf67c5c 306 //biceps right arm read+filtering
daniQQue 57:c546edf67c5c 307 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 57:c546edf67c5c 308 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 57:c546edf67c5c 309 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 57:c546edf67c5c 310 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 57:c546edf67c5c 311 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 57:c546edf67c5c 312
daniQQue 57:c546edf67c5c 313 //triceps right arm read+filtering
daniQQue 57:c546edf67c5c 314 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 57:c546edf67c5c 315 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 57:c546edf67c5c 316 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 57:c546edf67c5c 317 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 57:c546edf67c5c 318 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 57:c546edf67c5c 319
daniQQue 57:c546edf67c5c 320 //biceps is +, triceps is -
daniQQue 57:c546edf67c5c 321 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 57:c546edf67c5c 322
daniQQue 57:c546edf67c5c 323 if (bicepstriceps_rightarm > max_biceps) //determine what the highest reachable emg signal is
daniQQue 57:c546edf67c5c 324 {
daniQQue 57:c546edf67c5c 325 max_biceps = bicepstriceps_rightarm;
daniQQue 57:c546edf67c5c 326
daniQQue 57:c546edf67c5c 327 }
daniQQue 57:c546edf67c5c 328 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 57:c546edf67c5c 329 }
daniQQue 57:c546edf67c5c 330 treshold_biceps_right=percentage_max_biceps*max_biceps; //determine new treshold, right biceps is +
daniQQue 57:c546edf67c5c 331 treshold_biceps_left=-treshold_biceps_right; //determine new treshold, right biceps is -
daniQQue 57:c546edf67c5c 332
daniQQue 57:c546edf67c5c 333 //toggle lights to see the calibration is done. Also show in putty that the calibration is done.
daniQQue 57:c546edf67c5c 334 blue=!blue;
daniQQue 57:c546edf67c5c 335
daniQQue 57:c546edf67c5c 336 pc.printf(" end of calibration\r\n",treshold_biceps_right );
daniQQue 57:c546edf67c5c 337 pc.printf(" change of cv biceps: %f ",treshold_biceps_right );
daniQQue 57:c546edf67c5c 338
daniQQue 57:c546edf67c5c 339 wait(0.2f);
daniQQue 57:c546edf67c5c 340
daniQQue 57:c546edf67c5c 341 //remind the person of what motor will go on an which direction
daniQQue 57:c546edf67c5c 342 if (switch_signal%2==0)
daniQQue 57:c546edf67c5c 343 {green=0;
daniQQue 57:c546edf67c5c 344 red=1;}
daniQQue 57:c546edf67c5c 345
daniQQue 57:c546edf67c5c 346 else {green=1;
daniQQue 57:c546edf67c5c 347 red=0;}
daniQQue 57:c546edf67c5c 348 }
daniQQue 57:c546edf67c5c 349 //reattach the functions to the tickers that were detached.
daniQQue 57:c546edf67c5c 350 ticker_switch.attach(&switch_function,1.0);
daniQQue 57:c546edf67c5c 351 sample_timer.attach(&filter, 0.001);
daniQQue 57:c546edf67c5c 352 }
daniQQue 57:c546edf67c5c 353 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 354
daniQQue 57:c546edf67c5c 355 //The calibration of the Triceps threshold is started by a button.
daniQQue 57:c546edf67c5c 356 //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
daniQQue 57:c546edf67c5c 357 void calibration_triceps(){
daniQQue 57:c546edf67c5c 358 if(button_calibration_triceps==0){ //only runs when button is pressed
daniQQue 57:c546edf67c5c 359
daniQQue 57:c546edf67c5c 360 //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
daniQQue 57:c546edf67c5c 361 ticker_switch.detach();
daniQQue 57:c546edf67c5c 362 sample_timer.detach();
daniQQue 57:c546edf67c5c 363
daniQQue 57:c546edf67c5c 364 //toggel LEDS and let the user know that callibration of triceps is starting.
daniQQue 57:c546edf67c5c 365 red=1;
daniQQue 57:c546edf67c5c 366 green=1;
daniQQue 57:c546edf67c5c 367 blue=0;
daniQQue 57:c546edf67c5c 368
daniQQue 57:c546edf67c5c 369 pc.printf("start of calibration triceps\r\n");
daniQQue 57:c546edf67c5c 370 pc.printf("\r\n");
daniQQue 57:c546edf67c5c 371
daniQQue 57:c546edf67c5c 372 //start calibration of triceps
daniQQue 57:c546edf67c5c 373 for(int n =0; n<1500;n++) //read for 2000 samples as calibration
daniQQue 57:c546edf67c5c 374 {
daniQQue 57:c546edf67c5c 375 //biceps right arm read+filtering
daniQQue 57:c546edf67c5c 376 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 57:c546edf67c5c 377 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 57:c546edf67c5c 378 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 57:c546edf67c5c 379 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 57:c546edf67c5c 380 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 57:c546edf67c5c 381
daniQQue 57:c546edf67c5c 382 //triceps right arm read+filtering
daniQQue 57:c546edf67c5c 383 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 57:c546edf67c5c 384 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 57:c546edf67c5c 385 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 57:c546edf67c5c 386 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 57:c546edf67c5c 387 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 57:c546edf67c5c 388
daniQQue 57:c546edf67c5c 389 //biceps is +, triceps is -
daniQQue 57:c546edf67c5c 390 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 57:c546edf67c5c 391
daniQQue 57:c546edf67c5c 392 if (bicepstriceps_rightarm < max_triceps) //determine what the lowest reachable emg of triceps (max in negative part) signal is
daniQQue 57:c546edf67c5c 393 {
daniQQue 57:c546edf67c5c 394 max_triceps = bicepstriceps_rightarm;
daniQQue 57:c546edf67c5c 395
daniQQue 57:c546edf67c5c 396 }
daniQQue 57:c546edf67c5c 397 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 57:c546edf67c5c 398 }
daniQQue 57:c546edf67c5c 399 treshold_triceps=percentage_max_triceps*max_triceps; //calculate the new treshold. This is a negative number due to the sum!
daniQQue 57:c546edf67c5c 400
daniQQue 57:c546edf67c5c 401 //Let the user know that the calibration is done.
daniQQue 57:c546edf67c5c 402 pc.printf(" end of calibration\r\n");
daniQQue 57:c546edf67c5c 403 pc.printf(" change of cv triceps: %f ",treshold_triceps );
daniQQue 57:c546edf67c5c 404 blue=!blue;
daniQQue 57:c546edf67c5c 405 wait(0.2f);
daniQQue 57:c546edf67c5c 406 if (switch_signal%2==0)
daniQQue 57:c546edf67c5c 407 {green=0;
daniQQue 57:c546edf67c5c 408 red=1;}
daniQQue 57:c546edf67c5c 409
daniQQue 57:c546edf67c5c 410 else {green=1;
daniQQue 57:c546edf67c5c 411 red=0;}
daniQQue 57:c546edf67c5c 412 }
daniQQue 57:c546edf67c5c 413
daniQQue 57:c546edf67c5c 414 //reattach the functions to the tickers that were detached.
daniQQue 57:c546edf67c5c 415 sample_timer.attach(&filter, 0.001);
daniQQue 57:c546edf67c5c 416 ticker_switch.attach(&switch_function,1.0);
daniQQue 57:c546edf67c5c 417 }
daniQQue 57:c546edf67c5c 418 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 419
daniQQue 57:c546edf67c5c 420 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 421 //program
daniQQue 57:c546edf67c5c 422 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 423 int main()
daniQQue 0:34c739fcc3e0 424 {
daniQQue 40:187ef29de53d 425
daniQQue 49:818a0e90ed9c 426 pc.baud(115200); //connect with pc with baudrate 115200
daniQQue 49:818a0e90ed9c 427 green=1; //led is off (1), at beginning
daniQQue 49:818a0e90ed9c 428 red=0; //led is on (0), at beginning
daniQQue 49:818a0e90ed9c 429
daniQQue 49:818a0e90ed9c 430 //attach tickers to functions
daniQQue 57:c546edf67c5c 431 sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
daniQQue 57:c546edf67c5c 432 ticker_switch.attach(&switch_function,1.0); //it is possible to switch only once in a second, this ensures that the switch is not reacting on one signal multiple times.
sivuu 50:2c03357de7cc 433 ticker_referenceangle.attach(&reference, Ts);
sivuu 50:2c03357de7cc 434 ticker_controllerm1.attach(&m1_controller, Ts);
FloorC 55:d742332ced11 435 ticker_encoder.attach(&encoders, Ts);
daniQQue 57:c546edf67c5c 436 ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop EMG sampling and switch
daniQQue 57:c546edf67c5c 437 ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop EMG sampling and switch
sivuu 50:2c03357de7cc 438
sivuu 50:2c03357de7cc 439 //PID controller
sivuu 50:2c03357de7cc 440 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
daniQQue 44:c79e5944ac91 441
sivuu 51:b344a92b6a5f 442 //Encoder
FloorC 55:d742332ced11 443 //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
sivuu 51:b344a92b6a5f 444
daniQQue 49:818a0e90ed9c 445 //Show the user what the starting motor will be and what will happen
daniQQue 47:ddaa59d48aca 446 pc.printf("We will start the demonstration\r\n");
daniQQue 49:818a0e90ed9c 447 pc.printf("\r\n\r\n\r\n");
daniQQue 49:818a0e90ed9c 448
daniQQue 47:ddaa59d48aca 449 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 450 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 451 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 452 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 453 }
daniQQue 47:ddaa59d48aca 454
daniQQue 47:ddaa59d48aca 455
daniQQue 47:ddaa59d48aca 456 else
daniQQue 45:08bddea67bd8 457 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 45:08bddea67bd8 458 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 459 pc.printf("\r\n");
daniQQue 45:08bddea67bd8 460 }
daniQQue 45:08bddea67bd8 461
daniQQue 57:c546edf67c5c 462 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 463 //endless loop
daniQQue 0:34c739fcc3e0 464
daniQQue 40:187ef29de53d 465
daniQQue 49:818a0e90ed9c 466 while (true) { // neverending loop
daniQQue 8:cd0cb71b69f2 467
FloorC 56:a38412383477 468 if (onoffsignal_biceps==-1){ //left biceps contracted
FloorC 56:a38412383477 469
FloorC 56:a38412383477 470 if (switch_signal%2==0){ //switch even
FloorC 56:a38412383477 471
sivuu 51:b344a92b6a5f 472 speedmotor1=controlOutput;
daniQQue 57:c546edf67c5c 473
sivuu 51:b344a92b6a5f 474 if (speedmotor1<0){
daniQQue 57:c546edf67c5c 475 richting_motor1 = cw; // motor 1, right
sivuu 51:b344a92b6a5f 476 }
sivuu 51:b344a92b6a5f 477 else {
daniQQue 57:c546edf67c5c 478 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 479 }
daniQQue 57:c546edf67c5c 480 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
daniQQue 57:c546edf67c5c 481
daniQQue 40:187ef29de53d 482 }
sivuu 51:b344a92b6a5f 483
daniQQue 40:187ef29de53d 484
daniQQue 49:818a0e90ed9c 485 else //switch odd
daniQQue 40:187ef29de53d 486 {
daniQQue 49:818a0e90ed9c 487 richting_motor2 = ccw; //motor 2, up
daniQQue 49:818a0e90ed9c 488 pwm_motor2 = speedmotor2;//speed of motor 2
daniQQue 40:187ef29de53d 489
daniQQue 40:187ef29de53d 490 }
daniQQue 40:187ef29de53d 491
daniQQue 8:cd0cb71b69f2 492 }
sivuu 52:0135deb3b07f 493 else if (onoffsignal_biceps==1) //right biceps contracted
daniQQue 40:187ef29de53d 494 {
daniQQue 49:818a0e90ed9c 495 if (switch_signal%2==0) //switch signal even
daniQQue 40:187ef29de53d 496 {
sivuu 51:b344a92b6a5f 497 speedmotor1=controlOutput;
daniQQue 57:c546edf67c5c 498
sivuu 51:b344a92b6a5f 499 if (speedmotor1<0){
daniQQue 57:c546edf67c5c 500 richting_motor1 = cw; //motor 1, right
sivuu 51:b344a92b6a5f 501 }
sivuu 51:b344a92b6a5f 502 else {
daniQQue 57:c546edf67c5c 503 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 504 }
sivuu 51:b344a92b6a5f 505 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
daniQQue 57:c546edf67c5c 506
daniQQue 40:187ef29de53d 507 }
daniQQue 57:c546edf67c5c 508 else //switch signal odd
daniQQue 40:187ef29de53d 509 {
daniQQue 57:c546edf67c5c 510 richting_motor2 = cw; //motor 2, down
daniQQue 49:818a0e90ed9c 511 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 512
daniQQue 40:187ef29de53d 513 }
daniQQue 40:187ef29de53d 514 }
daniQQue 49:818a0e90ed9c 515 else{
daniQQue 57:c546edf67c5c 516 //no contraction of biceps, thus no motoraction.
daniQQue 40:187ef29de53d 517 pwm_motor2=0;
daniQQue 40:187ef29de53d 518 pwm_motor1=0;
FloorC 55:d742332ced11 519 start_motor = true;
FloorC 56:a38412383477 520
FloorC 56:a38412383477 521
daniQQue 40:187ef29de53d 522 }
daniQQue 40:187ef29de53d 523
daniQQue 49:818a0e90ed9c 524 }//while true closed
daniQQue 0:34c739fcc3e0 525
daniQQue 49:818a0e90ed9c 526 } //int main closed
daniQQue 49:818a0e90ed9c 527
daniQQue 57:c546edf67c5c 528 //=======================================================================================================================================================