Oud verslag voor Biquad.
Dependencies: Biquad HIDScope QEI angleandposition controlandadjust mbed
Fork of includeair by
main.cpp@32:c940f6b6a6a0, 2015-10-28 (annotated)
- Committer:
- Gerth
- Date:
- Wed Oct 28 13:09:25 2015 +0000
- Revision:
- 32:c940f6b6a6a0
- Parent:
- 31:8646e853979f
- Child:
- 33:1c7b498ded25
ander verplaatsingsprofiel, met cosinus dingen;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Gerth | 0:dd66fff537d7 | 1 | #include "mbed.h" |
Gerth | 1:917c07a4f3ec | 2 | #include "QEI.h" |
Gerth | 1:917c07a4f3ec | 3 | #include "HIDScope.h" |
Gerth | 1:917c07a4f3ec | 4 | #include "Biquad.h" |
Gerth | 1:917c07a4f3ec | 5 | #include "controlandadjust.h" |
Gerth | 16:63320b8f79c2 | 6 | #include "angleandposition.h" |
Gerth | 1:917c07a4f3ec | 7 | |
Gerth | 20:c5fb2ff5d457 | 8 | ///////////////////////////////////////////////info out |
Gerth | 20:c5fb2ff5d457 | 9 | HIDScope scope(6);// number of hidscope channels |
Gerth | 25:21dcd3f9eac2 | 10 | const double scope_frequency=500; //HIDscope frequency |
Gerth | 25:21dcd3f9eac2 | 11 | Timer checktimer; |
Gerth | 25:21dcd3f9eac2 | 12 | volatile double checktimervalue=0; |
Gerth | 20:c5fb2ff5d457 | 13 | |
Gerth | 20:c5fb2ff5d457 | 14 | Serial pc(USBTX,USBRX);// serial connection to pc |
Gerth | 1:917c07a4f3ec | 15 | |
Gerth | 16:63320b8f79c2 | 16 | DigitalOut ledred(LED_RED); |
Gerth | 16:63320b8f79c2 | 17 | DigitalOut ledgreen(LED_GREEN); |
Gerth | 16:63320b8f79c2 | 18 | DigitalOut ledblue(LED_BLUE); |
Gerth | 14:4c4f45a1dd23 | 19 | |
Gerth | 20:c5fb2ff5d457 | 20 | Ticker scope_ticker; |
Gerth | 20:c5fb2ff5d457 | 21 | /////////////////////////////////////////////ENCODERS |
Gerth | 9:4ee354663560 | 22 | const float cpr_sensor=32; |
Gerth | 20:c5fb2ff5d457 | 23 | const float cpr_shaft=cpr_sensor*131;//counts per rotation of the sensor |
Gerth | 20:c5fb2ff5d457 | 24 | |
Gerth | 20:c5fb2ff5d457 | 25 | QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding |
Gerth | 9:4ee354663560 | 26 | QEI encoder2(D10,D11,NC,cpr_sensor); |
Gerth | 20:c5fb2ff5d457 | 27 | |
Gerth | 9:4ee354663560 | 28 | const double PIE=3.14159265359; |
Gerth | 20:c5fb2ff5d457 | 29 | const float counttorad=((2*PIE)/cpr_shaft);// counts per rotation of the shaft |
Gerth | 1:917c07a4f3ec | 30 | |
Gerth | 24:dd75c961ae88 | 31 | |
Gerth | 6:37c94a5e205f | 32 | /////////////////////////////////CALIBRATION (MODE) |
Gerth | 20:c5fb2ff5d457 | 33 | const double radpersec_calibrate=0.1*PIE;// speed of arms when in calibration mode |
Gerth | 20:c5fb2ff5d457 | 34 | int modecounter=1;//counter in which mode the robot is |
Gerth | 20:c5fb2ff5d457 | 35 | const double readbuttoncalibrate_frequency=10;//frequency at which the buttons are read when in calibration mode |
Gerth | 20:c5fb2ff5d457 | 36 | const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode |
Gerth | 6:37c94a5e205f | 37 | |
Gerth | 20:c5fb2ff5d457 | 38 | DigitalIn changemodebutton(PTA4);// button to change mode (sw3) |
Gerth | 7:7fbb2c028778 | 39 | Ticker readbuttoncalibrate_ticker; |
Gerth | 14:4c4f45a1dd23 | 40 | Ticker ledblink_ticker; |
Gerth | 14:4c4f45a1dd23 | 41 | |
Gerth | 20:c5fb2ff5d457 | 42 | DigitalIn buttonR(D2);//rigth button on biorobotics shield |
Gerth | 20:c5fb2ff5d457 | 43 | DigitalIn buttonL(D3);//left button on biorobotics shield |
Gerth | 8:54a7da09ccad | 44 | |
Gerth | 27:31cb8fbd976d | 45 | /////////////////READSIGNAL |
Gerth | 28:71a90073e482 | 46 | const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1 |
Gerth | 27:31cb8fbd976d | 47 | Ticker readsignal_ticker; |
Gerth | 27:31cb8fbd976d | 48 | |
Gerth | 27:31cb8fbd976d | 49 | |
Gerth | 27:31cb8fbd976d | 50 | DigitalOut led1(PTC12);// rigth led on biorobotics shield |
Gerth | 27:31cb8fbd976d | 51 | DigitalOut led2(D9);//left led on biorobotics shield |
Gerth | 27:31cb8fbd976d | 52 | |
Gerth | 1:917c07a4f3ec | 53 | //////////////////////////////////CONTROLLER |
Gerth | 25:21dcd3f9eac2 | 54 | const double control_frequency=250;// frequency at which the controller is called |
Gerth | 1:917c07a4f3ec | 55 | //controller constants |
Gerth | 31:8646e853979f | 56 | float Kp_shoot=6; |
Gerth | 31:8646e853979f | 57 | float Ki_shoot=2; |
Gerth | 31:8646e853979f | 58 | float Kd_shoot=0.5; |
Gerth | 31:8646e853979f | 59 | float Kp_move=2; |
Gerth | 31:8646e853979f | 60 | float Ki_move=1.25; |
Gerth | 31:8646e853979f | 61 | float Kd_move=0.05; |
Gerth | 31:8646e853979f | 62 | |
Gerth | 27:31cb8fbd976d | 63 | float factor_taup=1.5; |
Gerth | 27:31cb8fbd976d | 64 | float tau_p=1.0/(factor_taup*control_frequency); |
Gerth | 27:31cb8fbd976d | 65 | controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency |
Gerth | 20:c5fb2ff5d457 | 66 | |
Gerth | 1:917c07a4f3ec | 67 | Ticker control_ticker; |
Gerth | 20:c5fb2ff5d457 | 68 | |
Gerth | 27:31cb8fbd976d | 69 | const double Ts_control=1.0/control_frequency; |
Gerth | 27:31cb8fbd976d | 70 | |
Gerth | 26:c935e39cce8a | 71 | float error1=0,//controller error storage variables |
Gerth | 26:c935e39cce8a | 72 | error2=0; |
Gerth | 1:917c07a4f3ec | 73 | |
Gerth | 1:917c07a4f3ec | 74 | InterruptIn valuechangebutton(PTC6);//button to change controller constants |
Gerth | 1:917c07a4f3ec | 75 | |
Gerth | 1:917c07a4f3ec | 76 | //safetyandthreshold |
Gerth | 3:48438eea184e | 77 | AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm |
Gerth | 3:48438eea184e | 78 | AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person |
Gerth | 1:917c07a4f3ec | 79 | |
Gerth | 1:917c07a4f3ec | 80 | Ticker safetyandthreshold_ticker; // ticker to read potmeters |
Gerth | 3:48438eea184e | 81 | const double safetyandthreshold_frequency=1; // frequency for the ticker |
Gerth | 3:48438eea184e | 82 | |
Gerth | 4:bf7765b0f612 | 83 | float threshold_value=1;//initial threshold value |
Gerth | 1:917c07a4f3ec | 84 | |
Gerth | 1:917c07a4f3ec | 85 | ////////////////////////////////FILTER |
Gerth | 20:c5fb2ff5d457 | 86 | const double filter_frequency=500; |
Gerth | 20:c5fb2ff5d457 | 87 | #include "filtervalues.h"// call the values for the biquads |
Gerth | 20:c5fb2ff5d457 | 88 | |
Gerth | 1:917c07a4f3ec | 89 | Ticker filter_ticker; |
Gerth | 1:917c07a4f3ec | 90 | |
Gerth | 20:c5fb2ff5d457 | 91 | Biquad myfilter1;// make filter for signal 1 |
Gerth | 20:c5fb2ff5d457 | 92 | Biquad myfilter2;//make filter for signal 2 |
Gerth | 1:917c07a4f3ec | 93 | |
Gerth | 20:c5fb2ff5d457 | 94 | AnalogIn emg1_input(A0);//input for first emg signal |
Gerth | 20:c5fb2ff5d457 | 95 | AnalogIn emg2_input(A1);//input for second emg signal |
Gerth | 20:c5fb2ff5d457 | 96 | |
Gerth | 20:c5fb2ff5d457 | 97 | volatile double filteredsignal1=0;//the first filtered emg signal |
Gerth | 20:c5fb2ff5d457 | 98 | volatile double filteredsignal2=0;//the second filtered emg signal |
Gerth | 5:8ac5d0651e4d | 99 | float filter_extragain=1; |
Gerth | 1:917c07a4f3ec | 100 | |
Gerth | 4:bf7765b0f612 | 101 | |
Gerth | 4:bf7765b0f612 | 102 | //////////////////////////////// POSITION AND ANGLE SHIZZLE |
Gerth | 32:c940f6b6a6a0 | 103 | const float safetymarginfield=0.1; //adjustable, tweak for maximum but safe range |
Gerth | 24:dd75c961ae88 | 104 | const float mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed |
Gerth | 26:c935e39cce8a | 105 | const float y_start=0.145;//starting y position of the pod |
Gerth | 20:c5fb2ff5d457 | 106 | const float y_punch=0.473;// position to where there is punched |
Gerth | 32:c940f6b6a6a0 | 107 | const float timetoshoot=0.35;// time it can take to shoot |
Gerth | 31:8646e853979f | 108 | const float timetogoback=1;// time it can take to go back after shooting |
Gerth | 20:c5fb2ff5d457 | 109 | |
Gerth | 4:bf7765b0f612 | 110 | float desired_position=0; |
Gerth | 20:c5fb2ff5d457 | 111 | float desired_angle1=0; |
Gerth | 20:c5fb2ff5d457 | 112 | float desired_angle2=0; |
Gerth | 19:6f22b5687587 | 113 | |
Gerth | 20:c5fb2ff5d457 | 114 | const float fieldwidth=0.473; |
Gerth | 20:c5fb2ff5d457 | 115 | const float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield |
Gerth | 3:48438eea184e | 116 | |
Gerth | 20:c5fb2ff5d457 | 117 | angleandposition anglepos;// initiate the angle and position calculation library |
Gerth | 20:c5fb2ff5d457 | 118 | |
Gerth | 23:1c4a18799464 | 119 | const float radtodeg=(180/PIE); |
Gerth | 23:1c4a18799464 | 120 | |
Gerth | 19:6f22b5687587 | 121 | |
Gerth | 18:1c3254a32fd1 | 122 | |
Gerth | 1:917c07a4f3ec | 123 | //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS |
Gerth | 1:917c07a4f3ec | 124 | volatile bool scopedata_go=false, |
Gerth | 1:917c07a4f3ec | 125 | control_go=false, |
Gerth | 1:917c07a4f3ec | 126 | filter_go=false, |
Gerth | 3:48438eea184e | 127 | safetyandthreshold_go=false, |
Gerth | 6:37c94a5e205f | 128 | readsignal_go=false, |
Gerth | 8:54a7da09ccad | 129 | switchedmode=true, |
Gerth | 14:4c4f45a1dd23 | 130 | readbuttoncalibrate_go=false, |
Gerth | 14:4c4f45a1dd23 | 131 | ledblink_go=false; |
Gerth | 1:917c07a4f3ec | 132 | |
Gerth | 1:917c07a4f3ec | 133 | void scopedata_activate() |
Gerth | 1:917c07a4f3ec | 134 | { |
Gerth | 1:917c07a4f3ec | 135 | scopedata_go=true; |
Gerth | 1:917c07a4f3ec | 136 | } |
Gerth | 1:917c07a4f3ec | 137 | void control_activate() |
Gerth | 1:917c07a4f3ec | 138 | { |
Gerth | 1:917c07a4f3ec | 139 | control_go=true; |
Gerth | 1:917c07a4f3ec | 140 | } |
Gerth | 1:917c07a4f3ec | 141 | void filter_activate() |
Gerth | 1:917c07a4f3ec | 142 | { |
Gerth | 1:917c07a4f3ec | 143 | filter_go=true; |
Gerth | 1:917c07a4f3ec | 144 | } |
Gerth | 1:917c07a4f3ec | 145 | void safetyandthreshold_activate() |
Gerth | 1:917c07a4f3ec | 146 | { |
Gerth | 1:917c07a4f3ec | 147 | safetyandthreshold_go=true; |
Gerth | 1:917c07a4f3ec | 148 | } |
Gerth | 3:48438eea184e | 149 | void readsignal_activate() |
Gerth | 3:48438eea184e | 150 | { |
Gerth | 3:48438eea184e | 151 | readsignal_go=true; |
Gerth | 3:48438eea184e | 152 | } |
Gerth | 7:7fbb2c028778 | 153 | void readbuttoncalibrate_activate() |
Gerth | 7:7fbb2c028778 | 154 | { |
Gerth | 7:7fbb2c028778 | 155 | readbuttoncalibrate_go=true; |
Gerth | 7:7fbb2c028778 | 156 | } |
Gerth | 14:4c4f45a1dd23 | 157 | void ledblink_activate() |
Gerth | 14:4c4f45a1dd23 | 158 | { |
Gerth | 14:4c4f45a1dd23 | 159 | ledblink_go=true; |
Gerth | 14:4c4f45a1dd23 | 160 | } |
Gerth | 1:917c07a4f3ec | 161 | |
Gerth | 1:917c07a4f3ec | 162 | ////////////////////////FUNCTIONS |
Gerth | 1:917c07a4f3ec | 163 | //gather data and send to scope |
Gerth | 32:c940f6b6a6a0 | 164 | void scopedata(double wanted_y) |
Gerth | 1:917c07a4f3ec | 165 | { |
Gerth | 30:cd4d9754a357 | 166 | scope.set(0,desired_position-anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses())); |
Gerth | 31:8646e853979f | 167 | scope.set(1,wanted_y-anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses())); |
Gerth | 30:cd4d9754a357 | 168 | scope.set(2,error1*radtodeg); |
Gerth | 30:cd4d9754a357 | 169 | scope.set(3,error2*radtodeg); |
Gerth | 30:cd4d9754a357 | 170 | scope.set(4,mycontroller.motor1pwm()); |
Gerth | 30:cd4d9754a357 | 171 | scope.set(5,mycontroller.motor2pwm()); |
Gerth | 3:48438eea184e | 172 | scope.send(); |
Gerth | 4:bf7765b0f612 | 173 | } |
Gerth | 1:917c07a4f3ec | 174 | //read potmeters and adjust the safetyfactor and threshold |
Gerth | 1:917c07a4f3ec | 175 | void safetyandthreshold() |
Gerth | 1:917c07a4f3ec | 176 | { |
Gerth | 3:48438eea184e | 177 | mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal |
Gerth | 28:71a90073e482 | 178 | threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals |
Gerth | 1:917c07a4f3ec | 179 | } |
Gerth | 1:917c07a4f3ec | 180 | /////filter |
Gerth | 28:71a90073e482 | 181 | void filtereverything(bool makeempty) |
Gerth | 1:917c07a4f3ec | 182 | { |
Gerth | 1:917c07a4f3ec | 183 | //pass1 so f1 |
Gerth | 2:c7707856d137 | 184 | double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1); |
Gerth | 2:c7707856d137 | 185 | double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1); |
Gerth | 1:917c07a4f3ec | 186 | |
Gerth | 1:917c07a4f3ec | 187 | //pass2 so f2 |
Gerth | 2:c7707856d137 | 188 | double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2); |
Gerth | 2:c7707856d137 | 189 | double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2); |
Gerth | 1:917c07a4f3ec | 190 | |
Gerth | 1:917c07a4f3ec | 191 | //pass3 so f3 |
Gerth | 2:c7707856d137 | 192 | double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3); |
Gerth | 2:c7707856d137 | 193 | double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3); |
Gerth | 1:917c07a4f3ec | 194 | |
Gerth | 1:917c07a4f3ec | 195 | //pass4 so f4 |
Gerth | 2:c7707856d137 | 196 | double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4); |
Gerth | 2:c7707856d137 | 197 | double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4); |
Gerth | 1:917c07a4f3ec | 198 | |
Gerth | 1:917c07a4f3ec | 199 | //pass5 so f5 |
Gerth | 2:c7707856d137 | 200 | double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5); |
Gerth | 2:c7707856d137 | 201 | double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5); |
Gerth | 1:917c07a4f3ec | 202 | |
Gerth | 1:917c07a4f3ec | 203 | ///// take absolute value |
Gerth | 2:c7707856d137 | 204 | double pass5_emg1_abs=(fabs(pass5_emg1)); |
Gerth | 2:c7707856d137 | 205 | double pass5_emg2_abs=(fabs(pass5_emg2)); |
Gerth | 1:917c07a4f3ec | 206 | |
Gerth | 1:917c07a4f3ec | 207 | //pass6 so f6 |
Gerth | 2:c7707856d137 | 208 | double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6); |
Gerth | 2:c7707856d137 | 209 | double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6); |
Gerth | 1:917c07a4f3ec | 210 | |
Gerth | 1:917c07a4f3ec | 211 | |
Gerth | 1:917c07a4f3ec | 212 | //pass7 so f7 |
Gerth | 2:c7707856d137 | 213 | double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7); |
Gerth | 2:c7707856d137 | 214 | double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7); |
Gerth | 1:917c07a4f3ec | 215 | |
Gerth | 2:c7707856d137 | 216 | filteredsignal1=(pass7_emg1*9e11*filter_extragain); |
Gerth | 2:c7707856d137 | 217 | filteredsignal2=(pass7_emg2*9e11*filter_extragain); |
Gerth | 30:cd4d9754a357 | 218 | |
Gerth | 28:71a90073e482 | 219 | if (makeempty==true) {//this is needed so the filtered value is nog high after shooting |
Gerth | 28:71a90073e482 | 220 | pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0; |
Gerth | 28:71a90073e482 | 221 | pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0; |
Gerth | 28:71a90073e482 | 222 | v1_f1_emg1=v1_f1_emg2=v1_f2_emg1=v1_f2_emg2=v1_f3_emg1=v1_f3_emg2=v1_f4_emg1=v1_f4_emg2=v1_f5_emg1=0; |
Gerth | 28:71a90073e482 | 223 | v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0; |
Gerth | 28:71a90073e482 | 224 | } |
Gerth | 28:71a90073e482 | 225 | |
Gerth | 1:917c07a4f3ec | 226 | } |
Gerth | 1:917c07a4f3ec | 227 | //adjust controller values when sw2 is pressed |
Gerth | 1:917c07a4f3ec | 228 | void valuechange() |
Gerth | 1:917c07a4f3ec | 229 | { |
Gerth | 3:48438eea184e | 230 | mycontroller.STOP(); |
Gerth | 32:c940f6b6a6a0 | 231 | pc.printf("KP shoot is now %f, enter new value\n",Kp_shoot); |
Gerth | 32:c940f6b6a6a0 | 232 | pc.scanf("%f", &Kp_shoot); |
Gerth | 1:917c07a4f3ec | 233 | |
Gerth | 32:c940f6b6a6a0 | 234 | pc.printf("KI shoot is now %f, enter new value\n",Ki_shoot); |
Gerth | 32:c940f6b6a6a0 | 235 | pc.scanf("%f", &Ki_shoot); |
Gerth | 1:917c07a4f3ec | 236 | |
Gerth | 32:c940f6b6a6a0 | 237 | pc.printf("KD shoot is now %f, enter new value\n",Kd_shoot); |
Gerth | 32:c940f6b6a6a0 | 238 | pc.scanf("%f", &Kd_shoot); |
Gerth | 28:71a90073e482 | 239 | |
Gerth | 28:71a90073e482 | 240 | pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup); |
Gerth | 27:31cb8fbd976d | 241 | pc.scanf("%f", &factor_taup); |
Gerth | 3:48438eea184e | 242 | |
Gerth | 3:48438eea184e | 243 | pc.printf("Extra gain is now %f, enter new value\n",filter_extragain); |
Gerth | 5:8ac5d0651e4d | 244 | pc.scanf("%f", &filter_extragain); |
Gerth | 1:917c07a4f3ec | 245 | } |
Gerth | 11:c10651055871 | 246 | |
Gerth | 20:c5fb2ff5d457 | 247 | // shoot the pod forward |
Gerth | 19:6f22b5687587 | 248 | void shoot() |
Gerth | 14:4c4f45a1dd23 | 249 | { |
Gerth | 19:6f22b5687587 | 250 | ledgreen=1; |
Gerth | 32:c940f6b6a6a0 | 251 | double time=0; |
Gerth | 32:c940f6b6a6a0 | 252 | double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); |
Gerth | 32:c940f6b6a6a0 | 253 | double profile_angle=0; |
Gerth | 32:c940f6b6a6a0 | 254 | double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()); |
Gerth | 32:c940f6b6a6a0 | 255 | double y_during_punch=y_start; |
Gerth | 19:6f22b5687587 | 256 | Timer shoottimer; |
Gerth | 19:6f22b5687587 | 257 | shoottimer.reset(); |
Gerth | 19:6f22b5687587 | 258 | shoottimer.start(); |
Gerth | 23:1c4a18799464 | 259 | //forward |
Gerth | 19:6f22b5687587 | 260 | while (time<=timetoshoot) { |
Gerth | 19:6f22b5687587 | 261 | ledblue=!ledblue; |
Gerth | 25:21dcd3f9eac2 | 262 | |
Gerth | 32:c940f6b6a6a0 | 263 | profile_angle+=stepsize; // add stepsize to angle |
Gerth | 32:c940f6b6a6a0 | 264 | |
Gerth | 32:c940f6b6a6a0 | 265 | y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting |
Gerth | 32:c940f6b6a6a0 | 266 | |
Gerth | 20:c5fb2ff5d457 | 267 | if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety |
Gerth | 19:6f22b5687587 | 268 | y_during_punch=y_punch; |
Gerth | 19:6f22b5687587 | 269 | } else { |
Gerth | 19:6f22b5687587 | 270 | y_during_punch=y_during_punch; |
Gerth | 32:c940f6b6a6a0 | 271 | } |
Gerth | 19:6f22b5687587 | 272 | |
Gerth | 29:cd47a5a772db | 273 | desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles |
Gerth | 29:cd47a5a772db | 274 | desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch); |
Gerth | 19:6f22b5687587 | 275 | |
Gerth | 25:21dcd3f9eac2 | 276 | error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors |
Gerth | 25:21dcd3f9eac2 | 277 | error2=(desired_angle2-counttorad*encoder2.getPulses()); |
Gerth | 19:6f22b5687587 | 278 | |
Gerth | 31:8646e853979f | 279 | mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller |
Gerth | 31:8646e853979f | 280 | scopedata(y_during_punch);//send data to hidscope WARING lower freqyency than normal |
Gerth | 19:6f22b5687587 | 281 | |
Gerth | 20:c5fb2ff5d457 | 282 | time+=(Ts_control);// add time it should take to calculated time |
Gerth | 28:71a90073e482 | 283 | //pc.printf("Time = %f\n",time); |
Gerth | 28:71a90073e482 | 284 | filtereverything(true);//set al filter variables to 0 |
Gerth | 20:c5fb2ff5d457 | 285 | wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up |
Gerth | 11:c10651055871 | 286 | } |
Gerth | 23:1c4a18799464 | 287 | //back |
Gerth | 23:1c4a18799464 | 288 | time=0; |
Gerth | 23:1c4a18799464 | 289 | shoottimer.reset(); |
Gerth | 32:c940f6b6a6a0 | 290 | stepsize=(PIE)/(timetogoback*control_frequency); |
Gerth | 32:c940f6b6a6a0 | 291 | profile_angle=0; |
Gerth | 26:c935e39cce8a | 292 | while (time<=timetogoback) { |
Gerth | 23:1c4a18799464 | 293 | ledblue=!ledblue; |
Gerth | 25:21dcd3f9eac2 | 294 | |
Gerth | 32:c940f6b6a6a0 | 295 | profile_angle+=stepsize; // add stepsize to y position |
Gerth | 32:c940f6b6a6a0 | 296 | y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting |
Gerth | 23:1c4a18799464 | 297 | if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety |
Gerth | 23:1c4a18799464 | 298 | y_during_punch=y_start; |
Gerth | 23:1c4a18799464 | 299 | } else { |
Gerth | 23:1c4a18799464 | 300 | y_during_punch=y_during_punch; |
Gerth | 23:1c4a18799464 | 301 | } |
Gerth | 23:1c4a18799464 | 302 | |
Gerth | 29:cd47a5a772db | 303 | desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles |
Gerth | 29:cd47a5a772db | 304 | desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch); |
Gerth | 23:1c4a18799464 | 305 | |
Gerth | 25:21dcd3f9eac2 | 306 | error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors |
Gerth | 25:21dcd3f9eac2 | 307 | error2=(desired_angle2-counttorad*encoder2.getPulses()); |
Gerth | 23:1c4a18799464 | 308 | |
Gerth | 31:8646e853979f | 309 | mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller |
Gerth | 31:8646e853979f | 310 | scopedata(y_during_punch);//send data to hidscope WARING lower freqyency than normal |
Gerth | 23:1c4a18799464 | 311 | |
Gerth | 23:1c4a18799464 | 312 | time+=(Ts_control);// add time it should take to calculated time |
Gerth | 28:71a90073e482 | 313 | //pc.printf("Time = %f\n",time); |
Gerth | 28:71a90073e482 | 314 | filtereverything(false); |
Gerth | 23:1c4a18799464 | 315 | wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up |
Gerth | 25:21dcd3f9eac2 | 316 | } |
Gerth | 19:6f22b5687587 | 317 | shoottimer.stop(); |
Gerth | 16:63320b8f79c2 | 318 | ledblue=1; |
Gerth | 19:6f22b5687587 | 319 | ledgreen=0; |
Gerth | 11:c10651055871 | 320 | } |
Gerth | 11:c10651055871 | 321 | |
Gerth | 16:63320b8f79c2 | 322 | ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION |
Gerth | 3:48438eea184e | 323 | void readsignal() |
Gerth | 3:48438eea184e | 324 | { |
Gerth | 11:c10651055871 | 325 | //check if pod has to shoot |
Gerth | 4:bf7765b0f612 | 326 | if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) { |
Gerth | 4:bf7765b0f612 | 327 | led1=led2=1; |
Gerth | 11:c10651055871 | 328 | shoot(); |
Gerth | 14:4c4f45a1dd23 | 329 | // check if pod has to move to the right |
Gerth | 4:bf7765b0f612 | 330 | } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) { |
Gerth | 4:bf7765b0f612 | 331 | led1=1; |
Gerth | 4:bf7765b0f612 | 332 | led2=0; |
Gerth | 16:63320b8f79c2 | 333 | desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right |
Gerth | 11:c10651055871 | 334 | if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge |
Gerth | 11:c10651055871 | 335 | desired_position=maxdisplacement; |
Gerth | 11:c10651055871 | 336 | } else { |
Gerth | 11:c10651055871 | 337 | desired_position=desired_position; |
Gerth | 11:c10651055871 | 338 | } |
Gerth | 14:4c4f45a1dd23 | 339 | // check if pod has to move to the left |
Gerth | 4:bf7765b0f612 | 340 | } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) { |
Gerth | 4:bf7765b0f612 | 341 | led1=0; |
Gerth | 4:bf7765b0f612 | 342 | led2=1; |
Gerth | 16:63320b8f79c2 | 343 | desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left |
Gerth | 11:c10651055871 | 344 | if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge |
Gerth | 14:4c4f45a1dd23 | 345 | desired_position=(-1*maxdisplacement); |
Gerth | 11:c10651055871 | 346 | } else { |
Gerth | 11:c10651055871 | 347 | desired_position=desired_position; |
Gerth | 11:c10651055871 | 348 | } |
Gerth | 3:48438eea184e | 349 | } else { |
Gerth | 4:bf7765b0f612 | 350 | led1=led2=0; |
Gerth | 3:48438eea184e | 351 | } |
Gerth | 16:63320b8f79c2 | 352 | } |
Gerth | 28:71a90073e482 | 353 | |
Gerth | 16:63320b8f79c2 | 354 | ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION |
Gerth | 17:72d3522165ac | 355 | void readsignalbutton() |
Gerth | 16:63320b8f79c2 | 356 | { |
Gerth | 19:6f22b5687587 | 357 | //write value of button to variable |
Gerth | 17:72d3522165ac | 358 | int buttonr=buttonR.read(); |
Gerth | 17:72d3522165ac | 359 | int buttonl=buttonL.read(); |
Gerth | 16:63320b8f79c2 | 360 | //check if pod has to shoot |
Gerth | 17:72d3522165ac | 361 | if (buttonr==0 && buttonl==0) { |
Gerth | 16:63320b8f79c2 | 362 | led1=led2=1; |
Gerth | 16:63320b8f79c2 | 363 | shoot(); |
Gerth | 16:63320b8f79c2 | 364 | // check if pod has to move to the right |
Gerth | 17:72d3522165ac | 365 | } else if (buttonr==0 && buttonl==1) { |
Gerth | 16:63320b8f79c2 | 366 | led1=1; |
Gerth | 16:63320b8f79c2 | 367 | led2=0; |
Gerth | 16:63320b8f79c2 | 368 | desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right |
Gerth | 17:72d3522165ac | 369 | if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge |
Gerth | 16:63320b8f79c2 | 370 | desired_position=maxdisplacement; |
Gerth | 16:63320b8f79c2 | 371 | } else { |
Gerth | 16:63320b8f79c2 | 372 | desired_position=desired_position; |
Gerth | 16:63320b8f79c2 | 373 | } |
Gerth | 17:72d3522165ac | 374 | // check if pod has to move to the left |
Gerth | 17:72d3522165ac | 375 | } else if (buttonr==1 && buttonl==0) { |
Gerth | 16:63320b8f79c2 | 376 | led1=0; |
Gerth | 16:63320b8f79c2 | 377 | led2=1; |
Gerth | 16:63320b8f79c2 | 378 | desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left |
Gerth | 17:72d3522165ac | 379 | if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge |
Gerth | 16:63320b8f79c2 | 380 | desired_position=(-1*maxdisplacement); |
Gerth | 16:63320b8f79c2 | 381 | } else { |
Gerth | 16:63320b8f79c2 | 382 | desired_position=desired_position; |
Gerth | 16:63320b8f79c2 | 383 | } |
Gerth | 16:63320b8f79c2 | 384 | } else { |
Gerth | 16:63320b8f79c2 | 385 | led1=led2=0; |
Gerth | 16:63320b8f79c2 | 386 | } |
Gerth | 3:48438eea184e | 387 | } |
Gerth | 3:48438eea184e | 388 | |
Gerth | 28:71a90073e482 | 389 | void changemode() //this makes the counter higher to switch between modes |
Gerth | 6:37c94a5e205f | 390 | { |
Gerth | 10:9e96d14d7034 | 391 | mycontroller.STOP(); |
Gerth | 6:37c94a5e205f | 392 | switchedmode=true; |
Gerth | 6:37c94a5e205f | 393 | modecounter++; |
Gerth | 15:17de575b7385 | 394 | if (modecounter==4) { |
Gerth | 6:37c94a5e205f | 395 | modecounter=0; |
Gerth | 6:37c94a5e205f | 396 | } else { |
Gerth | 6:37c94a5e205f | 397 | modecounter=modecounter; |
Gerth | 6:37c94a5e205f | 398 | } |
Gerth | 20:c5fb2ff5d457 | 399 | wait(1);// needed because else it is checked too fast if the button is pressed and modes change too fast |
Gerth | 25:21dcd3f9eac2 | 400 | // tried it with interruptin but dinn't work |
Gerth | 6:37c94a5e205f | 401 | } |
Gerth | 0:dd66fff537d7 | 402 | |
Gerth | 16:63320b8f79c2 | 403 | ///////////////////////////////////////////////////MAIN |
Gerth | 7:7fbb2c028778 | 404 | |
Gerth | 0:dd66fff537d7 | 405 | int main() |
Gerth | 0:dd66fff537d7 | 406 | { |
Gerth | 1:917c07a4f3ec | 407 | //tickers |
Gerth | 1:917c07a4f3ec | 408 | safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency); |
Gerth | 1:917c07a4f3ec | 409 | filter_ticker.attach(&filter_activate,1.0/filter_frequency); |
Gerth | 1:917c07a4f3ec | 410 | control_ticker.attach(&control_activate,1.0/control_frequency); |
Gerth | 1:917c07a4f3ec | 411 | scope_ticker.attach(&scopedata_activate,1.0/scope_frequency); |
Gerth | 3:48438eea184e | 412 | readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency); |
Gerth | 7:7fbb2c028778 | 413 | readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency); |
Gerth | 14:4c4f45a1dd23 | 414 | ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency); |
Gerth | 14:4c4f45a1dd23 | 415 | |
Gerth | 20:c5fb2ff5d457 | 416 | pc.baud(115200);//set baudrate to 115200 |
Gerth | 1:917c07a4f3ec | 417 | while(1) { |
Gerth | 27:31cb8fbd976d | 418 | valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter |
Gerth | 25:21dcd3f9eac2 | 419 | checktimer.reset(); |
Gerth | 26:c935e39cce8a | 420 | checktimer.start(); |
Gerth | 20:c5fb2ff5d457 | 421 | if (changemodebutton==0) {// check if the change mode button is pressed |
Gerth | 6:37c94a5e205f | 422 | changemode(); |
Gerth | 1:917c07a4f3ec | 423 | } |
Gerth | 20:c5fb2ff5d457 | 424 | if (scopedata_go==true) {//send scopedata |
Gerth | 26:c935e39cce8a | 425 | //TIME THIS LOOP TAKES 0.000008 SEC (PEAKS AT 0.000015) |
Gerth | 31:8646e853979f | 426 | scopedata(y_start); |
Gerth | 26:c935e39cce8a | 427 | scopedata_go=false; |
Gerth | 9:4ee354663560 | 428 | } |
Gerth | 20:c5fb2ff5d457 | 429 | if (safetyandthreshold_go==true) {// check the potmeters |
Gerth | 26:c935e39cce8a | 430 | //TIME THIS LOOP TAKES: 0.000032 SEC |
Gerth | 9:4ee354663560 | 431 | safetyandthreshold(); |
Gerth | 9:4ee354663560 | 432 | safetyandthreshold_go=false; |
Gerth | 26:c935e39cce8a | 433 | } |
Gerth | 7:7fbb2c028778 | 434 | ///////////////////////////////////////////NORMAL RUNNING MODE |
Gerth | 7:7fbb2c028778 | 435 | if(modecounter==0) { |
Gerth | 6:37c94a5e205f | 436 | if (switchedmode==true) { |
Gerth | 20:c5fb2ff5d457 | 437 | encoder1.reset();// reset encoders so they are at 0 degrees |
Gerth | 6:37c94a5e205f | 438 | encoder2.reset(); |
Gerth | 6:37c94a5e205f | 439 | pc.printf("Program running\n");// |
Gerth | 16:63320b8f79c2 | 440 | ledgreen=0; |
Gerth | 16:63320b8f79c2 | 441 | led1=led2=ledred=ledblue=1; |
Gerth | 6:37c94a5e205f | 442 | switchedmode=false; |
Gerth | 6:37c94a5e205f | 443 | } |
Gerth | 20:c5fb2ff5d457 | 444 | if (filter_go==true) {// filter the emg signal |
Gerth | 26:c935e39cce8a | 445 | // TIME THIS LOOP TAKES: 0.000173 SEC |
Gerth | 28:71a90073e482 | 446 | filtereverything(false); |
Gerth | 26:c935e39cce8a | 447 | filter_go=false; |
Gerth | 6:37c94a5e205f | 448 | } |
Gerth | 20:c5fb2ff5d457 | 449 | if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position |
Gerth | 26:c935e39cce8a | 450 | // TIME THIS LOOP TAKES: 0.000005 SEC |
Gerth | 17:72d3522165ac | 451 | readsignal(); |
Gerth | 17:72d3522165ac | 452 | readsignal_go=false; |
Gerth | 26:c935e39cce8a | 453 | } |
Gerth | 20:c5fb2ff5d457 | 454 | if (control_go==true) {// calculate angles from positions and send error to controller |
Gerth | 26:c935e39cce8a | 455 | //TIME THIS LOOP TAKES: 0.000223 SEC |
Gerth | 26:c935e39cce8a | 456 | |
Gerth | 20:c5fb2ff5d457 | 457 | desired_angle1=anglepos.positiontoangle1(desired_position,y_start); |
Gerth | 20:c5fb2ff5d457 | 458 | desired_angle2=anglepos.positiontoangle2(desired_position,y_start); |
Gerth | 16:63320b8f79c2 | 459 | |
Gerth | 20:c5fb2ff5d457 | 460 | float error1=(desired_angle1-counttorad*encoder1.getPulses()); |
Gerth | 20:c5fb2ff5d457 | 461 | float error2=(desired_angle2-counttorad*encoder2.getPulses()); |
Gerth | 31:8646e853979f | 462 | mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move); |
Gerth | 6:37c94a5e205f | 463 | control_go=false; |
Gerth | 26:c935e39cce8a | 464 | } |
Gerth | 20:c5fb2ff5d457 | 465 | valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter |
Gerth | 1:917c07a4f3ec | 466 | } |
Gerth | 7:7fbb2c028778 | 467 | ////////////////////////////////////////////////////CALIBRATE RIGHT ARM |
Gerth | 6:37c94a5e205f | 468 | if (modecounter==1) { |
Gerth | 6:37c94a5e205f | 469 | if(switchedmode==true) { |
Gerth | 6:37c94a5e205f | 470 | pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n"); |
Gerth | 16:63320b8f79c2 | 471 | led1=led2=ledred=0; |
Gerth | 16:63320b8f79c2 | 472 | ledgreen=ledblue=1; |
Gerth | 6:37c94a5e205f | 473 | switchedmode=false; |
Gerth | 14:4c4f45a1dd23 | 474 | } |
Gerth | 14:4c4f45a1dd23 | 475 | if (ledblink_go==true) { |
Gerth | 20:c5fb2ff5d457 | 476 | led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated) |
Gerth | 16:63320b8f79c2 | 477 | ledblink_go=false; |
Gerth | 6:37c94a5e205f | 478 | } |
Gerth | 20:c5fb2ff5d457 | 479 | if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm |
Gerth | 9:4ee354663560 | 480 | if (buttonR.read()==0 && buttonL.read()==1) { |
Gerth | 20:c5fb2ff5d457 | 481 | desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency); |
Gerth | 9:4ee354663560 | 482 | readbuttoncalibrate_go=false; |
Gerth | 9:4ee354663560 | 483 | } |
Gerth | 9:4ee354663560 | 484 | if (buttonR.read()==1 && buttonL.read()==0) { |
Gerth | 20:c5fb2ff5d457 | 485 | desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency); |
Gerth | 9:4ee354663560 | 486 | readbuttoncalibrate_go=false; |
Gerth | 9:4ee354663560 | 487 | } |
Gerth | 9:4ee354663560 | 488 | } |
Gerth | 25:21dcd3f9eac2 | 489 | if (control_go==true) {// calculate errors and send them to controllers |
Gerth | 25:21dcd3f9eac2 | 490 | error1=(desired_angle1-counttorad*encoder1.getPulses()); |
Gerth | 25:21dcd3f9eac2 | 491 | error2=0; |
Gerth | 31:8646e853979f | 492 | mycontroller.PI(error1,error2,Kp_move,Ki_move); |
Gerth | 20:c5fb2ff5d457 | 493 | control_go=false; |
Gerth | 20:c5fb2ff5d457 | 494 | } |
Gerth | 8:54a7da09ccad | 495 | } |
Gerth | 8:54a7da09ccad | 496 | ////////////////////////////////////////////CALIBRATE LEFT ARM |
Gerth | 8:54a7da09ccad | 497 | if (modecounter==2) { |
Gerth | 8:54a7da09ccad | 498 | if(switchedmode==true) { |
Gerth | 8:54a7da09ccad | 499 | pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n"); |
Gerth | 16:63320b8f79c2 | 500 | led1=led2=ledred=0; |
Gerth | 16:63320b8f79c2 | 501 | ledgreen=ledblue=1; |
Gerth | 8:54a7da09ccad | 502 | switchedmode=false; |
Gerth | 8:54a7da09ccad | 503 | } |
Gerth | 14:4c4f45a1dd23 | 504 | if (ledblink_go==true) { |
Gerth | 20:c5fb2ff5d457 | 505 | led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated) |
Gerth | 16:63320b8f79c2 | 506 | ledblink_go=false; |
Gerth | 14:4c4f45a1dd23 | 507 | } |
Gerth | 25:21dcd3f9eac2 | 508 | if (readbuttoncalibrate_go==true) {// |
Gerth | 20:c5fb2ff5d457 | 509 | if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm |
Gerth | 20:c5fb2ff5d457 | 510 | desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency); |
Gerth | 8:54a7da09ccad | 511 | readbuttoncalibrate_go=false; |
Gerth | 7:7fbb2c028778 | 512 | } |
Gerth | 8:54a7da09ccad | 513 | if (buttonR.read()==1 && buttonL.read()==0) { |
Gerth | 20:c5fb2ff5d457 | 514 | desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency); |
Gerth | 8:54a7da09ccad | 515 | readbuttoncalibrate_go=false; |
Gerth | 7:7fbb2c028778 | 516 | } |
Gerth | 6:37c94a5e205f | 517 | } |
Gerth | 25:21dcd3f9eac2 | 518 | if (control_go==true) {// calculate errors and send to controller |
Gerth | 25:21dcd3f9eac2 | 519 | error1=0; |
Gerth | 25:21dcd3f9eac2 | 520 | error2=(desired_angle2-counttorad*encoder2.getPulses()); |
Gerth | 31:8646e853979f | 521 | mycontroller.PI(error1,error2,Kp_move,Ki_move); |
Gerth | 20:c5fb2ff5d457 | 522 | control_go=false; |
Gerth | 20:c5fb2ff5d457 | 523 | } |
Gerth | 3:48438eea184e | 524 | } |
Gerth | 16:63320b8f79c2 | 525 | ///////////////////////////////BUTTONCONTROLMODE |
Gerth | 15:17de575b7385 | 526 | if (modecounter==3) { |
Gerth | 16:63320b8f79c2 | 527 | if (switchedmode==true) { |
Gerth | 19:6f22b5687587 | 528 | pc.printf("Buttonmode, you can use the buttons to control the robot\n"); |
Gerth | 16:63320b8f79c2 | 529 | led1=led2=0; |
Gerth | 25:21dcd3f9eac2 | 530 | ledred=ledblue=1; |
Gerth | 21:6954cc25f2a7 | 531 | encoder1.reset();// reset encoders so they are at 0 degrees |
Gerth | 21:6954cc25f2a7 | 532 | encoder2.reset(); |
Gerth | 16:63320b8f79c2 | 533 | switchedmode=false; |
Gerth | 16:63320b8f79c2 | 534 | } |
Gerth | 16:63320b8f79c2 | 535 | if (ledblink_go==true) { |
Gerth | 15:17de575b7385 | 536 | ledgreen=!ledgreen; |
Gerth | 16:63320b8f79c2 | 537 | ledblink_go=false; |
Gerth | 15:17de575b7385 | 538 | } |
Gerth | 20:c5fb2ff5d457 | 539 | if (readsignal_go==true) {// read buttons and adjus wanted position |
Gerth | 17:72d3522165ac | 540 | readsignalbutton(); |
Gerth | 17:72d3522165ac | 541 | readsignal_go=false; |
Gerth | 17:72d3522165ac | 542 | } |
Gerth | 20:c5fb2ff5d457 | 543 | if (control_go==true) {// calculate wanted angles from position, errors and send to controller |
Gerth | 20:c5fb2ff5d457 | 544 | desired_angle1=anglepos.positiontoangle1(desired_position,y_start); |
Gerth | 20:c5fb2ff5d457 | 545 | desired_angle2=anglepos.positiontoangle2(desired_position,y_start); |
Gerth | 17:72d3522165ac | 546 | |
Gerth | 25:21dcd3f9eac2 | 547 | error1=(desired_angle1-counttorad*encoder1.getPulses()); |
Gerth | 25:21dcd3f9eac2 | 548 | error2=(desired_angle2-counttorad*encoder2.getPulses()); |
Gerth | 31:8646e853979f | 549 | mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move); |
Gerth | 17:72d3522165ac | 550 | control_go=false; |
Gerth | 16:63320b8f79c2 | 551 | } |
Gerth | 16:63320b8f79c2 | 552 | } |
Gerth | 25:21dcd3f9eac2 | 553 | checktimervalue=checktimer.read(); |
Gerth | 25:21dcd3f9eac2 | 554 | checktimer.stop(); |
Gerth | 0:dd66fff537d7 | 555 | } |
Gerth | 8:54a7da09ccad | 556 | } |