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@55:d742332ced11, 2016-11-03 (annotated)

Committer:
FloorC
Date:
Thu Nov 03 15:27:59 2016 +0000
Revision:
55:d742332ced11
Parent:
54:fb72c58a7150
Child:
56:a38412383477
links rechts pid controller zonder dat de fout op nul wordt gezet bij motor uit, WERKT GOED!

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
sivuu 51:b344a92b6a5f 313 counts_encoder1 = Encoder1.getPulses();
sivuu 51:b344a92b6a5f 314 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
sivuu 51:b344a92b6a5f 315 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 52:0135deb3b07f 316
sivuu 52:0135deb3b07f 317 pc.printf("%f \r\n", d_ref);
FloorC 55:d742332ced11 318 //pc.printf("%f ", rev_counts_motor1_rad);
FloorC 55:d742332ced11 319 //pc.printf("%f",w_var);
FloorC 55:d742332ced11 320 pc.printf("%d\n",start_motor);
FloorC 55:d742332ced11 321
sivuu 51:b344a92b6a5f 322
daniQQue 45:08bddea67bd8 323 if (onoffsignal_biceps==-1) //left biceps contracted
daniQQue 40:187ef29de53d 324 {
daniQQue 49:818a0e90ed9c 325 if (switch_signal%2==0) //switch even
daniQQue 40:187ef29de53d 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;
daniQQue 40:187ef29de53d 377 }
daniQQue 40:187ef29de53d 378
daniQQue 49:818a0e90ed9c 379 }//while true closed
daniQQue 0:34c739fcc3e0 380
daniQQue 49:818a0e90ed9c 381 } //int main closed
daniQQue 49:818a0e90ed9c 382
daniQQue 49:818a0e90ed9c 383 //=============================================================================================1