task main()
{
waitForStart();
while (true) {
// Drive controls
motor[leftDrive] = joystickValue(1, 1, 2);
motor[rightDrive] = joystickValue(1, 2, 2);
// Tread controls
if (joy1Btn(CONTROLLER_R1)) {
// Suck in
motor[leftTread] = TREAD_SPEED;
motor[rightTread] = TREAD_SPEED;
servo[blockLoader] = BLOCK_LOADER_OUT;
} else if (joy1Btn(CONTROLLER_R2)) {
// Push out
motor[leftTread] = -TREAD_SPEED;
motor[rightTread] = -TREAD_SPEED;
if (ServoValue(blockLoader) > BLOCK_LOADER_IN)
servo[blockLoader] = ServoValue[blockLoader] - 3;
} else {
motor[leftTread] = 0;
motor[rightTread] = 0;
}
// Rotation controls
if (joy1Btn(CONTROLLER_L1)) {
motor[rotator] = MOTOR_SPEED_ROTATOR;
} else if (joy1Btn(CONTROLLER_L2)) {
motor[rotator] = -MOTOR_SPEED_ROTATOR;
} else {
motor[rotator] = 0;
}
// Twister controls
if (joy1Btn(CONTROLLER_Y)) {
servo[flagExtender] = 152;
} else if (joy1Btn(CONTROLLER_A)) {
servo[flagExtender] = 115;
} else {
//servo[flagExtender] = 135;
}
if (joy1Btn(CONTROLLER_X)) {
motor[flagMotor] = 100;
} else if (joy1Btn(CONTROLLER_B)) {
motor[flagMotor] = -100;
} else {
motor[flagMotor] = 0;
}
// Correctional controls -- Controller 2
if (joy2Btn(CONTROLLER_L1)) {
servo[flipper] = ServoValue[flipper] + 10;
} else if (joy2Btn(CONTROLLER_L2)) {
servo[flipper] = ServoValue[flipper] - 10;
}
}
}
void initializeRobot()
{
servo[claw] = 39; // Store Claw
ClearTimer(T1);
ClearTimer(T2);
ClearTimer(T3);
ClearTimer(T4);
servo[rightgrab] = grabDownPosition;//put to starting position
while(ServoValue(rightgrab) != grabDownPosition) {}
batteryTest();
return;
}
void initializeRobot()
{
ClearTimer(T1);
ClearTimer(T2);
ClearTimer(T3);
ClearTimer(T4);
servo[leftgrab] = grabDownPosition;//put grabber at starting position
while(ServoValue(leftgrab) != grabDownPosition) {}
batteryTest();
servo[claw] = 39;
return;
}
task main()
{
waitForStart(); // Wait for the tele-op period to begin
nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm
servoTarget(clawL) = 225; // Initialize the claw to be open
servoTarget(clawR) = 60;
PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun
wait1Msec(10); // Wait one tenth of a second
while (true)
{
getJoystickSettings(joystick); // Read the value of the joysticks
nxtDisplayTextLine(6, "Encoder: %d", nMotorEncoder[motorLift1]); // Display the value of the arm encoder
// The following code makes the directional pad on controller 1 give precise digital control of the drivetrain:
if (joystick.joy1_TopHat == 0) // If "up" on the directional pad is pressed:
{
go_forward(25); // Go forward at 25% speed
}
else if (joystick.joy1_TopHat == 1) // Else if "top right" on the directional pad is pressed:
{
strafe_forward_right(25); // Strafe diagonally forward and to the right at 25% speed
}
else if (joystick.joy1_TopHat == 2) // Else if "right" on the directional pad is pressed:
{
strafe_right(25); // Strafe to the right at 25% speed
}
else if (joystick.joy1_TopHat == 3) // Else if "bottom right" on the directional pad is pressed:
{
strafe_backward_right(25); // Strafe diagonally backward and to the right 25% speed
}
else if (joystick.joy1_TopHat == 4) // Else if "down" on the directional pad is pressed:
{
go_backward(25); // Go backward 25% speed
}
else if (joystick.joy1_TopHat == 5) // Else if "bottom left" on the directional pad is pressed:
{
strafe_backward_left(25); // Strafe diagonally backward and to the left 25% speed
}
else if (joystick.joy1_TopHat == 6) // Else if "left" on the directional pad is pressed:
{
strafe_left(25); // Strafe left 25% speed
}
else if (joystick.joy1_TopHat == 7) // Else if "top left" on the directional pad is pressed:
{
strafe_forward_left(25); // Strafe diagonally forward and to the left 25% speed
}
else if (joy1Btn(5)) // Else if button 6 is pressed:
{
rotate_clockwise(25); // Rotate the robot clockwise 25% speed
}
else if (joy1Btn(6)) // Else if button 5 is pressed:
{
rotate_counter_clockwise(25); // Rotate the robot counter-clockwise 25% speed
}
// The following code makes the joysticks on controller 1 give analogue control of the drivetrain:
else
{
motor[motorFL] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 + joystick.joy1_x2); // Scale the motors to the average
motor[motorFR] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 - joystick.joy1_x2); //of the left joystick Y value and
motor[motorBR] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 - joystick.joy1_x2); //the right joystick X value
motor[motorBL] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 + joystick.joy1_x2);
}
// The following code assigns the shoulder buttons on controller 2 to open and close the claw:
if (joy2Btn(5))
{
servoTarget(clawL) = 55; // If button 5 is pressed on controller 2, close the claw
servoTarget(clawR) = 170;
}
else if (joy2Btn(6))
{
servoTarget(clawL) = 115; // If button 6 is pressed on controller 2, open the claw
servoTarget(clawR) = 100;
}
else if (joy2Btn(8))
{
servoTarget(clawL) = ServoValue(clawL) + 1; // While button 7 is held, slowly close the claw
servoTarget(clawR) = ServoValue(clawR) - 1;
}
else if (joy2Btn(7))
{
servoTarget(clawL) = ServoValue(clawL) - 1; // While button 8 is held, slowly open the claw
servoTarget(clawR) = ServoValue(clawR) + 1;
}
else
{
servoTarget(clawL) = ServoValue(clawL); // Otherwise, do not move the claw
servoTarget(clawR) = ServoValue(clawR);
}
// The following code gives analogue control of the arm with the joystick, and assigns buttons 1-4 to move the arm to their respective rows on the scoring rack
if (joy2Btn(1)) // If button 1 is pressed, move the arm to the lowest row on the scoring rack
{
moveArm(level1Value);
}
else if (joy2Btn(2)) // If button 2 is pressed, move the arm to the middle row on the scoring rack
{
moveArm(level2Value);
}
else if (joy2Btn(3)) // If button 3 is pressed, move the arm to the top row on the scoring rack
{
moveArm(level3Value);
}
else if (joy2Btn(4)) // If button 4 is pressed, lower the arm all the way
{
moveArm(0);
}
else
{
motor[motorLift1] = (joystick.joy2_y1 / 12); // Otherwise, control the arm with joystick 1 on controller 2
motor[motorLift1] = (joystick.joy2_y1 / 24); // Or control the arm at half speed with joystick 2 on controller 2
// Note: these values are divided by 12 and 24 to bring the arm speed down to a reasonable level
}
}
}