Daniqe Kottelenberg / Mbed 2 deprecated Wubs_demo

met knopjes, voor Wubs, zit PID in dus restrictie.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of a_pid_kal_end_def by Floor Couwenberg

main.cpp@59:1725a3f02f37, 2016-11-08 (annotated)

Committer:
FloorC
Date:
Tue Nov 08 20:31:45 2016 +0000
Revision:
59:1725a3f02f37
Parent:
58:c91723359f62
Child:
60:c165691c4e86
alle pid gecomment

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
FloorC 59:1725a3f02f37 14 AnalogIn emg_biceps_right_in (A0); //analog in to get EMG biceps (r) in to c++
FloorC 59:1725a3f02f37 15 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 38 DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
FloorC 59:1725a3f02f37 39 DigitalIn button_calibration_triceps (SW2); // button to start calibration triceps
daniQQue 40:187ef29de53d 40
daniQQue 40:187ef29de53d 41 //motors
FloorC 59:1725a3f02f37 42 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 43 PwmOut pwm_motor1(D6);
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 58 double treshold_biceps_right = 0.04; //common values that work.
FloorC 59:1725a3f02f37 59 double treshold_biceps_left = -0.04; //tested on multiple persons
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 70 int switch_signal = 0; //start of counter, switch made by even and odd numbers
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 75 float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm 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
FloorC 59:1725a3f02f37 82 int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1
FloorC 59:1725a3f02f37 83 float rev_counts_motor1; //calculated revolutions
daniQQue 57:c546edf67c5c 84 float rev_counts_motor1_rad; //calculated revolutions in rad!
FloorC 59:1725a3f02f37 85 const float gearboxratio=131.25; //gearboxratio from encoder to motor
FloorC 59:1725a3f02f37 86 const float rev_rond=64.0; //number of revolutions per rotation
daniQQue 57:c546edf67c5c 87
FloorC 59:1725a3f02f37 88 QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the encoder
sivuu 50:2c03357de7cc 89
sivuu 50:2c03357de7cc 90 //reference
FloorC 59:1725a3f02f37 91 volatile float d_ref = 0; //reference angle, starts off 0
FloorC 59:1725a3f02f37 92 const float w_ref = 1.5; //reference speed, constant
FloorC 59:1725a3f02f37 93 volatile double t_start; //starttime of the timer
FloorC 59:1725a3f02f37 94 volatile double w_var; //variable reference speed for making the reference signal
FloorC 59:1725a3f02f37 95 const double Ts = 0.001; //time step for diverse tickers. It is comparable to a frequency of 1000Hz
sivuu 50:2c03357de7cc 96
sivuu 50:2c03357de7cc 97 //controller
FloorC 59:1725a3f02f37 98 const double Kp = 1.2614; //calculated value for the proportional action of the PID
FloorC 59:1725a3f02f37 99 const double Ki = 0.4219; //calculated value for the integral action of the PID
FloorC 59:1725a3f02f37 100 const double Kd = 0.8312; //calculated value for the derivative action of the PID
FloorC 59:1725a3f02f37 101 const double N = 100; //specified value for the filter coefficient of the PID
FloorC 59:1725a3f02f37 102 volatile double error1; //calculated error
FloorC 59:1725a3f02f37 103 volatile double controlOutput; //output of the PID-controller
FloorC 59:1725a3f02f37 104 bool start_motor = true; //bool to start the reference when the motor turns
FloorC 59:1725a3f02f37 105
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
FloorC 59:1725a3f02f37 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 10 Hz
FloorC 59:1725a3f02f37 112 BiQuad filternotch1_b1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
daniQQue 40:187ef29de53d 113
daniQQue 40:187ef29de53d 114 //t1= triceps right arm
FloorC 59:1725a3f02f37 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 10 Hz
FloorC 59:1725a3f02f37 116 BiQuad filternotch1_t1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
daniQQue 5:688b1b5530d8 117
daniQQue 40:187ef29de53d 118 //b2= biceps left arm
FloorC 59:1725a3f02f37 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 10 Hz
FloorC 59:1725a3f02f37 120 BiQuad filternotch1_b2 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
daniQQue 40:187ef29de53d 121
daniQQue 40:187ef29de53d 122 //after abs filtering
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 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
FloorC 59:1725a3f02f37 132 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
FloorC 59:1725a3f02f37 133 if(switch_signal_triceps==1){
daniQQue 45:08bddea67bd8 134 switch_signal++;
daniQQue 49:818a0e90ed9c 135
FloorC 59:1725a3f02f37 136 // 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 137
daniQQue 40:187ef29de53d 138 green=!green;
daniQQue 40:187ef29de53d 139 red=!red;
daniQQue 47:ddaa59d48aca 140
FloorC 59:1725a3f02f37 141 if (switch_signal%2==0){
FloorC 59:1725a3f02f37 142 pc.printf("If you contract the biceps, the robot will go right \r\n");
FloorC 59:1725a3f02f37 143 pc.printf("If you contract the triceps, the robot will go left \r\n");
FloorC 59:1725a3f02f37 144 pc.printf("\r\n");
FloorC 59:1725a3f02f37 145 }
daniQQue 47:ddaa59d48aca 146
daniQQue 47:ddaa59d48aca 147
FloorC 59:1725a3f02f37 148 else{
FloorC 59:1725a3f02f37 149 pc.printf("If you contract the biceps, the robot will go up \r\n");
FloorC 59:1725a3f02f37 150 pc.printf("If you contract the triceps, the robot will go down \r\n");
FloorC 59:1725a3f02f37 151 pc.printf("\r\n");
FloorC 59:1725a3f02f37 152 }
daniQQue 47:ddaa59d48aca 153
daniQQue 40:187ef29de53d 154 }
FloorC 59:1725a3f02f37 155 }
daniQQue 49:818a0e90ed9c 156
daniQQue 57:c546edf67c5c 157 //=======================================================================================================================================================
daniQQue 49:818a0e90ed9c 158 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 40:187ef29de53d 159
daniQQue 57:c546edf67c5c 160 //Filter void :// funciton which is called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 0:34c739fcc3e0 161 void filter(){
daniQQue 40:187ef29de53d 162 //biceps right arm read+filtering
daniQQue 49:818a0e90ed9c 163 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 164 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 165 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 166 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 167 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 168
daniQQue 40:187ef29de53d 169 //triceps right arm read+filtering
daniQQue 49:818a0e90ed9c 170 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 171 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 172 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 173 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 174 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 7:42d0e38196f1 175
daniQQue 40:187ef29de53d 176 //biceps left arm read+filtering
daniQQue 49:818a0e90ed9c 177 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 178 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 179 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 180 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 181 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 182
daniQQue 40:187ef29de53d 183 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 184 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 185 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 186 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 187
daniQQue 44:c79e5944ac91 188 //creating of on/off signal with the created on/off signals, with if statement for right arm!
FloorC 59:1725a3f02f37 189 if (signal_biceps_sum>treshold_biceps_right){
FloorC 59:1725a3f02f37 190 onoffsignal_biceps=1;
FloorC 59:1725a3f02f37 191 }
daniQQue 7:42d0e38196f1 192
FloorC 59:1725a3f02f37 193 else if (signal_biceps_sum<treshold_biceps_left){
FloorC 59:1725a3f02f37 194 onoffsignal_biceps=-1;
FloorC 59:1725a3f02f37 195 }
daniQQue 40:187ef29de53d 196
FloorC 59:1725a3f02f37 197 else{
FloorC 59:1725a3f02f37 198 onoffsignal_biceps=0;
FloorC 59:1725a3f02f37 199 }
daniQQue 7:42d0e38196f1 200
daniQQue 40:187ef29de53d 201 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 202
FloorC 59:1725a3f02f37 203 if (bicepstriceps_rightarm<treshold_triceps){
FloorC 59:1725a3f02f37 204 switch_signal_triceps=1;
FloorC 59:1725a3f02f37 205 }
daniQQue 40:187ef29de53d 206
FloorC 59:1725a3f02f37 207 else{
FloorC 59:1725a3f02f37 208 switch_signal_triceps=0;
FloorC 59:1725a3f02f37 209 }
daniQQue 49:818a0e90ed9c 210
daniQQue 49:818a0e90ed9c 211 //send signals to scope to monitor the EMG signals
FloorC 59:1725a3f02f37 212 scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
FloorC 59:1725a3f02f37 213 scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
FloorC 59:1725a3f02f37 214 scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
FloorC 59:1725a3f02f37 215 scope.set(3, bicepstriceps_rightarm); // set on/off signal for the motors to scope in channel 3
FloorC 59:1725a3f02f37 216 scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
daniQQue 40:187ef29de53d 217
FloorC 59:1725a3f02f37 218 scope.send(); //send all the signals to the scope
FloorC 59:1725a3f02f37 219 }
daniQQue 57:c546edf67c5c 220 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 221
daniQQue 57:c546edf67c5c 222 //reference void makes the reference that the controllor should follow. There is only a controller for motor 1.
FloorC 59:1725a3f02f37 223 void reference(){
FloorC 59:1725a3f02f37 224 if (start_motor == true){ //bool that is true when the motor starts turning
FloorC 59:1725a3f02f37 225 timer.start(); //timer that starts counting in milliseconds
sivuu 50:2c03357de7cc 226 }
FloorC 59:1725a3f02f37 227 if (onoffsignal_biceps==-1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
FloorC 59:1725a3f02f37 228 t_start = timer.read_ms(); //read the current time passed from the timer
FloorC 59:1725a3f02f37 229 start_motor = false; //it means that the motor is not running or has started up
FloorC 55:d742332ced11 230
FloorC 59:1725a3f02f37 231 if (t_start < 1.0){ //the time passed is less than one second
FloorC 59:1725a3f02f37 232 w_var = t_start*1.5; //the reference velocity is the time passed multiplied with the eventual constant velocity it should reach
FloorC 59:1725a3f02f37 233 }
FloorC 55:d742332ced11 234
FloorC 59:1725a3f02f37 235 else{
FloorC 59:1725a3f02f37 236 w_var = 1.5; //if the time passed is more than one second, the velocity is constant
FloorC 59:1725a3f02f37 237 }
FloorC 59:1725a3f02f37 238
FloorC 59:1725a3f02f37 239 d_ref = d_ref + w_var * Ts; //makes the reference angle
FloorC 59:1725a3f02f37 240
FloorC 59:1725a3f02f37 241 }
FloorC 59:1725a3f02f37 242 if (d_ref > 12){ //set the restrictions
FloorC 59:1725a3f02f37 243 d_ref = 12;
FloorC 59:1725a3f02f37 244 start_motor = true; //after the restriction is reached the motor (if turned the other way) will start up again so the bool has to be set to true
FloorC 55:d742332ced11 245 }
FloorC 55:d742332ced11 246
sivuu 50:2c03357de7cc 247 else{
FloorC 59:1725a3f02f37 248 d_ref = d_ref; //if there is no signal, the referance angle is constant
sivuu 50:2c03357de7cc 249 }
sivuu 50:2c03357de7cc 250
FloorC 59:1725a3f02f37 251 if (onoffsignal_biceps==1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
FloorC 55:d742332ced11 252 t_start = timer.read_ms();
FloorC 55:d742332ced11 253 start_motor = false;
FloorC 55:d742332ced11 254
FloorC 55:d742332ced11 255 if (t_start < 1.0){
FloorC 55:d742332ced11 256 w_var = t_start*1.5;
FloorC 59:1725a3f02f37 257 }
FloorC 59:1725a3f02f37 258
FloorC 55:d742332ced11 259 else {
FloorC 55:d742332ced11 260 w_var = 1.5;
FloorC 59:1725a3f02f37 261 }
FloorC 59:1725a3f02f37 262 d_ref = d_ref - w_var * Ts; //the motor should turn the other way now so the reference becomes negative
sivuu 50:2c03357de7cc 263 }
FloorC 59:1725a3f02f37 264
FloorC 59:1725a3f02f37 265 if (d_ref < -12){ //negative restriction
FloorC 59:1725a3f02f37 266 d_ref = -12;
FloorC 59:1725a3f02f37 267 start_motor = true;
FloorC 55:d742332ced11 268 }
FloorC 59:1725a3f02f37 269
sivuu 50:2c03357de7cc 270 else{
sivuu 50:2c03357de7cc 271 d_ref = d_ref;
sivuu 50:2c03357de7cc 272 }
sivuu 50:2c03357de7cc 273
sivuu 50:2c03357de7cc 274 }
daniQQue 57:c546edf67c5c 275 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 276 //This void calculates the error and makes the control output.
sivuu 50:2c03357de7cc 277 void m1_controller(){
FloorC 59:1725a3f02f37 278 error1 = d_ref-rev_counts_motor1_rad; //calculate the error = reference-output
FloorC 59:1725a3f02f37 279 controlOutput = PID_controller.step(error1); //give the error as input to the controller
sivuu 50:2c03357de7cc 280 }
daniQQue 57:c546edf67c5c 281 //=======================================================================================================================================================
sivuu 50:2c03357de7cc 282
daniQQue 57:c546edf67c5c 283 //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 284 void encoders(){
FloorC 55:d742332ced11 285 counts_encoder1 = Encoder1.getPulses();
FloorC 55:d742332ced11 286 rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
FloorC 59:1725a3f02f37 287 rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; //calculate the angle in radians
FloorC 55:d742332ced11 288 }
FloorC 55:d742332ced11 289
daniQQue 57:c546edf67c5c 290 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 291
daniQQue 57:c546edf67c5c 292 //The calibration of the Biceps threshold is started by a button.
daniQQue 57:c546edf67c5c 293 //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
daniQQue 57:c546edf67c5c 294 void calibration_biceps(){
daniQQue 57:c546edf67c5c 295 if (button_calibration_biceps==0){ //only runs when button is pressed
daniQQue 57:c546edf67c5c 296
FloorC 59:1725a3f02f37 297 //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
FloorC 59:1725a3f02f37 298 ticker_switch.detach();
FloorC 59:1725a3f02f37 299 sample_timer.detach();
FloorC 59:1725a3f02f37 300
FloorC 59:1725a3f02f37 301 //let the user know what is happening, blue led on: calibration is going.
FloorC 59:1725a3f02f37 302 pc.printf("start of calibration biceps, contract maximal \r\n");
FloorC 59:1725a3f02f37 303 pc.printf("\r\n");
FloorC 59:1725a3f02f37 304 red=1;
FloorC 59:1725a3f02f37 305 green=1;
FloorC 59:1725a3f02f37 306 blue=0;
daniQQue 57:c546edf67c5c 307
daniQQue 57:c546edf67c5c 308 //start callibration of biceps
FloorC 59:1725a3f02f37 309 for(int n =0; n<1500;n++){ //read for 1500 samples as calibration
FloorC 59:1725a3f02f37 310
FloorC 59:1725a3f02f37 311 //biceps right arm read+filtering
FloorC 59:1725a3f02f37 312 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
FloorC 59:1725a3f02f37 313 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
FloorC 59:1725a3f02f37 314 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
FloorC 59:1725a3f02f37 315 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
FloorC 59:1725a3f02f37 316 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
FloorC 59:1725a3f02f37 317
FloorC 59:1725a3f02f37 318 //triceps right arm read+filtering
FloorC 59:1725a3f02f37 319 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
FloorC 59:1725a3f02f37 320 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
FloorC 59:1725a3f02f37 321 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
FloorC 59:1725a3f02f37 322 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
FloorC 59:1725a3f02f37 323 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
FloorC 59:1725a3f02f37 324
FloorC 59:1725a3f02f37 325 //biceps is +, triceps is -
FloorC 59:1725a3f02f37 326 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 57:c546edf67c5c 327
FloorC 59:1725a3f02f37 328 if (bicepstriceps_rightarm > max_biceps){ //determine what the highest reachable emg signal is
FloorC 59:1725a3f02f37 329
FloorC 59:1725a3f02f37 330 max_biceps = bicepstriceps_rightarm;
daniQQue 57:c546edf67c5c 331
FloorC 59:1725a3f02f37 332 }
daniQQue 57:c546edf67c5c 333 wait(0.001f); //to sample at same freq; 1000Hz
FloorC 59:1725a3f02f37 334 }
FloorC 59:1725a3f02f37 335
daniQQue 57:c546edf67c5c 336 treshold_biceps_right=percentage_max_biceps*max_biceps; //determine new treshold, right biceps is +
daniQQue 57:c546edf67c5c 337 treshold_biceps_left=-treshold_biceps_right; //determine new treshold, right biceps is -
daniQQue 57:c546edf67c5c 338
daniQQue 57:c546edf67c5c 339 //toggle lights to see the calibration is done. Also show in putty that the calibration is done.
daniQQue 57:c546edf67c5c 340 blue=!blue;
daniQQue 57:c546edf67c5c 341
daniQQue 57:c546edf67c5c 342 pc.printf(" end of calibration\r\n",treshold_biceps_right );
daniQQue 57:c546edf67c5c 343 pc.printf(" change of cv biceps: %f ",treshold_biceps_right );
daniQQue 57:c546edf67c5c 344
daniQQue 57:c546edf67c5c 345 wait(0.2f);
daniQQue 57:c546edf67c5c 346
daniQQue 57:c546edf67c5c 347 //remind the person of what motor will go on an which direction
FloorC 59:1725a3f02f37 348 if (switch_signal%2==0){
FloorC 59:1725a3f02f37 349 green=0;
FloorC 59:1725a3f02f37 350 red=1;
FloorC 59:1725a3f02f37 351 }
daniQQue 57:c546edf67c5c 352
FloorC 59:1725a3f02f37 353 else{
FloorC 59:1725a3f02f37 354 green=1;
FloorC 59:1725a3f02f37 355 red=0;
daniQQue 57:c546edf67c5c 356 }
FloorC 59:1725a3f02f37 357 }
FloorC 59:1725a3f02f37 358 //reattach the functions to the tickers that were detached.
FloorC 59:1725a3f02f37 359 ticker_switch.attach(&switch_function,1.0);
FloorC 59:1725a3f02f37 360 sample_timer.attach(&filter, 0.001);
FloorC 59:1725a3f02f37 361 }
daniQQue 57:c546edf67c5c 362 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 363
FloorC 59:1725a3f02f37 364 //The calibration of the triceps threshold is started by a button.
daniQQue 57:c546edf67c5c 365 //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
daniQQue 57:c546edf67c5c 366 void calibration_triceps(){
daniQQue 57:c546edf67c5c 367 if(button_calibration_triceps==0){ //only runs when button is pressed
daniQQue 57:c546edf67c5c 368
FloorC 59:1725a3f02f37 369 //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
FloorC 59:1725a3f02f37 370 ticker_switch.detach();
FloorC 59:1725a3f02f37 371 sample_timer.detach();
FloorC 59:1725a3f02f37 372
FloorC 59:1725a3f02f37 373 //toggel LEDS and let the user know that callibration of triceps is starting.
FloorC 59:1725a3f02f37 374 red=1;
FloorC 59:1725a3f02f37 375 green=1;
FloorC 59:1725a3f02f37 376 blue=0;
FloorC 59:1725a3f02f37 377
FloorC 59:1725a3f02f37 378 pc.printf("start of calibration triceps\r\n");
FloorC 59:1725a3f02f37 379 pc.printf("\r\n");
daniQQue 57:c546edf67c5c 380
FloorC 59:1725a3f02f37 381 //start calibration of triceps
FloorC 59:1725a3f02f37 382 for(int n =0; n<1500;n++){ //read for 2000 samples as calibration
FloorC 59:1725a3f02f37 383
FloorC 59:1725a3f02f37 384 //biceps right arm read+filtering
FloorC 59:1725a3f02f37 385 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
FloorC 59:1725a3f02f37 386 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
FloorC 59:1725a3f02f37 387 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
FloorC 59:1725a3f02f37 388 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
FloorC 59:1725a3f02f37 389 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
FloorC 59:1725a3f02f37 390
FloorC 59:1725a3f02f37 391 //triceps right arm read+filtering
FloorC 59:1725a3f02f37 392 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
FloorC 59:1725a3f02f37 393 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
FloorC 59:1725a3f02f37 394 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
FloorC 59:1725a3f02f37 395 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
FloorC 59:1725a3f02f37 396 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
FloorC 59:1725a3f02f37 397
FloorC 59:1725a3f02f37 398 //biceps is +, triceps is -
FloorC 59:1725a3f02f37 399 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
FloorC 59:1725a3f02f37 400
FloorC 59:1725a3f02f37 401 if (bicepstriceps_rightarm < max_triceps){ //determine what the lowest reachable emg of triceps (max in negative part) signal is
FloorC 59:1725a3f02f37 402
FloorC 59:1725a3f02f37 403 max_triceps = bicepstriceps_rightarm;
FloorC 59:1725a3f02f37 404
FloorC 59:1725a3f02f37 405 }
FloorC 59:1725a3f02f37 406 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 57:c546edf67c5c 407 }
daniQQue 57:c546edf67c5c 408 treshold_triceps=percentage_max_triceps*max_triceps; //calculate the new treshold. This is a negative number due to the sum!
FloorC 59:1725a3f02f37 409
daniQQue 57:c546edf67c5c 410 //Let the user know that the calibration is done.
daniQQue 57:c546edf67c5c 411 pc.printf(" end of calibration\r\n");
daniQQue 57:c546edf67c5c 412 pc.printf(" change of cv triceps: %f ",treshold_triceps );
daniQQue 57:c546edf67c5c 413 blue=!blue;
daniQQue 57:c546edf67c5c 414 wait(0.2f);
FloorC 59:1725a3f02f37 415 if (switch_signal%2==0){
FloorC 59:1725a3f02f37 416 green=0;
FloorC 59:1725a3f02f37 417 red=1;
daniQQue 57:c546edf67c5c 418 }
FloorC 59:1725a3f02f37 419
FloorC 59:1725a3f02f37 420 else{
FloorC 59:1725a3f02f37 421 green=1;
FloorC 59:1725a3f02f37 422 red=0;
FloorC 59:1725a3f02f37 423 }
FloorC 59:1725a3f02f37 424 }
daniQQue 57:c546edf67c5c 425
FloorC 59:1725a3f02f37 426 //reattach the functions to the tickers that were detached.
FloorC 59:1725a3f02f37 427 sample_timer.attach(&filter, 0.001);
FloorC 59:1725a3f02f37 428 ticker_switch.attach(&switch_function,1.0);
FloorC 59:1725a3f02f37 429 }
daniQQue 57:c546edf67c5c 430 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 431
daniQQue 57:c546edf67c5c 432 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 433 //program
daniQQue 57:c546edf67c5c 434 //=======================================================================================================================================================
FloorC 59:1725a3f02f37 435 int main(){
daniQQue 49:818a0e90ed9c 436
FloorC 59:1725a3f02f37 437 pc.baud(115200); //connect with pc with baudrate 115200
FloorC 59:1725a3f02f37 438 green=1; //led is off (1), at beginning
FloorC 59:1725a3f02f37 439 blue=1; //led is off (1), at beginning
FloorC 59:1725a3f02f37 440 red=0; //led is on (0), at beginning
FloorC 59:1725a3f02f37 441
FloorC 59:1725a3f02f37 442 //attach tickers to functions
FloorC 59:1725a3f02f37 443 sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
FloorC 59:1725a3f02f37 444 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.
FloorC 59:1725a3f02f37 445 ticker_referenceangle.attach(&reference, Ts);
FloorC 59:1725a3f02f37 446 ticker_controllerm1.attach(&m1_controller, Ts);
FloorC 59:1725a3f02f37 447 ticker_encoder.attach(&encoders, Ts);
FloorC 59:1725a3f02f37 448 ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop EMG sampling and switch
FloorC 59:1725a3f02f37 449 ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop EMG sampling and switch
FloorC 59:1725a3f02f37 450
FloorC 59:1725a3f02f37 451 //PID controller
FloorC 59:1725a3f02f37 452 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
FloorC 59:1725a3f02f37 453
FloorC 59:1725a3f02f37 454 //Encoder
FloorC 59:1725a3f02f37 455 //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
FloorC 59:1725a3f02f37 456
FloorC 59:1725a3f02f37 457 //Show the user what the starting motor will be and what will happen
FloorC 59:1725a3f02f37 458 pc.printf("We will start the demonstration\r\n");
FloorC 59:1725a3f02f37 459 pc.printf("\r\n\r\n\r\n");
daniQQue 44:c79e5944ac91 460
FloorC 59:1725a3f02f37 461 if (switch_signal%2==0){
FloorC 59:1725a3f02f37 462 pc.printf("If you contract the biceps, the robot will go right \r\n");
FloorC 59:1725a3f02f37 463 pc.printf("If you contract the triceps, the robot will go left \r\n");
FloorC 59:1725a3f02f37 464 pc.printf("\r\n");
FloorC 59:1725a3f02f37 465 }
daniQQue 47:ddaa59d48aca 466
daniQQue 47:ddaa59d48aca 467
FloorC 59:1725a3f02f37 468 else{
FloorC 59:1725a3f02f37 469 pc.printf("If you contract the biceps, the robot will go up \r\n");
FloorC 59:1725a3f02f37 470 pc.printf("If you contract the triceps, the robot will go down \r\n");
FloorC 59:1725a3f02f37 471 pc.printf("\r\n");
FloorC 59:1725a3f02f37 472 }
daniQQue 45:08bddea67bd8 473
daniQQue 57:c546edf67c5c 474 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 475 //endless loop
daniQQue 0:34c739fcc3e0 476
daniQQue 40:187ef29de53d 477
FloorC 59:1725a3f02f37 478 while (true) { //neverending loop
daniQQue 8:cd0cb71b69f2 479
FloorC 59:1725a3f02f37 480 if (onoffsignal_biceps==-1){ //left biceps contracted
FloorC 56:a38412383477 481
FloorC 59:1725a3f02f37 482 if (switch_signal%2==0){ //switch even
FloorC 56:a38412383477 483
FloorC 59:1725a3f02f37 484 speedmotor1=controlOutput; //output PID-controller is the speed for motor1
daniQQue 57:c546edf67c5c 485
FloorC 59:1725a3f02f37 486 if (speedmotor1<0){ //if the output of the controller is negative, the direction is clockwise
FloorC 59:1725a3f02f37 487 richting_motor1 = cw; //motor 1, right
sivuu 51:b344a92b6a5f 488 }
FloorC 59:1725a3f02f37 489 else { //if the output is positive, the direction is counterclockwise
FloorC 59:1725a3f02f37 490 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 491 }
FloorC 59:1725a3f02f37 492 pwm_motor1 = fabs(speedmotor1); //speed of motor 1, absolute because pwm cannot be negative
daniQQue 57:c546edf67c5c 493
FloorC 59:1725a3f02f37 494 }
sivuu 51:b344a92b6a5f 495
daniQQue 40:187ef29de53d 496
FloorC 59:1725a3f02f37 497 else{ //switch odd
FloorC 59:1725a3f02f37 498
FloorC 59:1725a3f02f37 499 richting_motor2 = ccw; //motor 2, up
FloorC 59:1725a3f02f37 500 pwm_motor2 = speedmotor2; //speed of motor 2
daniQQue 40:187ef29de53d 501
FloorC 59:1725a3f02f37 502 }
daniQQue 40:187ef29de53d 503
FloorC 59:1725a3f02f37 504 }
FloorC 59:1725a3f02f37 505 else if (onoffsignal_biceps==1){ //right biceps contracted
FloorC 59:1725a3f02f37 506
FloorC 59:1725a3f02f37 507 if (switch_signal%2==0){ //switch signal even
FloorC 59:1725a3f02f37 508 speedmotor1=controlOutput;
daniQQue 57:c546edf67c5c 509
FloorC 59:1725a3f02f37 510 if (speedmotor1<0){ //the same as for the left biceps, the robot turns in the right direction because of the reference signal
FloorC 59:1725a3f02f37 511 richting_motor1 = cw; //motor 1, right
sivuu 51:b344a92b6a5f 512 }
FloorC 59:1725a3f02f37 513 else {
FloorC 59:1725a3f02f37 514 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 515 }
sivuu 51:b344a92b6a5f 516 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
daniQQue 57:c546edf67c5c 517
FloorC 59:1725a3f02f37 518 }
FloorC 59:1725a3f02f37 519 else{ //switch signal odd
FloorC 59:1725a3f02f37 520 richting_motor2 = cw; //motor 2, down
FloorC 59:1725a3f02f37 521 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 522
FloorC 59:1725a3f02f37 523 }
FloorC 59:1725a3f02f37 524 }
FloorC 59:1725a3f02f37 525 else{
FloorC 59:1725a3f02f37 526 //no contraction of biceps, thus no motoraction.
FloorC 59:1725a3f02f37 527 pwm_motor2=0;
FloorC 59:1725a3f02f37 528 pwm_motor1=0;
FloorC 59:1725a3f02f37 529 start_motor = true; //every time the motor is off, the bool is reset so that the reference void can start when the motor starts
FloorC 56:a38412383477 530
FloorC 59:1725a3f02f37 531 }
daniQQue 40:187ef29de53d 532
FloorC 59:1725a3f02f37 533 } //while true closed
daniQQue 0:34c739fcc3e0 534
daniQQue 49:818a0e90ed9c 535 } //int main closed
daniQQue 49:818a0e90ed9c 536
daniQQue 57:c546edf67c5c 537 //=======================================================================================================================================================