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@21:6954cc25f2a7, 2015-10-20 (annotated)

Committer:
Gerth
Date:
Tue Oct 20 13:51:30 2015 +0000
Revision:
21:6954cc25f2a7
Parent:
20:c5fb2ff5d457
Child:
22:b29ba919d93e
adjusted  controller values, now go back after shooting with steps instead of ginormous fault;

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