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@19:6f22b5687587, 2015-10-20 (annotated)

Committer:
Gerth
Date:
Tue Oct 20 12:47:08 2015 +0000
Revision:
19:6f22b5687587
Parent:
18:1c3254a32fd1
Child:
20:c5fb2ff5d457
shooting working;

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