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

Committer:
Gerth
Date:
Tue Oct 20 13:56:29 2015 +0000
Revision:
22:b29ba919d93e
Parent:
21:6954cc25f2a7
Child:
23:1c4a18799464
added error to hidscope;

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