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;
}
