void paralellWall()
{
int rearSensor = SensorValue[S4] -2;
bool aligned = false;
while (!aligned)
{
rearSensor = SensorValue[S4] -2;
nxtDisplayTextLine(3, "s4: %d s3:%d", rearSensor, SensorValue[S3]);
if (closeEnough(rearSensor, SensorValue[S3]))
{
//Already close enough, we're done.
aligned = true;
}
else if (SensorValue[S3] > rearSensor)
{
//Front of robot is futher from the wall then back, small turn.
turnTime(25, "left", 150);
wait1Msec(100);
}
else if (SensorValue[S3] < rearSensor)
{
//Back of robot is further, small turn.
turnTime(25, "right", 150);
wait1Msec(100);
}
else
{
//Odd situation, no matches of the case list. Exit.;
aligned = true;
}
//next part, moving walk.
}
}
task main() {
runDriveTime(DRIVE_SPEED, TO_BUCKET_TIME);
wait1Msec(3000);
//runArmTime(ARM_SPEED,DROP_ARM_TIME);
//runIntakeTime(INTAKE_SPEED, RUN_INTAKE_TIME);
//runArmTime(-(ARM_SPEED), DROP_ARM_TIME);
runDriveTime(-DRIVE_SPEED, BACKOUT_TIME);
wait1Msec(3000);
turnTime(-TURN_SPEED, TURN_90_TIME);
wait1Msec(3000);
runDriveTime(DRIVE_SPEED, FORWARD_BEYOND_RAMP_TIME);
wait1Msec(3000);
turnTime(TURN_SPEED, TURN_90_TIME);
wait1Msec(3000);
runDriveTime(DRIVE_SPEED, FORWARD_TO_RAMP_TIME);
wait1Msec(3000);
turnTime(TURN_SPEED, TURN_90_TIME);
wait1Msec(3000);
runDriveTime(DRIVE_SPEED, FORWARD_ONTO_RAMP_TIME);
}