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@26:c935e39cce8a, 2015-10-26 (annotated)

Committer:
Gerth
Date:
Mon Oct 26 12:26:27 2015 +0000
Revision:
26:c935e39cce8a
Parent:
25:21dcd3f9eac2
Child:
27:31cb8fbd976d
adjusted calling the controller to the new library ( so error_prev error_int and Ts is moved to within the library;

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