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@34:bc2d64e86cb9, 2015-10-29 (annotated)

Committer:
Gerth
Date:
Thu Oct 29 10:35:42 2015 +0000
Revision:
34:bc2d64e86cb9
Parent:
33:1c7b498ded25
Child:
35:0141a1fe8138
pod returns to middle after shooting;

Who changed what in which revision?

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