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@24:dd75c961ae88, 2015-10-20 (annotated)

Committer:
Gerth
Date:
Tue Oct 20 15:14:40 2015 +0000
Revision:
24:dd75c961ae88
Parent:
23:1c4a18799464
Child:
25:21dcd3f9eac2
increased controller frequency and decreased hidscope frequency

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