task usercontrol() { bLCDBacklight = true; startTask(speedControl); while (true) { if(vexRT[Btn8U] == 1){ /* autoDrive(-265, 265, 70, false); wait1Msec(500); autoDrive(1100, 1100, 70, true); wait1Msec(500); autoDrive(330, -330, 70, false); wait1Msec(500); autoDrive(-200, 200, 70, false); wait1Msec(500); autoDrive(1100, 1100, 70, true); wait1Msec(500); autoDrive(190, -190, 70, false); */ autoDrive(-210, 210, 70, false); wait1Msec(500); autoDrive(2100, 2120, 70, true); wait1Msec(500); autoDrive(260, -260, 70, false); wait1Msec(500); } if(vexRT[Btn8D] == 1){ autoDrive(500, 500, 70, false); autoDrive(-500, -500, 70, false); } //Display main battery displayLCDString(0, 0, "Btry"); sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed displayNextLCDString(mainBattery); //Display flywheel controls clearLCDLine(1); displayLCDString(1, 0, "F:"); sprintf(mainBattery2, "%d %d %d", target, SensorValue[leftEncoder], SensorValue[rightEncoder]); //Build the value to be displayed displayNextLCDString(mainBattery2); } }
/// Atomic move-rotate for easy autonomous control. void DriveMotorSubsystem::MoveRotate(std::function<float ()> distGet, std::function<float ()> angleGet, std::function<float ()> speedGet, float distance,float angle) { autoDrive([&](float dist){leftController->Set(dist);}, [&](float dist){rightController->Set(dist);}, distGet, angleGet, speedGet, distance,angle); }
task main() { int _dirAC = 0; initializeRobot(); int leftright; leftright=getInt("L = 0","R = 1",0,1); //leftright=0; //for testing waitForStart(); reInitializeGyro(); //to be sure //for maintenance servo[SV_Swivel]=C_SVSWIVELCENTER; servo[SV_TiltRight]=C_SVTILTRIGHTUP; servo[SV_TiltLeft]=C_SVTILTLEFTUP; servo[SV_LightSensor]=C_SVLIGHTSENSORIN; int scorePlace; float atarget; //drive to beacon reading location switch(leftright){ case 0: //LEFT face backward autoDrive(5,33.0); wait1Msec(200); //starting on left autoDrive(3,6.0); wait1Msec(200); autoRotate(96.0-fGyroAngle); //45 degrees clockwise should be in IR sensor position wait1Msec(100); angleAbortTest(20.0); reInitializeGyro(); //IR read _dirAC = HTIRS2readACDir(HTIRS2); if(testScore==0) _dirAC = 5; if(testScore>0) _dirAC = 6; //For testing if(_dirAC==5) { scorePlace=0; //left autoRotate(45.0-fGyroAngle); } else { atarget=autoRotate(22.5-fGyroAngle); _dirAC = HTIRS2readACDir(HTIRS2); if(testScore==1) _dirAC = 5; if(testScore>1) _dirAC = 6; //Foing if(_dirAC==5) { scorePlace=1; }//center else {scorePlace=2;}//right IncRotate(atarget,22.5); } break; case 1: //RIGHT autoDrive(7,34.0); wait1Msec(200);//starting on right autoDrive(1,4.0); wait1Msec(200); //IR read _dirAC = HTIRS2readACDir(HTIRS2); if(testScore==0) _dirAC = 5; if(testScore>0) _dirAC = 6; //For testing if(_dirAC==5) { scorePlace=0; //left autoRotate(45.0-fGyroAngle); } else { atarget=autoRotate(22.5-fGyroAngle); _dirAC = HTIRS2readACDir(HTIRS2); if(testScore==1) _dirAC = 5; if(testScore>1) _dirAC = 6; //Foing if(_dirAC==5) { scorePlace=1; }//center else {scorePlace=2;}//right IncRotate(atarget,22.5); } break; } angleAbortTest(20.0); //scorePlace=2; //Now facing corner and know to which peg to go /* hogCPU(); while(true){ nxtDisplayBigTextLine(1,"SP = %d",scorePlace); } releaseCPU(); */ switch(scorePlace){ case 0: ScoreLeft(); break; case 1: FakeScoreMiddle(); break; case 2: FakeScoreRight(); break; } }
void testScoreRight(){ /* autoDrive(1,44.5);//proceed to white line autoDrive(7,6.0); //pushup against platform autoDrive(3,1.0); //backaway a little autoRotate(-45.0);//rotate to face platform angleAbortTest(); //put up lift to lower height putLiftUpAuto(); //need to position servos hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(100); servo[SV_Swivel]=C_SVSWIVELRIGHT; angleAbortTest(); autoDrive(0,4.0);//move up to platform wait1Msec(100); */ //go up on platform float fGyroTarget=fGyroAngle; autoDrive(0,2.0); wait1Msec(100); ClearTimer(T4); while(time1[T4]<1000){ goMaintainGyro(0,0,fGyroTarget); } //autoDrive(0,4.0); //find and lock on white line int loc; loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT); if(range<5) StopAllTasks(); //direction*=-1; setLight(loc-10); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,2000); wait1Msec(100); ClearTimer(T3); int spower; int ioffset=20; while(time1[T3]<5000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); ioffset=ServoValue[SV_LightSensor]-220; spower=abs(2*ioffset); if(spower>15) spower=15; if(ioffset>0) { goGyro(-spower,0); } if(ioffset<0) { goGyro(spower,spower); } } allDriveStop(); //setLight(102); setLight(C_SVLIGHTSENSORIN); wait1Msec(200); return; //track line in to position scoring //or get in position and move in-- timer based int signX=1,signY=1,power=10; ClearTimer(T4); fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); //trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); } return; //score rings scoreRingsAuto(); //back out and reset autoDrive(3,10.0); //need to reposition servos servo[SV_Swivel]=C_SVSWIVELCENTER; wait1Msec(100); hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTUP; servo[SV_TiltLeft]=C_SVTILTLEFTUP; releaseCPU(); putLiftDownAuto(); }
void ScoreRight(){ int lightSTarget; //position for right scoring int loc; int spower; int ioffset; wait1Msec(200); autoDrive(1,43.5);//proceed to white line autoDrive(3,3.0); //pushup against platform autoDrive(7,9.0); //pushup against platform //autoDrive(3,1.0); //backaway a little autoRotate(0.0-fGyroAngle);//rotate to face platform wait1Msec(50); angleAbortTest(20.0); putLiftUpAuto(); //need to position servos hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(200); servo[SV_Swivel]=C_SVSWIVELRIGHT; ClearTimer(T3); //nudge up to platform while(time1[T3]<3000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } angleAbortTest(20.0); reInitializeGyro(); lightSTarget=130; loc=scanForMax(C_SVLIGHTSENSORIN-60,C_SVLIGHTSENSORMXOUT); direction*=-1; if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc+20); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,500); ClearTimer(T3); //left-right positioning while(time1[T3]<2000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(6*ioffset); if(spower>30) spower=30; if(ioffset>0) { goGyro(-spower,0); } if(ioffset<0) { goGyro(spower,0); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T3); //nudge up again while(time1[T3]<1000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } angleAbortTest(20.0); reInitializeGyro(); ClearTimer(T4); //bump up on platform while(time1[T4]<300) goHolo(0,70,0); allDriveStop(); wait1Msec(50); ClearTimer(T4);//fix rotation for some time while(time1[T4]<1000){ goMaintainGyro(0,0,0); } allDriveStop(); lightSTarget=60; //find and lock on white line and adjust position loc=scanForMax(C_SVLIGHTSENSORIN-60,C_SVLIGHTSENSORMXOUT); direction*=-1; setLight(loc); if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc+20); wait1Msec(100); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,1000); ClearTimer(T3); //position to score while(time1[T3]<3000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(3*ioffset); if(spower>15) spower=15; if(ioffset>0) { goGyro(-spower,spower); } if(ioffset<0) { goGyro(spower,-spower); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T4); while(time1[T4]<500) goRotLS(30); allDriveStop(); int signX=1,signY=1,power=15; //direction 7 ClearTimer(T4); float fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); if(time1[T4]>500) power=5; } allDriveStop(); //score rings scoreRingsAuto(); //back out and reset autoDrive(5,10.0); putLiftDownAuto(); return; }
//Auton task autonomous() { if(SensorValue[autonSelector] > 1000){ bLCDBacklight = true; //First shots startTask(speedControl); target = 8; clearTimer(T1); motor[intake] = 127; while(time1[T1] < 22000){ if(speed >= speeds[target]-2){ motor[feed] = 127; } else{ motor[feed] = 0; } } motor[feed] = 0; motor[intake] = 0; target = 6; wait1Msec(250); target = 5; wait1Msec(250); target = 4; wait1Msec(250); target = 3; wait1Msec(250); target = 2; wait1Msec(250); target = 1; wait1Msec(250); target = 0; wait1Msec(250); stopTask(speedControl); //Drive across autoDrive(-210, 210, 70, false); wait1Msec(300); autoDrive(2100, 2100, 70, true); wait1Msec(300); autoDrive(250, -250, 70, false); wait1Msec(300); autoDrive(100, 100, 70, false); wait1Msec(100); //Second shots startTask(speedControl); target = 8; clearTimer(T1); motor[intake] = 127; while(time1[T1] < 22000){ if(speed >= speeds[target]-2){ motor[feed] = 127; } else{ motor[feed] = 0; } } motor[feed] = 0; motor[intake] = 0; target = 6; wait1Msec(250); target = 5; wait1Msec(250); target = 4; wait1Msec(250); target = 3; wait1Msec(250); target = 2; wait1Msec(250); target = 1; wait1Msec(250); target = 0; wait1Msec(250); stopTask(speedControl); //Display flywheel controls clearLCDLine(0); displayLCDString(0, 0, "EL:"); sprintf(mainBattery, "%d RL:%d", 0, SensorValue[leftEncoder]); //Build the value to be displayed displayNextLCDString(mainBattery); //Display flywheel controls clearLCDLine(1); displayLCDString(1, 0, "ER:"); sprintf(mainBattery2, "%d RR:%d", 0, SensorValue[rightEncoder]); //Build the value to be displayed displayNextLCDString(mainBattery2); //Wait wait1Msec(2000); } else{ //First shots startTask(speedControl); target = 12; clearTimer(T1); motor[intake] = 127; while(time1[T1] < 9000){ if(speed >= speeds[target]){ motor[feed] = 127; } else{ motor[feed] = 0; } } motor[feed] = 0; motor[intake] = 0; target = 6; wait1Msec(250); target = 5; wait1Msec(250); target = 4; wait1Msec(250); target = 3; wait1Msec(250); target = 2; wait1Msec(250); target = 1; wait1Msec(250); target = 0; wait1Msec(250); stopTask(speedControl); } }
void ScoreMiddle(){ int lightSTarget=70; //proceed to white line autoDrive(1,26.0); autoDrive(3,3.0); //pushup against platform autoDrive(7,9.0); //pushup against platform autoDrive(3,1.0); //backaway a little autoRotate(-45.0);//rotate to face platform wait1Msec(200); angleAbortTest(); putLiftUpAuto(); //need to position servos hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(100); servo[SV_Swivel]=C_SVSWIVELRIGHT; angleAbortTest(); float fGyroTarget=fGyroAngle; autoDrive(0,4.0); wait1Msec(200); autoDrive(0,9); //go up on platform wait1Msec(200); ClearTimer(T4);//fix rotation while(time1[T4]<1500){ goMaintainGyro(0,0,fGyroTarget); } //find and lock on white line int loc; loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT); if(range<C_RANGESAFETY) StopAllTasks(); direction*=-1; setLight(loc+10); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,2000); wait1Msec(100); ClearTimer(T3); int spower; int ioffset; while(time1[T3]<5000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(3*ioffset); if(spower>15) spower=15; if(ioffset>0) { goGyro(-spower,spower); } if(ioffset<0) { goGyro(spower,0); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); //track line in to position scoring //or get in position and move in-- timer based int signX=1,signY=1,power=10; ClearTimer(T4); fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); if(time1[T4]>100) power=5; //trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); } angleAbortTest(); //SAFETY CHECK FOR LINE loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT); if(abs(loc-lightSTarget)>25) StopAllTasks(); setLight(C_SVLIGHTSENSORIN); //score rings scoreRingsAuto(); //back out and reset autoDrive(3,10.0); //need to reposition servos servo[SV_Swivel]=C_SVSWIVELCENTER; wait1Msec(100); hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTUP; servo[SV_TiltLeft]=C_SVTILTLEFTUP; releaseCPU(); //put lift down putLiftDownAuto(); }
task main() { int _dirAC = 0; int loc; initializeRobot(); waitForStart(); putLiftUpAuto(); //need to position servos hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(200); servo[SV_Swivel]=C_SVSWIVELRIGHT; ClearTimer(T3); while(time1[T3]<3000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } fGyroAngle=0; int lightSTarget; lightSTarget=120; loc=scanForMax(C_SVLIGHTSENSORIN-60,C_SVLIGHTSENSORMXOUT); direction*=-1; if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc+20); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,500); // while(true){ // nxtDisplayBigTextLine(1,"%d",loc); // } int spower; int ioffset; ClearTimer(T3); while(time1[T3]<2000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(6*ioffset); if(spower>30) spower=30; if(ioffset>0) { goGyro(-spower,0); } if(ioffset<0) { goGyro(spower,0); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T3); while(time1[T3]<1000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } fGyroAngle=0; // while(true){ // nxtDisplayBigTextLine(1,"%d",ServoValue[SV_LightSensor]); // } ClearTimer(T3); while(time1[T3]<50){ ClearTimer(T4); while(time1[T4]<300) goHolo(0,80,0); allDriveStop(); wait1Msec(50); } ClearTimer(T4);//fix rotation for some time while(time1[T4]<1000){ goMaintainGyro(0,0,0); } allDriveStop(); lightSTarget=70; //find and lock on white line and adjust position loc=scanForMax(C_SVLIGHTSENSORIN-60,C_SVLIGHTSENSORMXOUT); direction*=-1; setLight(loc); //while(true){ // nxtDisplayBigTextLine(1,"%d",loc); //} if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc+20); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,1000); wait1Msec(100); //int spower; //int ioffset; ClearTimer(T3); while(time1[T3]<3000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(3*ioffset); if(spower>10) spower=10; if(ioffset>0) { goGyro(-spower,spower); } if(ioffset<0) { goGyro(spower,-spower); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T4); while(time1[T4]<500) goRotLS(30); allDriveStop(); //track line in to position scoring int signX=1,signY=1,power=15; //direction 7 ClearTimer(T4); float fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); if(time1[T4]>500) power=5; //trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); } allDriveStop(); //score rings scoreRingsAuto(); //back out and reset autoDrive(5,10.0); putLiftDownAuto(); return; loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT); //if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc); wait1Msec(1000); //trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,5000); return; ClearTimer(T3); while(time1[T3]<5000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } ClearTimer(T3); while(time1[T3]<50){ ClearTimer(T4); while(time1[T4]<300) goHolo(0,80,0); allDriveStop(); wait1Msec(50); } }
void ScoreLeft(){ int lightSTarget; //light sensor target on left int loc; autoDrive(5,2.0); //a little adjustment backleft autoRotate(-2.0); autoDrive(7,45.0);//proceed to white line autoDrive(5,3.0); //backup a little (in case on platform) autoDrive(1,9.5); //pushup against platform autoDrive(5,0.5); //backaway a little wait1Msec(200); autoRotate(90.0-fGyroAngle); //rotate to face platform wait1Msec(50); angleAbortTest(20.0); //test the angle is OK ///////////////////// putLiftUpAuto(); //need to position servos left hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(200); servo[SV_Swivel]=C_SVSWIVELRIGHT; ///////////////////////// nudgeUpFront(3000); angleAbortTest(20.0); reInitializeGyro(); //reset servo angle loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT+60); if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc-20); wait1Msec(200); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,500); //lightSTarget=180; lightSTarget=110; leftRightAdjust(2000,lightSTarget); setLight(C_SVLIGHTSENSORIN); nudgeUpFront(1200); wait1Msec(100); angleAbortTest(20.0); reInitializeGyro(); bumpUpFrontA(); ClearTimer(T4);//fix rotation for some time while(time1[T4]<1000){ goMaintainGyro(0,0,0); } allDriveStop(); //find and lock on white line and adjust position loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT+60); setLight(loc-20); wait1Msec(200); if(range<C_RANGESAFETY) StopAllTasks(); //direction*=-1; //move right to correct line trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,500); lightSTarget=200; leftRightAdjust(2000,lightSTarget); //need to position servos left hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); servo[SV_Swivel]=C_SVSWIVELLEFT; //move diagonally to score lightSTarget=220; int ioffset,spower; ClearTimer(T3); while(time1[T3]<1500){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,40); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,40); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(3*ioffset); if(spower>15) spower=15; if(ioffset>0) { goGyro(-spower,-spower); } if(ioffset<0) { goGyro(spower,spower); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T4); //rotate in to score while(time1[T4]<500) goRotRS(-30); allDriveStop(); int signX=-1,signY=1,power=15; //direction 7 ClearTimer(T4); float fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); if(time1[T4]>500) power=5; } allDriveStop(); //score rings scoreRingsAuto(); //back out and reset autoDrive(3,10.0); putLiftDownAuto(); return; }
void ScoreMiddle(){ int lightSTarget; //position for right scoring int loc; int spower; int ioffset; autoDrive(5,3.0); autoRotate(-2.0); autoDrive(7,26);//proceed to white line //autoDrive(5,3.0); //pushup against platform autoDrive(1,9.5); //pushup against platform autoDrive(5,0.5); //backaway a little wait1Msec(200); autoRotate(87.0-fGyroAngle); //rotate to face platform wait1Msec(50); angleAbortTest(20.0); putLiftUpAuto(); //need to position servos hogCPU(); servo[SV_TiltRight]=C_SVTILTRIGHTDOWN; servo[SV_TiltLeft]=C_SVTILTLEFTDOWN; releaseCPU(); wait1Msec(200); servo[SV_Swivel]=C_SVSWIVELLEFT; autoDrive(0,0.5); ClearTimer(T3); //nudge up while(time1[T3]<3000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,30,0); allDriveStop(); wait1Msec(50); } angleAbortTest(20.0); reInitializeGyro(); ClearTimer(T4); //bump up while(time1[T4]<300) goHolo(0,80,0); allDriveStop(); wait1Msec(50); ClearTimer(T4);//fix rotation for some time while(time1[T4]<500){ goMaintainGyro(0,0,0); } allDriveStop(); autoDrive(0,3.75); ClearTimer(T4);//fix rotation for some time while(time1[T4]<500){ goMaintainGyro(0,0,0); } allDriveStop(); ClearTimer(T3); //nudge back wheels up while(time1[T3]<3000){ ClearTimer(T4); while(time1[T4]<50) goHolo(0,25,0); allDriveStop(); wait1Msec(50); } angleAbortTest(20.0); reInitializeGyro(); ClearTimer(T3); while(time1[T3]<50){ ClearTimer(T4); while(time1[T4]<300) goHolo(0,80,0); allDriveStop(); wait1Msec(50); } allDriveStop(); ClearTimer(T4);//fix rotation for some time while(time1[T4]<1000){ goMaintainGyro(0,0,0); } allDriveStop(); //find and lock on white line and adjust position loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT+60); //direction*=-1; setLight(loc); if(range<C_RANGESAFETY || loc>210) { autoDrive(6,0.5); loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT+60); setLight(loc); } else{ if(loc<170){ autoDrive(2,0.5); loc=scanForMax(C_SVLIGHTSENSORIN,C_SVLIGHTSENSORMXOUT+60); setLight(loc); } } if(range<C_RANGESAFETY) StopAllTasks(); setLight(loc-20); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,1000); wait1Msec(100); lightSTarget=200; ClearTimer(T3); //track line for scoring position while(time1[T3]<3000){ trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,50); allDriveStop(); trackEdge(C_SVLIGHTSENSORMXOUT,C_SVLIGHTSENSORIN,20); ioffset=ServoValue[SV_LightSensor]-lightSTarget; spower=abs(3*ioffset); if(spower>10) spower=10; if(ioffset>0) { goGyro(-spower,-spower); } if(ioffset<0) { goGyro(spower,spower); } } allDriveStop(); setLight(C_SVLIGHTSENSORIN); ClearTimer(T4); while(time1[T4]<700) goRotRS(-30); allDriveStop(); //track line in to position scoring int signX=-1,signY=1,power=15; //direction 7 ClearTimer(T4); float fGyroTarget=fGyroAngle; while(time1[T4]<1000){ goMaintainGyro(signX*power,signY*power,fGyroTarget); if(time1[T4]>500) power=5; } allDriveStop(); //score rings scoreRingsAuto(); //back out and reset autoDrive(3,10.0); putLiftDownAuto(); return; }