task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart();
disableDiagnosticsDisplay();
//Initialize the gyro and turning
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
//Start actual movement code
moveForwardInches(60,43);//initial forwards
turn(g_PidTurn,45,20);
clearEncoders();
wait1Msec(50);
const int totalTics = 6806;//total tics from before IR to end- CHANGED FOR LESSENED AMOUNT OF FORWARDS
const int ticsToCenter = 3663;//tics from start to central beam
const int ticsToSubtract = 1665;//failsafe, may still need testing
//finding IR
while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //finds the beacon zone 4 (rough)
//nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
startBackward(27);
}
stopDrive();
wait1Msec(300);
while(HTIRS2readACDir(IR) != 5 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //slow down to look for basket (fine)
startForward(15);
}
int currentPosition = abs(nMotorEncoder[leftDrive]);
if (currentPosition > ticsToCenter)//check where we are
moveForwardInchesNoReset(20, 6);//move forwards 5 inches (buckets 1 and 2)
else
moveForwardInchesNoReset(20, 3);//forwards 3 inches (buckets 3 and 4)
stopDrive();//stops robot
servo[dumper] = 30;//dumps the block
motor[lift]= 50;//starts the lift up
wait1Msec(700);
motor[lift]= 0;//stops lift
servo[dumper] = servoRestPosition;//resets servo
int ticsToMove= totalTics - abs(nMotorEncoder[leftDrive]);//tics left after IR
displayCenteredTextLine(0,"TTM: %d", ticsToMove);
moveBackwardTics(90, ticsToMove); //move to end after IR
turn(g_PidTurn, -87,40); //turn to go towards ramp
moveForwardInches(90, 44, false, LEFTENCODER); //forwards to ramp
turn(g_PidTurn, 95, 40); //turn to face ramp
moveForwardInches(90, 45, false, LEFTENCODER);//onto ramp
motor[lift]= -50;//starts the lift down
wait1Msec(500);
motor[lift]= 0;//stops lift
while(true){}
}
void initializeRobot()
{
servoTarget[brake] = 65;
servoTarget[RightFlap] = 241;
servoTarget[LeftFlap] = 18;
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
GyroInit(g_Gyro, Gyro, 0);
return;
}
void initializeRobot() {
disableDiagnosticsDisplay();
//Choose an auto routine.
selectedAutoRoutine = getRouteChoice();
eraseDisplay();
selectedDelayTime = getDelayChoice();
nMotorEncoder[Arm] = 0;
nMotorEncoder[Joint] = 0;
GyroInit(gyroSensor);
resetEncoders();
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
//Align against bottom wall, with left edge of left wheels on left edge of third tile (6ft from right wall).
countdown(timeToWait);
moveForwardInches(80, 30, false, LEFTENCODER); //away from wall
turn(g_PidTurn, 90); //turn to parallel with buckets
moveBackwardInches(90,20,false,LEFTENCODER);
while (true)
{}
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
moveBackwardInches(80, 30, false, LEFTENCODER);
turn(g_PidTurn, -90);
moveBackwardInches(90,20,false,LEFTENCODER);
while (true)
{}
}
void initializeRobot()
{
GyroInit(gyro, gyroSensor, 0);
return;
}
