Peter Knoben / Mbed 2 deprecated Oudverslag

Oud verslag voor Biquad.

Dependencies:   Biquad HIDScope QEI angleandposition controlandadjust mbed

Fork of includeair by Jasper Gerth

main.cpp@28:71a90073e482, 2015-10-27 (annotated)

Committer:
Gerth
Date:
Tue Oct 27 13:05:36 2015 +0000
Revision:
28:71a90073e482
Parent:
27:31cb8fbd976d
Child:
29:cd47a5a772db
adjusted shoot loop so filter resets and the filterded value does not stay high;

Who changed what in which revision?

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