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@25:21dcd3f9eac2, 2015-10-23 (annotated)

Committer:
Gerth
Date:
Fri Oct 23 10:18:21 2015 +0000
Revision:
25:21dcd3f9eac2
Parent:
24:dd75c961ae88
Child:
26:c935e39cce8a
checked if loops do not take too much time and added timer

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