Daniqe Kottelenberg / Mbed 2 deprecated a_pid_kal_end_def

Wil je hier nog je PID controler kort uitleggen plus waarden aanpassen?

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of a_pid_kal_end by Daniqe Kottelenberg

main.cpp@56:a38412383477, 2016-11-03 (annotated)

Committer:
FloorC
Date:
Thu Nov 03 16:41:10 2016 +0000
Revision:
56:a38412383477
Parent:
55:d742332ced11
Child:
57:c546edf67c5c
DEMO_VERSION!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 46:4a8889f9dc9f 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 46:4a8889f9dc9f 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
FloorC 55:d742332ced11 23 Ticker ticker_encoder;
FloorC 55:d742332ced11 24
FloorC 55:d742332ced11 25 //Timer
FloorC 55:d742332ced11 26 Timer timer;
daniQQue 49:818a0e90ed9c 27
daniQQue 49:818a0e90ed9c 28 //Monitoring
daniQQue 46:4a8889f9dc9f 29 HIDScope scope(5); //open 5 channels in hidscope
daniQQue 46:4a8889f9dc9f 30 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 49:818a0e90ed9c 31 DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
daniQQue 49:818a0e90ed9c 32 DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
daniQQue 40:187ef29de53d 33
daniQQue 40:187ef29de53d 34 //motors
sivuu 50:2c03357de7cc 35 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 36 PwmOut pwm_motor1(D6);
sivuu 50:2c03357de7cc 37 DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator
daniQQue 47:ddaa59d48aca 38 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 39
sivuu 50:2c03357de7cc 40 //encoders
sivuu 50:2c03357de7cc 41 DigitalIn encoder1A(D13);
sivuu 50:2c03357de7cc 42 DigitalIn encoder1B(D12);
sivuu 50:2c03357de7cc 43 DigitalIn encoder2A(D11);
sivuu 50:2c03357de7cc 44 DigitalIn encoder2B(D10);
sivuu 50:2c03357de7cc 45
sivuu 50:2c03357de7cc 46 //controller
sivuu 50:2c03357de7cc 47 BiQuad PID_controller;
sivuu 50:2c03357de7cc 48
sivuu 50:2c03357de7cc 49
daniQQue 46:4a8889f9dc9f 50 //=====================================================================================
daniQQue 0:34c739fcc3e0 51 //define variables
daniQQue 47:ddaa59d48aca 52
sivuu 50:2c03357de7cc 53 //thresholds
sivuu 50:2c03357de7cc 54 double treshold_biceps_right = 0.04; //common values that work.
sivuu 52:0135deb3b07f 55 double treshold_biceps_left = -0.04; // tested on multiple persons
sivuu 52:0135deb3b07f 56 double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold
daniQQue 49:818a0e90ed9c 57
daniQQue 49:818a0e90ed9c 58 //on/off and switch signals
daniQQue 49:818a0e90ed9c 59 int switch_signal = 0; //start of counter, switch made by even and odd numbers
daniQQue 49:818a0e90ed9c 60 int onoffsignal_biceps;
daniQQue 45:08bddea67bd8 61 int switch_signal_triceps;
daniQQue 46:4a8889f9dc9f 62
daniQQue 49:818a0e90ed9c 63 //motorvariables
daniQQue 49:818a0e90ed9c 64 float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
daniQQue 49:818a0e90ed9c 65 float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
daniQQue 46:4a8889f9dc9f 66
daniQQue 49:818a0e90ed9c 67 int cw=0; //clockwise direction
daniQQue 49:818a0e90ed9c 68 int ccw=1; //counterclockwise direction
sivuu 50:2c03357de7cc 69
sivuu 50:2c03357de7cc 70 //encoder
sivuu 50:2c03357de7cc 71 int counts_encoder1;
sivuu 50:2c03357de7cc 72 //int counts_encoder2;
sivuu 50:2c03357de7cc 73 float rev_counts_motor1;
sivuu 50:2c03357de7cc 74 float rev_counts_motor1_rad;
sivuu 51:b344a92b6a5f 75 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 51:b344a92b6a5f 76 const float rev_rond=64.0; // aantal revoluties per omgang van de encoder
FloorC 55:d742332ced11 77 QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING);
sivuu 50:2c03357de7cc 78
sivuu 50:2c03357de7cc 79 //reference
sivuu 50:2c03357de7cc 80 volatile float d_ref = 0;
FloorC 55:d742332ced11 81 const float w_ref = 1.5;
FloorC 55:d742332ced11 82 volatile double t_start;
FloorC 55:d742332ced11 83 volatile double w_var;
sivuu 50:2c03357de7cc 84 const double Ts = 0.001;
sivuu 50:2c03357de7cc 85
sivuu 50:2c03357de7cc 86 //controller
sivuu 50:2c03357de7cc 87 const double Kp = 0.3823;
sivuu 50:2c03357de7cc 88 const double Ki = 0.1279;
sivuu 50:2c03357de7cc 89 const double Kd = 0.2519;
sivuu 50:2c03357de7cc 90 const double N = 100;
sivuu 50:2c03357de7cc 91 volatile double error1;
sivuu 50:2c03357de7cc 92 volatile double controlOutput;
FloorC 55:d742332ced11 93 bool start_motor = true;
FloorC 55:d742332ced11 94 volatile double starttime;
daniQQue 44:c79e5944ac91 95 //=======================================
daniQQue 44:c79e5944ac91 96 //filter coefficients
daniQQue 40:187ef29de53d 97
daniQQue 40:187ef29de53d 98 //b1 = biceps right arm
daniQQue 49:818a0e90ed9c 99 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?
daniQQue 49:818a0e90ed9c 100 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?
daniQQue 40:187ef29de53d 101
daniQQue 40:187ef29de53d 102 //t1= triceps right arm
daniQQue 49:818a0e90ed9c 103 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?
daniQQue 49:818a0e90ed9c 104 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?
daniQQue 5:688b1b5530d8 105
daniQQue 40:187ef29de53d 106 //b2= biceps left arm
daniQQue 49:818a0e90ed9c 107 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?
daniQQue 49:818a0e90ed9c 108 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?
daniQQue 40:187ef29de53d 109
daniQQue 40:187ef29de53d 110 //after abs filtering
daniQQue 49:818a0e90ed9c 111 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?
daniQQue 49:818a0e90ed9c 112 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?
daniQQue 49:818a0e90ed9c 113 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?
daniQQue 10:7255b59224cc 114
daniQQue 45:08bddea67bd8 115 //======================================================================
daniQQue 45:08bddea67bd8 116 //voids
daniQQue 45:08bddea67bd8 117 //======================================================================
daniQQue 45:08bddea67bd8 118
daniQQue 40:187ef29de53d 119 //function teller
daniQQue 49:818a0e90ed9c 120 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
daniQQue 45:08bddea67bd8 121 if(switch_signal_triceps==1)
daniQQue 40:187ef29de53d 122 {
daniQQue 45:08bddea67bd8 123 switch_signal++;
daniQQue 49:818a0e90ed9c 124
daniQQue 49:818a0e90ed9c 125 // 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 126
daniQQue 40:187ef29de53d 127 green=!green;
daniQQue 40:187ef29de53d 128 red=!red;
daniQQue 47:ddaa59d48aca 129
daniQQue 45:08bddea67bd8 130 if (switch_signal%2==0)
daniQQue 49:818a0e90ed9c 131 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 132 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 133 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 134 }
daniQQue 47:ddaa59d48aca 135
daniQQue 47:ddaa59d48aca 136
daniQQue 47:ddaa59d48aca 137 else
daniQQue 40:187ef29de53d 138 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 139 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 140 pc.printf("\r\n");
daniQQue 40:187ef29de53d 141 }
daniQQue 47:ddaa59d48aca 142
daniQQue 40:187ef29de53d 143 }
daniQQue 40:187ef29de53d 144 }
daniQQue 49:818a0e90ed9c 145
daniQQue 49:818a0e90ed9c 146 //======================================================================
daniQQue 49:818a0e90ed9c 147 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 40:187ef29de53d 148
daniQQue 0:34c739fcc3e0 149 void filter(){
daniQQue 40:187ef29de53d 150 //biceps right arm read+filtering
daniQQue 49:818a0e90ed9c 151 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 152 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 153 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 154 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 155 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 156
daniQQue 40:187ef29de53d 157 //triceps right arm read+filtering
daniQQue 49:818a0e90ed9c 158 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 159 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 160 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 161 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 162 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 7:42d0e38196f1 163
daniQQue 40:187ef29de53d 164 //biceps left arm read+filtering
daniQQue 49:818a0e90ed9c 165 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 166 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 167 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 168 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 169 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 170
daniQQue 40:187ef29de53d 171 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 172 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 173 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 174 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 175
daniQQue 44:c79e5944ac91 176 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 49:818a0e90ed9c 177 if (signal_biceps_sum>treshold_biceps_right)
daniQQue 49:818a0e90ed9c 178 {onoffsignal_biceps=1;}
daniQQue 7:42d0e38196f1 179
daniQQue 49:818a0e90ed9c 180 else if (signal_biceps_sum<treshold_biceps_left)
daniQQue 49:818a0e90ed9c 181 { onoffsignal_biceps=-1; }
daniQQue 40:187ef29de53d 182
daniQQue 49:818a0e90ed9c 183 else
daniQQue 49:818a0e90ed9c 184 {onoffsignal_biceps=0;}
daniQQue 7:42d0e38196f1 185
daniQQue 40:187ef29de53d 186 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 187
daniQQue 49:818a0e90ed9c 188 if (bicepstriceps_rightarm<treshold_triceps)
daniQQue 49:818a0e90ed9c 189 { switch_signal_triceps=1; }
daniQQue 40:187ef29de53d 190
daniQQue 40:187ef29de53d 191 else
daniQQue 49:818a0e90ed9c 192 { switch_signal_triceps=0; }
daniQQue 49:818a0e90ed9c 193
daniQQue 49:818a0e90ed9c 194 //send signals to scope to monitor the EMG signals
daniQQue 49:818a0e90ed9c 195 scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
daniQQue 49:818a0e90ed9c 196 scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
daniQQue 49:818a0e90ed9c 197 scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
daniQQue 49:818a0e90ed9c 198 scope.set(3, onoffsignal_biceps); // set on/off signal for the motors to scope in channel 3
daniQQue 49:818a0e90ed9c 199 scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
daniQQue 40:187ef29de53d 200
daniQQue 40:187ef29de53d 201 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 202 }
sivuu 50:2c03357de7cc 203
sivuu 50:2c03357de7cc 204 void reference(){
FloorC 55:d742332ced11 205 if (start_motor == true){
FloorC 55:d742332ced11 206 timer.start();
sivuu 50:2c03357de7cc 207 }
FloorC 55:d742332ced11 208 if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even
FloorC 55:d742332ced11 209 t_start = timer.read_ms();
FloorC 55:d742332ced11 210 start_motor = false;
FloorC 55:d742332ced11 211
FloorC 55:d742332ced11 212 if (t_start < 1.0){
FloorC 55:d742332ced11 213 w_var = t_start*1.5;
FloorC 55:d742332ced11 214 }
FloorC 55:d742332ced11 215
FloorC 55:d742332ced11 216 else {
FloorC 55:d742332ced11 217 w_var = 1.5;
FloorC 55:d742332ced11 218 }
FloorC 55:d742332ced11 219
FloorC 55:d742332ced11 220 d_ref = d_ref + w_var * Ts;
FloorC 55:d742332ced11 221
FloorC 55:d742332ced11 222 }
FloorC 55:d742332ced11 223 if (d_ref > 12){
FloorC 55:d742332ced11 224 d_ref = 12;
FloorC 55:d742332ced11 225 start_motor = true;
sivuu 50:2c03357de7cc 226 //d_ref_const_cw = 1;
FloorC 55:d742332ced11 227 }
sivuu 50:2c03357de7cc 228 else{
sivuu 50:2c03357de7cc 229 d_ref = d_ref;
sivuu 50:2c03357de7cc 230 }
sivuu 50:2c03357de7cc 231
sivuu 52:0135deb3b07f 232 if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even //left biceps contracted{
FloorC 55:d742332ced11 233 t_start = timer.read_ms();
FloorC 55:d742332ced11 234 start_motor = false;
FloorC 55:d742332ced11 235
FloorC 55:d742332ced11 236 if (t_start < 1.0){
FloorC 55:d742332ced11 237 w_var = t_start*1.5;
FloorC 55:d742332ced11 238 }
FloorC 55:d742332ced11 239 else {
FloorC 55:d742332ced11 240 w_var = 1.5;
FloorC 55:d742332ced11 241 }
FloorC 55:d742332ced11 242 d_ref = d_ref - w_var * Ts;
sivuu 50:2c03357de7cc 243
sivuu 50:2c03357de7cc 244 }
FloorC 55:d742332ced11 245 if (d_ref < -12){
FloorC 55:d742332ced11 246 d_ref = -12;
FloorC 55:d742332ced11 247 start_motor = true;
FloorC 55:d742332ced11 248 }
sivuu 50:2c03357de7cc 249 else{
sivuu 50:2c03357de7cc 250 d_ref = d_ref;
sivuu 50:2c03357de7cc 251 }
sivuu 50:2c03357de7cc 252
sivuu 50:2c03357de7cc 253 }
sivuu 50:2c03357de7cc 254
sivuu 50:2c03357de7cc 255 void m1_controller(){
sivuu 50:2c03357de7cc 256 error1 = d_ref-rev_counts_motor1_rad;
sivuu 51:b344a92b6a5f 257 controlOutput = PID_controller.step(error1);
sivuu 50:2c03357de7cc 258 }
sivuu 50:2c03357de7cc 259
FloorC 55:d742332ced11 260 void encoders(){
FloorC 55:d742332ced11 261 counts_encoder1 = Encoder1.getPulses();
FloorC 55:d742332ced11 262 rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
FloorC 55:d742332ced11 263 rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
FloorC 55:d742332ced11 264
FloorC 55:d742332ced11 265 }
FloorC 55:d742332ced11 266
daniQQue 49:818a0e90ed9c 267 //======================================================================
daniQQue 0:34c739fcc3e0 268 //program
daniQQue 49:818a0e90ed9c 269 //======================================================================
daniQQue 0:34c739fcc3e0 270 int main()
daniQQue 0:34c739fcc3e0 271 {
daniQQue 40:187ef29de53d 272
daniQQue 49:818a0e90ed9c 273 pc.baud(115200); //connect with pc with baudrate 115200
daniQQue 49:818a0e90ed9c 274 green=1; //led is off (1), at beginning
daniQQue 49:818a0e90ed9c 275 red=0; //led is on (0), at beginning
daniQQue 49:818a0e90ed9c 276
daniQQue 49:818a0e90ed9c 277 //attach tickers to functions
sivuu 50:2c03357de7cc 278 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 45:08bddea67bd8 279 ticker_switch.attach(&switch_function,1.0);
sivuu 50:2c03357de7cc 280 ticker_referenceangle.attach(&reference, Ts);
sivuu 50:2c03357de7cc 281 ticker_controllerm1.attach(&m1_controller, Ts);
FloorC 55:d742332ced11 282 ticker_encoder.attach(&encoders, Ts);
sivuu 50:2c03357de7cc 283
sivuu 50:2c03357de7cc 284 //PID controller
sivuu 50:2c03357de7cc 285 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
daniQQue 44:c79e5944ac91 286
sivuu 51:b344a92b6a5f 287 //Encoder
FloorC 55:d742332ced11 288 //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
sivuu 51:b344a92b6a5f 289
daniQQue 49:818a0e90ed9c 290 //Show the user what the starting motor will be and what will happen
daniQQue 47:ddaa59d48aca 291 pc.printf("We will start the demonstration\r\n");
daniQQue 49:818a0e90ed9c 292 pc.printf("\r\n\r\n\r\n");
daniQQue 49:818a0e90ed9c 293
daniQQue 47:ddaa59d48aca 294 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 295 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 296 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 297 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 298 }
daniQQue 47:ddaa59d48aca 299
daniQQue 47:ddaa59d48aca 300
daniQQue 47:ddaa59d48aca 301 else
daniQQue 45:08bddea67bd8 302 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 45:08bddea67bd8 303 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 304 pc.printf("\r\n");
daniQQue 45:08bddea67bd8 305 }
daniQQue 45:08bddea67bd8 306
daniQQue 44:c79e5944ac91 307 //==============================================================================================
daniQQue 0:34c739fcc3e0 308 //endless loop
daniQQue 0:34c739fcc3e0 309
daniQQue 40:187ef29de53d 310
daniQQue 49:818a0e90ed9c 311 while (true) { // neverending loop
daniQQue 8:cd0cb71b69f2 312
FloorC 56:a38412383477 313 /*counts_encoder1 = Encoder1.getPulses();
sivuu 51:b344a92b6a5f 314 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
FloorC 56:a38412383477 315 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; */
sivuu 52:0135deb3b07f 316
FloorC 56:a38412383477 317 pc.printf("%f %f \r \n", d_ref, rev_counts_motor1_rad);
FloorC 55:d742332ced11 318 //pc.printf("%f ", rev_counts_motor1_rad);
FloorC 55:d742332ced11 319 //pc.printf("%f",w_var);
FloorC 56:a38412383477 320 //pc.printf("%d\n",start_motor);
FloorC 55:d742332ced11 321
sivuu 51:b344a92b6a5f 322
FloorC 56:a38412383477 323 if (onoffsignal_biceps==-1){ //left biceps contracted
FloorC 56:a38412383477 324
FloorC 56:a38412383477 325 if (switch_signal%2==0){ //switch even
FloorC 56:a38412383477 326
sivuu 51:b344a92b6a5f 327 speedmotor1=controlOutput;
sivuu 51:b344a92b6a5f 328 //richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 329 //pwm_motor1 = speedmotor1; //speed of motor 1
sivuu 51:b344a92b6a5f 330 if (speedmotor1<0){
sivuu 52:0135deb3b07f 331 richting_motor1 = cw;
sivuu 51:b344a92b6a5f 332 }
sivuu 51:b344a92b6a5f 333 else {
sivuu 52:0135deb3b07f 334 richting_motor1 = ccw;
sivuu 51:b344a92b6a5f 335 }
sivuu 51:b344a92b6a5f 336 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
sivuu 52:0135deb3b07f 337 // pc.printf("%f\r\n", pwm_motor1.read());
daniQQue 40:187ef29de53d 338 }
sivuu 51:b344a92b6a5f 339
daniQQue 40:187ef29de53d 340
daniQQue 49:818a0e90ed9c 341 else //switch odd
daniQQue 40:187ef29de53d 342 {
daniQQue 49:818a0e90ed9c 343 richting_motor2 = ccw; //motor 2, up
daniQQue 49:818a0e90ed9c 344 pwm_motor2 = speedmotor2;//speed of motor 2
daniQQue 40:187ef29de53d 345
daniQQue 40:187ef29de53d 346 }
daniQQue 40:187ef29de53d 347
daniQQue 8:cd0cb71b69f2 348 }
sivuu 52:0135deb3b07f 349 else if (onoffsignal_biceps==1) //right biceps contracted
daniQQue 40:187ef29de53d 350 {
daniQQue 49:818a0e90ed9c 351 if (switch_signal%2==0) //switch signal even
daniQQue 40:187ef29de53d 352 {
sivuu 51:b344a92b6a5f 353 speedmotor1=controlOutput;
sivuu 51:b344a92b6a5f 354 //richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 355 //pwm_motor1 = speedmotor1; //speed of motor 1
sivuu 51:b344a92b6a5f 356 if (speedmotor1<0){
sivuu 52:0135deb3b07f 357 richting_motor1 = cw;
sivuu 51:b344a92b6a5f 358 }
sivuu 51:b344a92b6a5f 359 else {
sivuu 52:0135deb3b07f 360 richting_motor1 = ccw;
sivuu 51:b344a92b6a5f 361 }
sivuu 51:b344a92b6a5f 362 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
sivuu 52:0135deb3b07f 363 // pc.printf("%f\r\n", pwm_motor1.read());
daniQQue 40:187ef29de53d 364 }
daniQQue 49:818a0e90ed9c 365 else //switch signal odd
daniQQue 40:187ef29de53d 366 {
daniQQue 49:818a0e90ed9c 367 richting_motor2 = cw; //motor 2. down
daniQQue 49:818a0e90ed9c 368 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 369
daniQQue 40:187ef29de53d 370 }
daniQQue 40:187ef29de53d 371 }
daniQQue 49:818a0e90ed9c 372 else{
daniQQue 49:818a0e90ed9c 373 //no contraction of biceps
daniQQue 40:187ef29de53d 374 pwm_motor2=0;
daniQQue 40:187ef29de53d 375 pwm_motor1=0;
FloorC 55:d742332ced11 376 start_motor = true;
FloorC 56:a38412383477 377
FloorC 56:a38412383477 378
daniQQue 40:187ef29de53d 379 }
daniQQue 40:187ef29de53d 380
daniQQue 49:818a0e90ed9c 381 }//while true closed
daniQQue 0:34c739fcc3e0 382
daniQQue 49:818a0e90ed9c 383 } //int main closed
daniQQue 49:818a0e90ed9c 384
daniQQue 49:818a0e90ed9c 385 //=============================================================================================1