Anonymous

  #define Fwdleft() \ T_path=3;T_wait=(timervl_selflngth/3);turn(); \ T_path=1;T_wait=(timervl_360dg/4); turn();#define Fwdright() \ T_path=3;T_wait=(timervl_selflngth/3);turn(); \ T_path=2;T_wait=(timervl_360dg/4); turn();#define lab_hh() OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);OnFwd(OUT_A);Wait(110);Off(OUT_A+OUT_C);\#define hittaturn() OnRev(OUT_A+OUT_C);Wait(40);Off(OUT_A+OUT_C);OnFwd(OUT_C);Wait(140);Off(OUT_A+OUT_C);\                    OnFwd(OUT_B);Wait(40);Off(OUT_B);#define LineLeft() OnFwd(OUT_A); OnRev(OUT_C); Wait(turntime_fline); Off(OUT_A+OUT_C);#define LineRight() OnFwd(OUT_C); OnRev(OUT_A); Wait(turntime_fline); Off(OUT_A+OUT_C);#define CarefulLineLeft() OnFwd(OUT_A);OnRev(OUT_C);Wait(5);\                          OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);#define CarefulLineRight() OnFwd(OUT_C);OnRev(OUT_A);Wait(5);\                          OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);#define MGates() Wait(40); Off(OUT_A+OUT_C); OnRev(OUT_B);Wait(30);Off(OUT_B);OnFwd(OUT_B);Wait(10);\        Off(OUT_B);Off(OUT_A+OUT_C);        #define MWall() OnRev(OUT_A+OUT_C);Wait(75);Off(OUT_C);Wait(70);OnFwd(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);\                SetPower(OUT_A+OUT_C, 2);#define PickBall() OnFwd(OUT_A+OUT_C);PlaySound(SOUND_UP);Wait(40);\        Off(OUT_A+OUT_C);OnRev(OUT_B);Wait(20);Off(OUT_B); \        PlaySound(SOUND_DOWN);\        OnRev(OUT_A+OUT_C);Wait(20);OnRev(OUT_A);OnFwd(OUT_C);Wait(70);OnFwd(OUT_A+OUT_C);Wait(40);        #define PickBall2() Off(OUT_A+OUT_C);SetPower(OUT_A+OUT_C,2);OnFwd(OUT_A+OUT_C);Wait(40);\                    Off(OUT_A+OUT_C);OnRev(OUT_B);Wait(20);Off(OUT_B);Off(OUT_A+OUT_C);OnRev(OUT_A+OUT_C);\                    Wait(80);OnFwd(OUT_C);OnRev(OUT_A);Wait(40);Off(OUT_A+OUT_C);OnFwd(OUT_A+OUT_C);Wait(80);        #define DropBall() Off(OUT_A+OUT_C);OnFwd(OUT_B);Wait(20);OnFwd(OUT_B);Wait(30);Off(OUT_B);\        Wait(200);OnRev(OUT_B);Wait(40);Off(OUT_B);OnRev(OUT_A+OUT_C);Wait(60);Off(OUT_A+OUT_C); \        OnRev(OUT_A+OUT_C);Wait(60);OnFwd(OUT_A);Wait(100);Off(OUT_A+OUT_C);#define OppnaGrind() OnRev(OUT_B); Wait(time_liftdown); Off(OUT_B);\                     OnRev(OUT_A+OUT_C); Wait(50); Off(OUT_A+OUT_C);\                     OnFwd(OUT_C);Wait(time_liftdown);Off(OUT_B);#define StopLab() Off(OUT_A+OUT_C);#define MotorsOff() Off(OUT_A+OUT_C);#define MotorsOn() OnFwd(OUT_A+OUT_C);#define InitSensors() SetSensor(SENSOR_1, SENSOR_LIGHT);SetSensor(SENSOR_3, SENSOR_LIGHT);SetSensor(SENSOR_2, SENSOR_TOUCH);#define LSENSOR SENSOR_1#define TSENSOR SENSOR_2#define RSENSOR SENSOR_3/* TABLE OF GLOBAL VARIABLES, SUBSTITUE FOR FUNCTION ARGUMENT INEPTABILITY *///Sensor valuesint Lwhite, Rwhite, Lgrey, Rgrey, Lblack, Rblack;//linje_till argumentsint Lthr, Rthr, LEthr, REthr, mode;//Miscellaneousint gate,turntime_fline=10,LabTimer=0;task main(){ InitSensors(); GetTimerValues(); SetPower(OUT_A+OUT_C,3); FindBlack(); hittaturn(); Lthr=Lblack;Rthr=Rblack;LEthr=Lgrey;REthr=Rgrey; linje_till(); MGates(); gate=true; linje_till(); gate=false; FindBlack(); lab_hh(); SetPower(OUT_A+OUT_C, 2); Lthr=Lgrey;Rthr=Rgrey; linje_till(); MWall(); turntime_fline=3;SetPower(OUT_A,1);SetPower(OUT_C,4); Lthr=Lblack;Rthr=Rblack; linje_till(); PickBall2();   turntime_fline=10;LEthr=1;REthr=1; linje_till(); DropBall(); SetPower(OUT_A+OUT_C,3);  Lthr=Lgrey;Rthr=Rgrey; linje_till(); PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);  FindWhite();  labyrint();}sub linje_till(){while (  !(    ( (LSENSOR>LEthr-2 && LSENSOR<LEthr+2) && (RSENSOR>REthr-2 && RSENSOR<REthr+2 ) ) /*Slutlinje träffad*/    |    (TSENSOR && gate==false) /*Trycksensor träffad*/    ) ) {  OnFwd(OUT_A+OUT_C);  if( (RSENSOR<Rthr+2) ) { LineRight(); }  else if ( (LSENSOR<Lthr+2) ) { LineLeft(); } }}void FindBlack(){ OnFwd(OUT_A+OUT_C); while(RSENSOR>Rblack+2);}void FindWhite(){ PlaySound(SOUND_DOWN); OnRev(OUT_A+OUT_C); while(LSENSOR<Lwhite-1) { LabTimer++; Wait(1); } PlaySound(SOUND_UP); Off(OUT_A+OUT_C);}sub GetTimerValues(){ until(TSENSOR);Rwhite=RSENSOR;Lwhite=LSENSOR;Wait(20);PlaySound(SOUND_CLICK); until(TSENSOR);Rgrey=RSENSOR;Lgrey=LSENSOR;Wait(20);PlaySound(SOUND_CLICK); until(TSENSOR);Rblack=RSENSOR;Lblack=LSENSOR;Wait(20);PlaySound(SOUND_CLICK);   until(TSENSOR);Wait(20);}sub labyrint(){ int labturns=9; int direction=1; int turntime=40;  while(labturns!=0) {  OnFwd(OUT_A+OUT_C);Wait(LabTimer);  direction++;  switch(direction)  {   case 1:   case 2:    OnRev(OUT_A);    Wait(turntime);   break;   case 3:    OnRev(OUT_C);    Wait(turntime);   break;   case 4:    OnRev(OUT_A);    Wait(turntime);    direction=0;   break;  }    direction++; }  }