Peter Knoben
Oud verslag voor Biquad.
Dependencies: Biquad HIDScope QEI angleandposition controlandadjust mbed
Fork of
includeair
by 
Jasper Gerth
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "HIDScope.h" 00004 #include "Biquad.h" 00005 #include "controlandadjust.h" 00006 #include "angleandposition.h" 00007 00008 ///////////////////////////////////////////////info out 00009 HIDScope scope(6);// number of hidscope channels 00010 Ticker scope_ticker;//ticker for the scope 00011 const double scope_frequency=200; //HIDscope frequency 00012 00013 Timer checktimer;// timer to check how much time it takes to run the control loop, to see if the frequencies are not too high 00014 volatile double checktimervalue=0; // make a variable to store the time the loop has taken 00015 00016 Serial pc(USBTX,USBRX);// serial connection to pc 00017 00018 DigitalOut ledred(LED_RED);// leds on the k64f board 00019 DigitalOut ledgreen(LED_GREEN); 00020 DigitalOut ledblue(LED_BLUE); 00021 00022 00023 /////////////////////////////////////////////ENCODERS 00024 const double cpr_sensor=32; //counts per rotation of the sensor 00025 const double cpr_shaft=cpr_sensor*131;//counts per rotation of the outputshaft 00026 00027 QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding 00028 QEI encoder2(D10,D11,NC,cpr_sensor);// first is pin a, pin b and the second is pin b, pin a so the encoders give positive rotation when the pod is moved forward 00029 00030 const double PIE=3.14159265359; // pi, for calculations 00031 const double counttorad=((2*PIE)/cpr_shaft);// to convert counts to rotation in rad 00032 00033 00034 /////////////////////////////////CALIBRATION (MODE) 00035 const double radpersec_calibrate=0.1*PIE;// speed of arms when in calibration mode 00036 int modecounter=1;//counter in which mode the robot is 00037 const double readbuttoncalibrate_frequency=50;//frequency at which the buttons are read when in calibration mode 00038 const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode 00039 00040 InterruptIn changemodebutton(PTA4);// button to change mode (sw3) 00041 Ticker readbuttoncalibrate_ticker;//ticker for reading out the buttons when calibrating 00042 Ticker ledblink_ticker;// ticker for blinking the leds 00043 00044 DigitalIn buttonR(D2);//rigth button on biorobotics shield 00045 DigitalIn buttonL(D3);//left button on biorobotics shield 00046 00047 /////////////////READSIGNAL 00048 const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1 00049 Ticker readsignal_ticker; // ticker for reading the signal of the emg 00050 00051 00052 DigitalOut led1(PTC12);// rigth led on biorobotics shield 00053 DigitalOut led2(D9);//left led on biorobotics shield 00054 00055 //////////////////////////////////CONTROLLER 00056 const double control_frequency=250;// frequency at which the controller is called 00057 //controller constants 00058 float Kp_shoot=6; 00059 float Ki_shoot=2; 00060 float Kd_shoot=0.5; 00061 float Kp_move=2; 00062 float Ki_move=1.25; 00063 float Kd_move=0.05; 00064 00065 controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency 00066 00067 Ticker control_ticker;//ticker for the controller 00068 const double Ts_control=1.0/control_frequency;//sample time of the controller 00069 00070 double error1=0,//controller error storage variables 00071 error2=0; 00072 00073 InterruptIn valuechangebutton(PTC6);//button to change filter gains per arm via Serial 00074 00075 //safetyandthreshold 00076 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm 00077 AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person 00078 00079 Ticker safetyandthreshold_ticker; // ticker to read potmeters 00080 const double safetyandthreshold_frequency=1; // frequency for the ticker 00081 00082 double threshold_value=1;//initial threshold value 00083 00084 ////////////////////////////////FILTER 00085 const double filter_frequency=500; // frequency at which the emg signal is filtered 00086 #include "filtervalues.h"// call the values for the biquads these are in a separate file to keep this code readable 00087 00088 Ticker filter_ticker;//Ticker for the filter 00089 00090 Biquad myfilter1;// make filter for signal 1 00091 Biquad myfilter2;//make filter for signal 2 00092 00093 AnalogIn emg1_input(A0);//input for first emg signal 00094 AnalogIn emg2_input(A1);//input for second emg signal 00095 00096 volatile double filteredsignal1=0;//the first filtered emg signal 00097 volatile double filteredsignal2=0;//the second filtered emg signal 00098 double filter_extragain1=1,filter_extragain2=1; // this is a factor to increase the gain per arm, adustable via serial 00099 00100 00101 //////////////////////////////// POSITION AND ANGLE 00102 const double safetymarginfield=0.05; //adjustable, tweak for maximum but safe range 00103 const double mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed 00104 const double y_start=0.225;//starting y position of the pod 00105 const double y_punch=0.473;// position to where there is punched 00106 const double timetoshoot=0.35;// time it can take to shoot 00107 const double timetogoback=1;// time it can take to go back after shooting 00108 00109 volatile double desired_position=0;//desired x position 00110 double desired_angle1=0; //desired angle of arm 1 (calculated with anglepos) 00111 double desired_angle2=0; // desired anvle of arm 2 (calculated with anglepos) 00112 00113 const double fieldwidth=0.473; // width of the field 00114 const double maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield 00115 00116 angleandposition anglepos;// initiate the angle and position calculation library 00117 00118 const double radtodeg=(180/PIE); // to convert radians to degrees 00119 00120 00121 /////////////////////////////////////////////////////////DEMO MODE 00122 const double demo_safety=0.05;// safety factor to stay off the edges 00123 const double demo_x_edge_rigth=(fieldwidth/2)-demo_safety;// rigth edge of the field 00124 const double demo_x_edge_left=-((fieldwidth/2)-demo_safety);// left edge of te field 00125 const double demo_goal_edge_rigth=0.1+demo_safety;// x edges of goal 00126 const double demo_goal_edge_left=-(0.1+demo_safety); 00127 const double demo_y_goal=0.1+demo_safety;// y position in front of goal 00128 const double demo_y_back=0.05+demo_safety;//back edge of the field 00129 const double demo_y_front=y_punch; 00130 00131 int demo_go=0; 00132 00133 00134 00135 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS 00136 volatile bool scopedata_go=false, 00137 control_go=false, 00138 filter_go=false, 00139 safetyandthreshold_go=false, 00140 readsignal_go=false, 00141 switchedmode=true, 00142 readbuttoncalibrate_go=false, 00143 ledblink_go=false, 00144 demo_next_step_go=false; 00145 00146 void scopedata_activate() 00147 { 00148 scopedata_go=true; 00149 } 00150 void control_activate() 00151 { 00152 control_go=true; 00153 } 00154 void filter_activate() 00155 { 00156 filter_go=true; 00157 } 00158 void safetyandthreshold_activate() 00159 { 00160 safetyandthreshold_go=true; 00161 } 00162 void readsignal_activate() 00163 { 00164 readsignal_go=true; 00165 } 00166 void readbuttoncalibrate_activate() 00167 { 00168 readbuttoncalibrate_go=true; 00169 } 00170 void ledblink_activate() 00171 { 00172 ledblink_go=true; 00173 } 00174 void demo_next_step_activate() 00175 { 00176 demo_next_step_go=true; 00177 } 00178 00179 ////////////////////////FUNCTIONS 00180 //gather data and send to scope 00181 void scopedata(double wanted_y) 00182 { 00183 scope.set(0,desired_position); // desired x position 00184 scope.set(1,wanted_y); // desired y position 00185 scope.set(2,filteredsignal1); // filterded emg signal rigth arm 00186 scope.set(3,filteredsignal2); // filtered emg signal left arm 00187 scope.set(4,threshold_value); // threshold value 00188 scope.set(5,mycontroller.motor1pwm()); //pwm signal send to motor 1 00189 scope.send(); // send info to HIDScope server 00190 } 00191 //read potmeters and adjust the safetyfactor and threshold 00192 void safetyandthreshold() 00193 { 00194 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal 00195 threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals 00196 } 00197 /////filter 00198 void filtereverything(bool makeempty) 00199 { 00200 //pass1 so f1 00201 double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1); 00202 double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1); 00203 00204 //pass2 so f2 00205 double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2); 00206 double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2); 00207 00208 //pass3 so f3 00209 double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3); 00210 double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3); 00211 00212 //pass4 so f4 00213 double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4); 00214 double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4); 00215 00216 //pass5 so f5 00217 double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5); 00218 double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5); 00219 00220 ///// take absolute value 00221 double pass5_emg1_abs=(fabs(pass5_emg1)); 00222 double pass5_emg2_abs=(fabs(pass5_emg2)); 00223 00224 //pass6 so f6 00225 double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6); 00226 double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6); 00227 00228 00229 //pass7 so f7 00230 double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7); 00231 double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7); 00232 00233 filteredsignal1=(pass7_emg1*9e11*filter_extragain1);//this is needed to make the signal ~1 when flexed 00234 filteredsignal2=(pass7_emg2*9e11*filter_extragain2);// filter_extragain is adjusted via serial 00235 00236 if (makeempty==true) {//this is needed so the filtered value is not high after shooting basically it resets the filter 00237 pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0; 00238 pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0; 00239 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; 00240 v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0; 00241 } 00242 00243 } 00244 //adjust controller values (and wheter or not demo is shown) when sw2 is pressed 00245 void valuechange() 00246 { 00247 if (modecounter==4) {// cannot be changed if demo loop is running 00248 pc.printf("Not now!\n"); 00249 } else { 00250 mycontroller.STOP(); // stop motors 00251 pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm 00252 pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1 00253 00254 pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm 00255 pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2 00256 00257 pc.printf("Do you want to enter demo mode after button control mode 1/0? (1 for yes, 0 for no.\n"); 00258 pc.scanf("%i", &demo_go);//read the input from serial and write to filter_extragain2 00259 00260 pc.printf("Done\n"); 00261 } 00262 } 00263 00264 // shoot the pod forward 00265 void shoot() 00266 { 00267 ledgreen=1; 00268 double time=0; 00269 double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize 00270 double profile_angle=0;// used for the profilie 00271 double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());// calculate actual x positon from the encoder angles 00272 double y_during_punch=y_start;//set initial y value to baseline 00273 Timer shoottimer; // make a timer 00274 shoottimer.reset(); 00275 shoottimer.start(); 00276 //forward 00277 while (time<=timetoshoot) { 00278 ledblue=!ledblue; // blink blue led (very fast, so not noticable blinking) 00279 00280 profile_angle+=stepsize; // add stepsize to angle 00281 00282 y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting 00283 00284 if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety 00285 y_during_punch=y_punch; 00286 } else { 00287 y_during_punch=y_during_punch; 00288 } 00289 00290 desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles 00291 desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch); 00292 00293 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors 00294 error2=(desired_angle2-counttorad*encoder2.getPulses()); 00295 00296 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller 00297 scopedata(y_during_punch);//send data to hidscope WARING higher freqyency than normal 00298 00299 time+=(Ts_control);// add time it should take to calculated time 00300 filtereverything(true);//set al filter variables to 0 00301 00302 wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, 00303 // if this is not done the loop wil go to fast and the motors can't keep up 00304 // this is because the control loop takes far less time than Ts_control 00305 } 00306 //back 00307 time=0; // reset time 00308 shoottimer.reset();//reset timer 00309 stepsize=(PIE)/(timetogoback*control_frequency);//calculate stepsize 00310 profile_angle=0;//reset angle for the displacement profile 00311 desired_position=0;// set desired x position to 0, so the robot is in the middle in front of the goal after shooting 00312 while (time<=timetogoback) { 00313 ledblue=!ledblue;// blink blue led 00314 00315 profile_angle+=stepsize; // add stepsize to y position 00316 y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting 00317 if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety 00318 y_during_punch=y_start; 00319 } else { 00320 y_during_punch=y_during_punch; 00321 } 00322 00323 desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles 00324 desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch); 00325 00326 00327 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors 00328 error2=(desired_angle2-counttorad*encoder2.getPulses()); 00329 00330 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller 00331 scopedata(y_during_punch);//send data to hidscope WARING higher freqyency than normal 00332 00333 time+=(Ts_control);// add time it should take to calculated time 00334 filtereverything(false);//start filtering the signal (lower frewquency than normal) 00335 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 00336 } 00337 shoottimer.stop(); 00338 ledblue=1; // blue led off 00339 ledgreen=0; // green led on 00340 } 00341 00342 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION 00343 void readsignal() 00344 { 00345 //check if pod has to shoot 00346 if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) { 00347 led1=led2=1; // ligth up both leds 00348 shoot(); 00349 // check if pod has to move to the right 00350 } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) { 00351 led1=1;// ligth up rigth led 00352 led2=0; 00353 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right 00354 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge 00355 desired_position=maxdisplacement; 00356 } else { 00357 desired_position=desired_position; 00358 } 00359 // check if pod has to move to the left 00360 } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) { 00361 led1=0; // ligth up left led 00362 led2=1; 00363 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left 00364 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge 00365 desired_position=(-1*maxdisplacement); 00366 } else { 00367 desired_position=desired_position; 00368 } 00369 } else { 00370 led1=led2=0; 00371 } 00372 } 00373 00374 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION 00375 void readsignalbutton() // same as above, only with buttons as input 00376 { 00377 //write value of button to variable 00378 int buttonr=buttonR.read(); 00379 int buttonl=buttonL.read(); 00380 //check if pod has to shoot 00381 if (buttonr==0 && buttonl==0) { 00382 led1=led2=1; 00383 shoot(); 00384 // check if pod has to move to the right 00385 } else if (buttonr==0 && buttonl==1) { 00386 led1=1; 00387 led2=0; 00388 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right 00389 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge 00390 desired_position=maxdisplacement; 00391 } else { 00392 desired_position=desired_position; 00393 } 00394 // check if pod has to move to the left 00395 } else if (buttonr==1 && buttonl==0) { 00396 led1=0; 00397 led2=1; 00398 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left 00399 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge 00400 desired_position=(-1*maxdisplacement); 00401 } else { 00402 desired_position=desired_position; 00403 } 00404 } else { 00405 led1=led2=0; 00406 } 00407 } 00408 00409 void changemode() //this makes the counter higher to switch between modes 00410 { 00411 mycontroller.STOP(); // stop the controller 00412 switchedmode=true; // set switchedmode to true so in the main loop some statements are executed 00413 modecounter++; // increase counter 00414 if (modecounter==6) { // reset counter if counter=6 00415 modecounter=0; 00416 } else { 00417 modecounter=modecounter; 00418 } 00419 00420 } 00421 00422 // function takes x,y and time as input and moves pod to the desired coordinates in the desired time 00423 void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos) 00424 { 00425 double timepos=0; 00426 Timer postimer; 00427 double pos_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()); 00428 double pos_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()); 00429 00430 double pos_stepsize_x=(pos_x_start-desired_x_pos)/(time_to_pos*control_frequency); 00431 double pos_stepsize_y=(pos_y_start-desired_y_pos)/(time_to_pos*control_frequency); 00432 00433 double pos_x_moving=pos_x_start; 00434 double pos_y_moving=pos_y_start; 00435 00436 postimer.start(); 00437 while(timepos<=time_to_pos) { 00438 pos_x_moving-=pos_stepsize_x; 00439 pos_y_moving-=pos_stepsize_y; 00440 00441 desired_angle1=anglepos.positiontoangle1(pos_x_moving,pos_y_moving);// calculate desired angles 00442 desired_angle2=anglepos.positiontoangle2( pos_x_moving,pos_y_moving); 00443 00444 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors 00445 error2=(desired_angle2-counttorad*encoder2.getPulses()); 00446 00447 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller 00448 00449 timepos+=(Ts_control);// add time it should take to calculated time 00450 wait(timepos-postimer.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 00451 } 00452 } 00453 00454 ///////////////////////////////////////////////////MAIN 00455 00456 int main() 00457 { 00458 //initiate tickers 00459 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency); 00460 filter_ticker.attach(&filter_activate,1.0/filter_frequency); 00461 control_ticker.attach(&control_activate,1.0/control_frequency); 00462 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency); 00463 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency); 00464 readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency); 00465 ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency); 00466 00467 pc.baud(115200);//set baudrate to 115200 00468 00469 while(1) {// while (1) so continious loop 00470 valuechangebutton.fall(&valuechange);// used to change the filter gains 00471 changemodebutton.rise(&changemode);// interruptin for next mode 00472 checktimer.reset();// reset the timer to check the time it takes to run the entire control loop 00473 checktimer.start(); 00474 00475 if (scopedata_go==true) {//send scopedata 00476 //TIME THIS LOOP TAKES: 0.000008 SEC (PEAKS AT 0.000015) 00477 scopedata(y_start);// call scopedata, use baseline as desired y position 00478 scopedata_go=false; 00479 } 00480 if (safetyandthreshold_go==true) {// check the potmeters 00481 //TIME THIS LOOP TAKES: 0.000032 SEC 00482 safetyandthreshold(); // read out the potmeters on top of the biorobotics board 00483 safetyandthreshold_go=false; 00484 } 00485 ///////////////////////////////////////////NORMAL RUNNING MODE 00486 if(modecounter==0) { 00487 if (switchedmode==true) { 00488 pc.printf("Program running\n");// print to serial 00489 led1=led2=ledgreen=0;// green led on leds on top off 00490 ledred=ledblue=1;//rest off 00491 gotopos(0,y_start,1); 00492 desired_position=0; 00493 switchedmode=false; 00494 } 00495 if (filter_go==true) {// filter the emg signal 00496 // TIME THIS LOOP TAKES: 0.000173 SEC 00497 filtereverything(false); 00498 filter_go=false; 00499 } 00500 if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position 00501 // TIME THIS LOOP TAKES: 0.000005 SEC 00502 readsignal(); 00503 readsignal_go=false; 00504 } 00505 if (control_go==true) {// calculate angles from positions and send error to controller 00506 //TIME THIS LOOP TAKES: 0.000223 SEC 00507 00508 desired_angle1=anglepos.positiontoangle1(desired_position,y_start); 00509 desired_angle2=anglepos.positiontoangle2(desired_position,y_start); 00510 00511 double error1=(desired_angle1-counttorad*encoder1.getPulses()); 00512 double error2=(desired_angle2-counttorad*encoder2.getPulses()); 00513 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move); 00514 control_go=false; 00515 } 00516 } 00517 ////////////////////////////////////////////////////CALIBRATE RIGHT ARM 00518 if (modecounter==1) { 00519 if(switchedmode==true) { 00520 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n"); 00521 led1=led2=ledred=0; 00522 ledgreen=ledblue=1; 00523 switchedmode=false; 00524 } 00525 if (ledblink_go==true) { 00526 led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated) 00527 ledblink_go=false; 00528 } 00529 if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm 00530 if (buttonR.read()==0 && buttonL.read()==1) { 00531 desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency); 00532 readbuttoncalibrate_go=false; 00533 } 00534 if (buttonR.read()==1 && buttonL.read()==0) { 00535 desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency); 00536 readbuttoncalibrate_go=false; 00537 } 00538 } 00539 if (control_go==true) {// calculate errors and send them to controllers 00540 error1=(desired_angle1-counttorad*encoder1.getPulses()); 00541 error2=0;// only adjust rigth arm 00542 mycontroller.PI(error1,error2,Kp_move,Ki_move); 00543 control_go=false; 00544 } 00545 } 00546 ////////////////////////////////////////////CALIBRATE LEFT ARM 00547 if (modecounter==2) { 00548 if(switchedmode==true) { 00549 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n"); 00550 led1=led2=ledred=0; 00551 ledgreen=ledblue=1; 00552 switchedmode=false; 00553 } 00554 if (ledblink_go==true) { 00555 led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated) 00556 ledblink_go=false; 00557 } 00558 if (readbuttoncalibrate_go==true) {// 00559 if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm 00560 desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency); 00561 readbuttoncalibrate_go=false; 00562 } 00563 if (buttonR.read()==1 && buttonL.read()==0) { 00564 desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency); 00565 readbuttoncalibrate_go=false; 00566 } 00567 } 00568 if (control_go==true) {// calculate errors and send to controller 00569 error1=0; // only adjus left arm 00570 error2=(desired_angle2-counttorad*encoder2.getPulses()); 00571 mycontroller.PI(error1,error2,Kp_move,Ki_move); 00572 control_go=false; 00573 } 00574 } 00575 ///////////////////////////////BUTTONCONTROLMODE 00576 if (modecounter==3) { 00577 if (switchedmode==true) { 00578 pc.printf("Buttonmode, you can use the buttons to control the robot\n"); 00579 led1=led2=0; 00580 ledred=ledblue=1; 00581 encoder1.reset();// reset encoders so they are at 0 degrees 00582 encoder2.reset(); 00583 desired_position=0;//set desired position to the middle of the field, where the pod actually is. 00584 switchedmode=false; 00585 } 00586 if (ledblink_go==true) { 00587 ledgreen=!ledgreen; // blink green led 00588 ledblink_go=false; 00589 } 00590 if (readsignal_go==true) {// read buttons and adjust wanted position 00591 readsignalbutton(); 00592 readsignal_go=false; 00593 } 00594 if (control_go==true) {// calculate wanted angles from position, errors and send to controller 00595 desired_angle1=anglepos.positiontoangle1(desired_position,y_start); 00596 desired_angle2=anglepos.positiontoangle2(desired_position,y_start); 00597 00598 error1=(desired_angle1-counttorad*encoder1.getPulses()); 00599 error2=(desired_angle2-counttorad*encoder2.getPulses()); 00600 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move); 00601 control_go=false; 00602 } 00603 } 00604 ///////////////////////////////////DEMO MODE 00605 if (modecounter==4 && demo_go==!1) {//if demo mode not enabled, move to next mode 00606 changemode(); 00607 } 00608 if (modecounter==4 && demo_go==1) { 00609 if (switchedmode==true) { 00610 pc.printf("Demo mode, the pod moves around the field\n"); 00611 led1=led2=ledred=ledblue=ledgreen=0; // rgb on, leds on top off 00612 gotopos(0,demo_y_goal,1); // go to goal @x=0 00613 switchedmode=false; 00614 } 00615 00616 // loop trough positions to make a path along the field, con of this method is that the mode is only changed when the loop is completed. 00617 // the button is read, but only when the loop is completed it is checked in what mode the robot has to be, that's why this mode is made optional, 00618 // because this mode is only neccesary for the presentation on 30-10-2015 (or for fun) 00619 gotopos(demo_goal_edge_left,demo_y_goal,0.5);// in front of goal to left edge of goal 00620 gotopos(demo_goal_edge_left,demo_y_back,0.25); // to back along left egde of goal 00621 gotopos(demo_x_edge_left,demo_y_back,0.5); //to left edge along back 00622 gotopos(demo_x_edge_left,demo_y_front,2);//from back to front along left edge 00623 gotopos(demo_x_edge_rigth,demo_y_front,2);// from left to rigth along front edge 00624 gotopos(demo_x_edge_rigth,demo_y_back,2);// from front to back along rigth edge 00625 gotopos(demo_goal_edge_rigth,demo_y_back,0.5);//from rigth edge to rigth edge of goal along back 00626 gotopos(demo_goal_edge_rigth,demo_y_goal,0.25); // from back to in front of goal along rigth edge goal 00627 gotopos(0,demo_y_goal,0.5); // from rigth edge goal to middle in front of goal 00628 gotopos(0,y_start,0.5); 00629 demo_go=0; 00630 } 00631 00632 //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE 00633 if(modecounter==5) { 00634 if (switchedmode==true) { 00635 pc.printf("Calibrate the EMG signals and threshold\n"); 00636 ledblue=ledgreen=1; 00637 led1=led2=ledred=0; 00638 switchedmode=false; 00639 } 00640 if(ledblink_go==true) { 00641 ledgreen=!ledgreen; 00642 ledred=!ledred; 00643 ledblink_go=false; 00644 } 00645 if (filter_go==true) {// filter the emg signal 00646 // TIME THIS LOOP TAKES: 0.000173 SEC 00647 filtereverything(false); 00648 filter_go=false; 00649 } 00650 } 00651 checktimervalue=checktimer.read(); // write the time it has taken to a variable, so this variable can be displayed using hidschope. 00652 // if chectimer.read() is used in scopedata, the value is probably ~0 00653 //because scopedata is called as one of the first functoins 00654 checktimer.stop(); 00655 } 00656 }
Generated on Wed Jul 13 2022 08:14:56 by
1.7.2