void chooseTube(eTube Tube)
{
if(Tube == eshort)
{
gyroTurn(50, 0, dRight);
travelDistance(1);
captureTube();
}
else if(Tube == emid)
{
gyroTurn(50, 90, dRight);
travelDistance(1);
captureTube();
}
else if(Tube == etall)
{
gyroTurn(50, 45, dRight);
travelDistance(1);
captureTube();
}
else
{
}
}
void placeTube(int possision)
{
if(possision == 1)
{
gyroTurn(70,50,dRight);
travelDistance(1);
captureTube();
}
else if(possision == 2)
{
gyroTurn(70, 50, dRight);
travelDistance(2);
captureTube();
}
else if(possision == 3)
{
gyroTurn(70, 45, dRight);
travelDistance(1);
captureTube();
}
else
{
}
releaseTube();
}
void autoRamp(bool useLift)
{
travelDistance(200, dBackward);
gyroTurn(30, 15, dRight);
goalRelease();
wait1Msec(100);
backward(15);
wait1Msec(1500);
stopMotors();
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHDOWN; // Set servo position
wait1Msec(50);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
wait1Msec(100);
travelDistance(10, dBackward);
strafe(75);
wait1Msec(500);
stopMotors();
travelDistance(100, dForward);
gyroTurn(30, 2, dLeft);
travelDistance(150, dForward);
gyroTurn(20, 100, dRight);
goalMed(useLift);
}
void autoFloor(bool useLift)
{
int irValue = getIRReading();
strafeDist(40, 100, dRight);
displayCenteredTextLine(1, "IR = %i", irValue);
if (irValue == 2)
{
strategyA(useLift);
}
else
{
irValue = getIRReading();
if (irValue == 2)
{
strategyA(useLift);
}
else
{
gyroTurn(10, 5, dRight);
int irValue1 = getIRReading();
gyroTurn(10, 10, dLeft);
int irValue2 = getIRReading();
eraseDisplay();
displayCenteredTextLine(2, "IR1 = %i", irValue1);
displayCenteredTextLine(3, "IR2 = %i", irValue2);
if (irValue1 == 2 || irValue2 == 2)
{
gyroTurn(10, 5, dRight);
strategyA(useLift);
}
else
{
strafeDist(40, 75, dRight);
gyroTurn(50, 30, dLeft);
resetEncoders();
while (irValue != 6)
{
irValue = getIRReading();
strafe(100);
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "distance=%i", enc);
wait1Msec(1);
}
stopMotors();
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(1, "enc = %i", enc);
if (enc < 1500)
{
strategyB(useLift);
}
else
{
strategyC(useLift);
}
}
}
}
}
void strategyZ() //use if in position 1, from ramp
{
displayCenteredTextLine(1, "strategy Z"); //display which strategy it chose
travelDistance(distanceZ1, dForward); //
gyroTurn(30, 90, dLeft);
travelDistance(distanceZ2, dForward);
gyroTurn(30, 90, dRight);
travelDistance(distanceZ3, dForward);
}
void strategyB(bool useLift)
{
displayCenteredTextLine(6, "Strategy B");
//strafeDist(15, 30, dRight);
gyroTurn(30, 15, dRight);
travelDistance(15, dBackward);
goalCenter(useLift);
gyroTurn(30, 10, dLeft);
strafeDist(45, 50, dLeft);
travelDistance(125, dBackward);
}
void strategyC(bool useLift)
{
displayCenteredTextLine(6, "Strategy C");
//strafeDist(50, 75, dRight);
gyroTurn(30, 48, dRight);
strafeDist(25, 75, dRight);
travelDistance(21, dBackward);
goalCenter(useLift);
gyroTurn(30, 10, dLeft);
strafeDist(45, 75, dLeft);
travelDistance(125, dBackward);
}
void strategyA(bool useLift)
{
displayCenteredTextLine(6, "Strategy A");
gyroTurn(30, 90, dLeft);
stopMotors();
wait1Msec(100);
travelDistance(40, dBackward);
wait1Msec(100);
strafeDist(27, 100, dLeft);
wait1Msec(100);
goalCenter(useLift);
strafeDist(45, 50, dLeft);
gyroTurn(30, 10, dLeft);
travelDistance(125, dBackward);
}
void strategyY() //use if in position 2, from ramp
{
displayCenteredTextLine(1, "strategy Y"); //display which strategy it chose
travelDistance(distanceY1, dForward); //travel forward a certain distance
gyroTurn(30, 45, dLeft); //turn left 45 degrees
travelDistance(distanceY1, dForward); //travel forward a certain distance
}
void getTube(eTube tube, int kickstandP)
{
if(kickstandP == 1)
{
strafe(1/*not real*/);
wait1Msec(1/*not real*/);
travelDistance(1/*not real*/);
gyroTurn(50,90,dRight);
travelDistance(1/*not real*/);
chooseTube(tube);
}
else if(kickstandP == 2)
{
chooseTube(tube);
}
else if(kickstandP == 3)
{
chooseTube(tube);
}
else
{
}
}
task autonomous()
{
SensorType[in8] = sensorNone;
wait1Msec(1000);
//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in8] = sensorGyro;
wait1Msec(2000);
if(SensorValue(limit1) == 0) //hang a cube from the back, move backwards until the limit switch is pressed
{
motor[port1] = -100;
motor[port2] = -100;
}
else if(SensorValue(limit1) == 1)
{
wait1Msec(1000); //wait for the cube to release from the robot
motor[port1] = 100;
motor[port2] = 100;
}
gyroTurn(45);//turns 45 degrees
drive_straight(sqrt(2), 50);//returns to starting position
drive_straight(1.5, 50);
//do something with intake
if(SensorValue(limit1) == 0)
{
motor[port1] = -100;
motor[port2] = -100;
}
}
void strategyX() //use if in position 3, from ramp
{
displayCenteredTextLine(1, "strategy X"); //display whch strategy it chose
strafe(50); //strafe right at half power
wait1Msec(timeX1); //keep going for a certain amount of time
stopMotors(); //stop
travelDistance(distanceX1, dForward); //travel forward a certain distance
gyroTurn(30, 80, dLeft); //turn left 80 degrees
travelDistance(distanceX2, dForward); //travel forward a certain distance
}
task main()
{
SensorType[in8] = sensorNone;
wait1Msec(1000);
//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in8] = sensorGyro;
wait1Msec(2000);
//int X2 = 0, Y1 = 0, X1 = 0, threshold = 15;
gyroTurn(900);
}
task usercontrol()
{
gyroTurn(setAngle);
}
task autonomous()
{
//Turn the robot by the desired angle
gyroTurn(-setAngle);
}
task main()
{
wait1Msec(3);
int beginning = beginInterface();
wait1Msec(500);
int ending = interface();
wait1Msec(500);
int timing = timeinterface();
servo(servo3) = 0;
waitForStart();
wait10Msec(timing * 100);
initialize();
wait10Msec(30);
switch(beginning) // test 'nTaskToStart' in the switch
{
case 2:
// Move away from the wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(4);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the west wall
gyroTurn(20, 87);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
// Drive toward the west wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(25);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the baskets
gyroTurn(-20, 40);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
case 3:
//Move away from the wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(2);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
case 4:
case 1:
default:
}
//-------------------------------
// We're aligned with the baskets.
// Start walking along them looking for the IR beacon
motor[MOTOR_LEFT] = -60;
motor[MOTOR_RIGHT] = -63;
driveTo(4);
if(CheckIR(4) || checkIR(5)) // The first basket
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
wait10Msec(4);
servoTarget(servo3) = 180;
wait10Msec(50);
//forward/back
}else { // The second basket
driveTo(13);
if(checkIR(4) || checkIR(5))
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180;
wait10Msec(150);
//forward/back
}else{ // The third basket
driveTo(35);
if(sensorValue[IRsensor] == 6 || sensorValue[IRsensor] == 5)
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180;
wait10Msec(150);
//place block
//forward/back
}else{ // The fourth basket
driveTo(45);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180; // Deploy the auto-scoring arm
wait10Msec(150);
//forward/back
}
}
}
servoTarget[servo3] = 0; // Retract the auto-scoring arm
switch(ending) // test 'nTaskToStart' in the switch
{
case 1: // if 'nTaskToStart' is '1':
// Drive to the end of the baskets.
motor[MOTOR_LEFT] = 100;
motor[MOTOR_RIGHT] = 97;
while(abs(nMotorEncoder[MOTOR_RIGHT]) > 2000)
{
}
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the ramp
gyroTurn(20, 65);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
// Drive in front of the ramp
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(20);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the ramp agai
gyroTurn(20, 30);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(25);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-20, 85);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -100;
motor[MOTOR_RIGHT] = -100;
driveTo(40);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
break; // break out of this switch statement and continue code after the '}'
case 2: // if 'nTaskToStart' is '2':
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(51);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(10);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-40, 70);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -50;
motor[MOTOR_RIGHT] = -50;
driveTo(35);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-40, 90);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(35);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
/* gyroTurn(-20, 70);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(40);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0; */
// start task Two
break; // break out of this switch statement and continue code after the '}'
default: // if 'nTaskToStart' is anything other than '1' or '2':
// start task Three
}
}
task
autonomous(){
//Turn the robot by the desired angle
gyroTurn(SET_ANGLE);
}