task main()
{
waitForStart();
initializeRobot();
wait1Msec(12000);
//sets seeker value
tHTIRS2DSPMode _mode = DSP_1200;
//Starts the first basket movements
moveRobot(firstBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to second basket movements
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to third basket movements
moveRobot(thirdBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches- 1, forwardSpeedMove, backwards);
}
//No sensor found so score in fourth basket
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
task main()
{
initializeRobot();
State desiredState;
// Initialize everything in the desired state to 0.
memset(&desiredState, 0, sizeof(desiredState));
desiredState.wristPosition = 20;
desiredState.tineLockPosition = 180;
waitForStart(); // wait for start of autonomous phase
//command format:
//command (State *state, int dir, int driveSpeed, int armSpeed, int wristPos, int liftSpeed, int time);
command(&desiredState, 0, 100, 0, desiredState.wristPosition, 0, 3000);
//raise lift and release arm
command(&desiredState, -1, 0, 0, desiredState.wristPosition, 100, 5000);
command(&desiredState, -1, 0, 0, 200, 0, 1);
}
task main()
{
initializeRobot();
waitForStart();
servoTarget[intakeServo] = 80;
//----------SAFE BEGIN------------
moveArm(100);
wait1Msec(600);
moveArm(0);
//---Move forward
moveStraight(6.0,100);
stopDriveTrain();
//---Turn Right
gyroCenterPivot(-30,100);
stopDriveTrain();
//---Move forward
moveStraight(20.0, 100);
stopDriveTrain();
//---Score
servo[intakeServo] = 150;
wait1Msec(700);
//---Move forward to get on ramp.
moveStraight(12.0, -100);
stopDriveTrain();
gyroCenterPivot(45,100);
stopDriveTrain();
moveStraight(60.0, 100);
stopDriveTrain();
//---Close box
servo[intakeServo] = 80;
wait1Msec(5000);
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[scoopL] = 255;
servo[scoopR] = 0;
MFD(64, 100, 1000000000);
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while (true)
{}
}
task main()
{
initializeRobot(); // Initialize The Robot's Servos and Motor's
waitForStart(); // wait for start of tele-op phase
StartTask(drive); // give driver control of the wheels
while (true)
{
getJoystickSettings(joystick);
// Gets Joystick Settings
//D-pad direction is up?
if (joystick.joy2_TopHat == TOPHAT_UP)
{
servoDestination += SERVO_RATE;
}
// D-pad direction is down?
if (joystick.joy2_TopHat == TOPHAT_DOWN)
{
servoDestination -= SERVO_RATE;
}
// Keep servo position values within the range [0, 255].
servoDestination = (servoDestination > 255) ? 255 : (((servoDestination < 0) ? 0 : servoDestination));
// Send destination to servo.
servo[lockingServo] = servoDestination;
// Don't increment variables too quickly.
wait1Msec(OneFrameMS);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//Start();
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
Move(10.0, 70.0); // drive forward 27 inches
wait10Msec(50);
Turn(90);
wait10Msec(50);
}
task main()
{
initializeRobot();
initialize_gyro();
starttask(process_gyro);
while(true)
{
nxtDisplayCenteredTextLine(3, "A gyro=%d", gyro.total/1000.0);
}
//move(1, 22.5);
//wait10Msec(500);
//move(-1,22.5);
//move forward, turn 45 degrees, move forward
//move(1, 24);
//wait10Msec(50);
//move(1, 0);
//turngyro_left(45.0, 50);
//move(1, 33.941);
}
task main()
{
initializeRobot();
while(true)
{
getJoystickSettings(joystick);
if(joy1_TopHat > 0)
{
for (int i = 1; i < 10; i++)
{
if (joy1Btn (i) == 1)
{
nxtDisplayCenteredTextLine( 1, "Button %d pushed.",i );
}
}
}
}
}
task main()
{
initializeRobot(); //Executes all code in the initializeRobot() function
StartTask( watchdog ); //Start watchdog task which is a failsafe for the Move function
waitForStart(); //Wait for the beginning of autonomous phase
wait10Msec(25); //Pause for 250 miliseconds
Move(23,75); //Move forward 23 inches at 75% power
wait10Msec(25); //Pause 250 miliseconds
motor[block_arm] = 100; //Give 100% power to the arm motor
wait10Msec(100); //Allow the arm to unfold for 1000 miliseconds
motor[block_arm] = 0; //Kill the power to the arm motor
wait10Msec(25); //Pause 250 miliseconds
Turn(-45);
wait10Msec(25); //Pause 250 miliseconds
Move(28,75);
wait10Msec(25); //Pause 250 miliseconds
}
task main()
{
initializeRobot();
waitForStart();
ClearTimer(T1);
move(-100);
while (time1[T1] < driveToOtherSide)
{
//nothing
}
turn(100);
ClearTimer(T1);
while (time1[T1] < turnTime){
//nothing
}
move(-100);
wait10Msec(60);
move(0);
}
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
while (true)
{
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific tele-op code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
// Insert code to have servos and motors respond to joystick and button values.
// Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
// You may be able to find "snippets" of code that are similar to the functions that you want to
// perform.
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
nMotorEncoder[armMotor] = 0;
nMotorEncoderTarget[armMotor] = 100;
motor[armMotor] = 20;
while(nMotorRunState[armMotor]!= runStateIdle){
}
motor[armMotor] = 0;
while (true)
{
}
}
task main()
{
bDisplayDiagnostics = false;
bNxtLCDStatusDisplay = false;
////////// INITIALIZATIONS //////////
initializeRobot(); // Execute robot initialization routine
/////////////// UNUSED VARIABLES /////////////////
//int maxArmHeightTicks = inchesToTicks(maxArmHeight);
//int minRingPickupHeightTicks = inchesToTicks(minRingPickupHeight);
//int WAMservoStep = 3; //Amount to inc1rement the WAM servo position
//int WAMservoStep12 = 17; // Move a servo 15 degrees
//float maxArmHeight = 45.25; // Maximum Safe Arm Height used during manual control of the arm
//float minRingPickupHeight = 8.0;
waitForStart(); // Wait for the signal to start from the Field Control System
//reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually
ProcessProto();
///////DISTANCES//////
static float cornerOfBlackPlatform = 24;
move_forward(cornerOfBlackPlatform, 2500, 50, 50);
wait10Msec(30);
PlaySound(soundBlip);
wait10Msec(30);
turngyro_left(45.0, 20);
//elevatorGoToHeight(selectLocation(5));
//elevatorGoToHeight(selectLocation(1));
//elevatorGoToHeight(selectLocation(4));
//elevatorGoToHeight(selectLocation(5));
}
task main()
{
initializeRobot();
State actualState;
State desiredState;
// Initialize everything in the desired state to 0.
memset(&desiredState, 0, sizeof(desiredState));
desiredState.wristPosition = 100;
desiredState.desiredDriveSpeed = DRIVESPEED;
//waitForStart(); // wait for start of tele-op phase
while (true)
{
// Wait for the next update from the joystick.
UserInput input;
getLatestInput(&desiredState, &input);
// Process the joystick input
handleDriveInputs(&desiredState, &input);
handleArmInputs(&desiredState, &input);
handleWristInputs(&desiredState, &input);
handleLiftInputs(&desiredState, &input);
handleTineInputs(&desiredState, &input);
handleOutriggerInputs(&desiredState, &input);
computeDriveMotorSpeeds(&desiredState);
computeActualState(&desiredState, &actualState);
updateAllMotors(&actualState);
// Display variable values for debugging purposes
showDiagnostics(&desiredState, &actualState, &input);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Off ramp
Move(-82.5, 0.5);
wait10Msec(10);
Move(-10, 0.2);
//grab goal
wait10Msec(60);
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(30);
Turn(30);
wait10Msec(10);
Move(110, 1);
wait10Msec(10);
Turn(150);
wait10Msec(10);
//score
motor[lift] = 75;
wait10Msec(800);
motor[lift] = 0;
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
//release goal
servo[hook1] = 255;
servo[hook2] = 0;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(180);
}
task main()
{
initializeRobot(); //Executes all code in the initializeRobot() function
StartTask( watchdog ); //Start watchdog task which is a failsafe for the Move function
waitForStart(); //Wait for the beginning of autonomous phase
wait10Msec(25); //Pause for 250 miliseconds
Move(1,40); //Move away from the wall 1 inch at 40% power
wait10Msec(25); //Pause for 250 miliseconds
motor[block_arm] = 100; //Give 100% power to the arm motor
wait10Msec(66); //Allow the arm to run for 660 miliseconds
motor[block_arm] = 0; //Kill the power to the arm motor
wait10Msec(25); //Pause for 250 miliseconds
servo[Claw] = 10; //Move block release servo to open position (10 degrees) allowing it to drop the block
wait10Msec(100); //Pause for 1000 miliseconds to allow the block to fall
motor[block_arm] = -100; //Give 100% power to the arm again but in the oppisite direction
wait10Msec(35); //Allow the arm to move back to a closed position
motor[block_arm] = 0; //Kill the power to the arm motor
wait10Msec(25); //Pause for 250 miliseconds
Turn(-80); //Turn 80 degrees away from the bucket
wait10Msec(25); //Pause for 250 miliseconds
Move(22,75); //Move 22 inches towards the ramp at 75% power
wait10Msec(25); //Pause for 250 miliseconds
Turn(125); //Turn 55 degrees towards the ramp
wait10Msec(25); //Pause for 250 miliseconds
Move(53,75); //Move onto the ramp for the end of the autonomous phase
wait10Msec(25); //Pause for 250 miliseconds
}
task main()
{
short zero = 6;
float liftmod = 75;
initializeRobot();
waitForStart();
while(true){
getJoystickSettings(joystick);
if(joystick.joy1_y1 > zero){ //up
if(liftCheck(liftmod)){
motor[lift] = liftmod;
}
}else if(joystick.joy1_y1 < -zero){ //down
if(liftCheck(liftmod)){
motor[lift] = -liftmod;
}
}else{
motor[lift] = 0;
}
}
}
task main()
{
initializeRobot();
State currentState;
memset(¤tState, 0, sizeof(State));
//waitForStart(); // Wait for the beginning of autonomous phase.
while (currentState.IRBeaconDir != 8)
{
getLatestInput(¤tState);
handleIRInputs(¤tState);
updateAllMotors(¤tState);
}
while (currentState.touched != 1)
{
getLatestInput(¤tState);
handleColorInputs(¤tState);
handleTouchInputs(¤tState);
updateAllMotors(¤tState);
}
}
task main()
{
initializeRobot();
waitForStart();
raiseArm(ArmUpRotations);
intializeClaw();
int _dirAC = 0;
wait1Msec(200);
int sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 5){
raiseArm(LeftArmUpRotations);
leftTurn(leftTurnRotations);
attackRack(leftFinalInches);
}
else{
firstMove(middleStartInches);
wait1Msec(300);
sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 7){
leftTurn(middleTurnRotations);
attackRack(middleFinalInches);
}
else{
raiseArm(RightArmUpRotations);
firstMove(rightStartInches - middleStartInches);
leftTurn(middleTurnRotations);
attackRack(rightFinalInches);
}
}
retreat();
}
task main()
{
bool goesback=false;
initializeRobot();
waitForStart();
distForward(42);wait1Msec(300);
ClearTimer(T1);
while(SensorValue[LSonar]>8 && time1(T1)<2000)
{
standardDrive(50,50);
}
standardDrive(0,0);wait1Msec(300);
if(SensorValue[LSonar]>8)
{
goesback=true;ClearTimer(T1);
while(SensorValue[LSonar]>8 && time1(T1)<1000)
{
standardDrive(-100,-100);
}
}
standardDrive(0,0);wait1Msec(300)
if(!goesback)
{
distForward(15);wait1Msec(200);
}
else
{
distForward(19);wait1Msec(200);
}
turnRSideTo(45);wait1Msec(300);
slowBackward(60);wait1Msec(200);
slowForward(8);wait1Msec(300);
turnRSideTo(90);wait1Msec(300);
slowBackward(24);wait1Msec(300);
standardDrive(0,0);wait1Msec(300);
}
task main()
{
initializeRobot();
while(nNxtButtonPressed != nxtOrange)
{
displayStatus();
}
waitTillNoButton();
eraseDisplay();
resetEncoders();
while(nNxtButtonPressed != nxtOrange)
{
while(nxtGrey != nNxtButtonPressed)
{
displayString(4,"PICK UP AND PRESS GREY BTN");
}
while((nMotorEncoder[rightFront] < 1800)||(nMotorEncoder[rightBack] < 1800)||(nMotorEncoder[leftBack] < 1800)||(nMotorEncoder[leftFront]< 1800))
{
eraseDisplay();
setMotors(70,70,70,70);
}
stopMotors();
displayString(1, "should be 1800");
displayString(3, "rightback encoder: %d", nMotorEncoder[rightFront]);
displayString(2, "rightfront encoder: %d", nMotorEncoder[rightBack]);
displayString(4, "leftfront encoder: %d", nMotorEncoder[leftFront]);
displayString(5, "leftback encoder: %d", nMotorEncoder[leftBack]);
}
waitTillNoButton();
}
task main()
{
//////////////////////////////////////////////////////////////////////////////////
waitForStart(); // Wait for the beginning of autonomous phase.
initializeRobot();
//////////////////////////////////////////////////////////////////////////////////
if(OnRamp)
{
moveTime(100,5000);//get off the ramp
turn(90);
moveTime(100,1100);
turn(-90);
moveTime(100,3000);
servo[Tower1]=15;
servo[Autonomous1] = 100;
servo[Autonomous2] = 155;
wait1Msec(1000);
motor[LBDMotor] = 0;
motor[LFDMotor] = 0;
motor[RBDMotor] = 0;
motor[RFDMotor] = 0;
wait1Msec(150000000);
}
}
task main()
{
initializeRobot();
waitForStart();
turnLeft(_45DEGREES - 35);
delayStop(10);
moveBackward(32);
delayStop(10);
turnLeft(_90DEGREES);
delayStop(10);
timedMove(5, 100, -1); // 5 seconds, 100 power, backwards
stopDrive();
delayStop(0);
timedMove(3, 100, 1); // 3 seconds, 100 power, forward
stopDrive();
delayStop(0);
timedMove(4, 100, -1); // 4 seconds, 100 power, backwards
stopDrive();
delayStop(0);
gotoIR_FORWARD();
storeEncoderValues();
resetEnc();
delayStop(300);
turnRight(_90DEGREES - 35);
resetEnc();
delayStop(0);
}
task main()
{
initializeRobot();
//6yui7o9ywaitForStart(); // Wait for the beginning of autonomous phase.
//servo[stilt] = 35;
float COUNTS_PER_INCH = 18.482;
int DistFrom = 14;
int Deadband = 1;
while((nMotorEncoder[ leftDrive] / COUNTS_PER_INCH) <= 80 && ((nMotorEncoder[rightDrive] / COUNTS_PER_INCH) <= 80))
{
if ((SensorValue[SONAR_1] < DistFrom - Deadband))
{
motor[leftDrive] = 30;
motor[rightDrive] = 7;
}
else if ((SensorValue[SONAR_1] > DistFrom + Deadband))
{
motor[leftDrive] = 7;
motor[rightDrive] = 30;
}
}
if (SensorValue[IR] = 6)
{
Turn(90);
wait10Msec(55);
Move(10);
wait10Msec(55);
}
if (SensorValue[IR] !=6)
{
Move
task main()
{
int forwardLeft, forwardRight, reverse=1;
initializeRobot();
waitForStart(); // wait for start of tele-op phase- used for the Samantha Module
while (true)
{
getJoystickSettings(joystick);
//joystick handling
if(joy1Btn(5)==1){
//if button 5 is pressed, robot will only drive straight forward/back, ignoring any turns
forwardLeft=(reverse*joystick.joy1_y1)/2;
forwardRight=forwardLeft;
}else{
//normal joystick motion
forwardLeft=(reverse*joystick.joy1_y1+joystick.joy1_x1)/2;
forwardRight=(reverse*joystick.joy1_y1-joystick.joy1_x1)/2;
}
//cancels out joystick defects
if(abs(forwardLeft)<10){
forwardLeft=0;}
if(abs(forwardRight)<10){
forwardRight=0;}
//Drive!
motor[driveLFront]=forwardLeft;
motor[driveLBack]=forwardLeft;
motor[driveRFront]=forwardRight;
motor[driveRBack]=forwardRight;
}
}
task main()
{
initializeRobot();
while(true)
{
getJoystickSettings(joystick);
main_wheel(joystick.joy1_x1, joystick.joy1_y1);
lift(joystick.joy1_x2);
if ( joy1Btn(8)==1 )
elevMoveFree(joystick.joy1_y2);
else
elevMove(joystick.joy1_y2);
resetElevEncoder(joy1Btn(3));
servoAction(joy1Btn(7));
displayMessage(joy1Btn(1));
wait1Msec(TIME_INTERVAL);
}
}
task main()
{
writeDebugStreamLine("initalizing");
initializeRobot();
writeDebugStreamLine("initalized");
#ifdef COMPETITION
waitForStart();
#else
while (nNxtButtonPressed != kEnterButton){}
#endif
rotateDegrees(45);
while (nNxtButtonPressed != kRightButton){}
rotateDegrees(45);
while (nNxtButtonPressed != kRightButton){}
if (TestHook == 1) {
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
while (nNxtButtonPressed != kRightButton){}
hookGrab();
wait1Msec(3000);
moveInches(-20);
hookUngrab();
while (nNxtButtonPressed != kLeftButton){}
}
if (TestHardware == 1) {
writeDebugStreamLine("Test Hardware");
trapDoorOpen();
trapDoorClose();
servo[collector] = COLLECTOR_IDLE;
setPosition (tas, POS_BALL_COLLECTING, DEFAULT_DISTANCE);
wait10Msec(300);
servo[collector] = COLLECTOR_IN;
wait10Msec(300);
servo[collector] = COLLECTOR_IDLE;
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
wait10Msec(1000);
trapDoorOpen();
wait10Msec(3000);
}
if (TestPositions == 1) {
writeDebugStreamLine("Test Positions");
// while (nNxtButtonPressed != kRightButton){}
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kLeftButton){}
// setPosition (tas, POS_AT_60CM, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kRightButton){}
//Test 90
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kLeftButton){}
// setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kRightButton){}
// trapDoorOpen();
// while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while(nNxtButtonPressed != kEnterButton){}
moveInches(24);
while (nNxtButtonPressed != kRightButton){}
trapDoorOpen();
while(nNxtButtonPressed != kEnterButton){}
moveInches(-24);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE+3.5);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
trapDoorOpen();
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_30CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
//while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
//while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
}
#if 0
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
wait10Msec(200); //wait 2 sec for raise
//moveInches(50);
moveInches(54);
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
wait10Msec(1000);
//moveInches(14);
moveInches(10);
trapDoorOpen();
wait10Msec(500);
moveInches(-20);
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
wait10Msec(2000);
#endif
}
task main()
{
initializeRobot();
//Comment out for tests
//waitForStart();
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while (true)
{
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*+++++++++++++++++++++++++++++++++++++++++++++| Notes |++++++++++++++++++++++++++++++++++++++++++++++
Conveyorbot - Remote Control
- NOTE: Conveyorbot can be used to pick up 4 of the game objects with its intake and score them
anywhere on the field.
- This program allows you to remotely control your robot using the USB Logitech Joysticks.
- This program also ignores low values that would cause your robot to move when the joysticks fail to
return back to exact center. You may need to adjust the 'threshold' value to get it just right.
Robot Model(s): TETRIX Conveyorbot (Virtual Worlds)
[I/O Port] [Name] [Type] [Description]
Motor Port D rightMotor TETRIX Motor Right side motor
Motor Port E leftMotor TETRIX Motor Left side motor
Motor Port F armMotor TETRIX Motor Arm motor
Motor Port G intakeMotor TETRIX Motor Intake motor
Sensor Port 2 gyro HiTechnic Gyro HiTechnic Gyro
Sensor Port 3 light Light Sensor NXT Light Sensor (Active)
Sensor Port 4 sonar Sonar Sensor NXT Sonar Sensor
*** Competition Information ***
Name:
Competition:
Average FPS (displayed on the Virtual World):
*** Program Information ***
Name of Program:
Type of Robot:
Any other relevant information:
----------------------------------------------------------------------------------------------------*/
//Integer variable that allows you to specify a "deadzone" where values (both positive or negative)
//less than the threshold will be ignored.
while (1==1) //Loop Forever
{
//Get the Latest joystick values
getJoystickSettings(joystick);
//Variable Control
int threshold = 10; //Motor Variables
int wSpeed = 10;
int hookDown = 10; //Servo Variables
int hookUp = 10;
int bucketUp = 10;
int bucketDown = 10;
//Operator Control Joy 2
if(joystick.joy2_TopHat == 0) //Up and Down on D-Pad adjust bucket height
{
motor[wenchA] = wSpeed;
motor[wenchB] = wSpeed;
}
else if(joystick.joy2_TopHat == 4)
{
motor[wenchA] = -wSpeed;
motor[wenchB] = -wSpeed;
}
else
{
motor[wenchA] = 0;
motor[wenchB] = 0;
}
//---
if(joy2Btn(4) == 1) //Buttons 4 and 1 control hook position
{
servo[hook] = hookDown;
}
if(joy2Btn(1) == 1)
{
servo[hook] = hookUp;
}
//---
if(joy2Btn(2) == 1) //Buttons 2 and 3 control bucket position
{
servo[bucket] = bucketDown;
}
if(joy2Btn(3) == 1)
{
servo[bucket] = bucketUp;
}
//---
//Driving Control Joy 1
if(abs(joystick.joy1_y1) > threshold) // If the abs. value of the right analog stick's Y-axis readings are above the threshold...
{
motor[frontLeft] = joystick.joy1_y1; // ...move the right side of the robot.
motor[backLeft] = joystick.joy1_y1;
}
else // Else the readings are within the threshold, so...
{
motor[frontLeft] = 0; // ...stop the right side of the robot.
motor[backLeft] = 0;
}
//---
if(abs(joystick.joy1_y2) > threshold) // If the abs. value of the left analog stick's Y-axis readings are above the threshold...
{
motor[frontRight] = joystick.joy1_y2; // ...move the left side of the robot.
motor[backRight] = joystick.joy1_y2;
}
else // Else the readings are within the threshold, so...
{
motor[frontRight] = 0; // ...stop the left side of the robot.
motor[backRight] = 0;
}
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
while(true)
{
bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase.
eraseDisplay();
// WriteShort(hFileHandle, nIoResult, nParm);
if ( externalBatteryAvg < 0)
{
while (externalBatteryAvg < 0)
{
eraseDisplay();
nxtDisplayTextLine(1, "Ext Batt: OFF"); //External battery is off or not connected
nxtDisplayCenteredBigTextLine(1, "What!?");
nxtDisplayTextLine(3, "My name is,");
nxtDisplayTextLine(4, "Iego Montoya,");
nxtDisplayTextLine(5, "you did not turn");
nxtDisplayTextLine(6, "on the robot");
nxtDisplayTextLine(7, "NOW YOU DIE");
nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!");
PlayImmediateTone(600, 20);
PlayImmediateTone(400, 20);
wait10Msec(20);
}
}
else
{
nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);
}
while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7)
{
eraseDisplay();
nxtDisplayBigTextLine(6, "Battery");
nxtScrollText("poopheads");
nxtScrollText("You didnt change the battery you shmuts!");
nxtScrollText("the battery you");
nxtScrollText("shmuts!");
PlayImmediateTone(600, 70);
PlayImmediateTone(400, 70);
wait10Msec(20);
}
nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage
nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]);
nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1));
nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]);
nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]);
wait10Msec(20);
}//End of While
//Actuation tests.
motor[R_Motor] = 100;
wait10Msec(30);
motor[R_Motor] = 0;
wait10Msec(30);
motor[L_Motor] = 100;
wait10Msec(30);
motor[L_Motor] = 0;
wait10Msec(30);
motor[Flag_Raiser] = 100;
wait10Msec(30);
motor[Flag_Raiser] = 0;
wait10Msec(20);
motor[Winch] = 100;
wait10Msec(30);
motor[Winch] = 0;
wait10Msec(30);
motor[Arm] = 100;
wait10Msec(30);
motor[Arm] = 0;
wait10Msec(30);
motor[Block_Sweep] = 100;
wait10Msec(30);
motor[Block_Sweep] = 0;
wait10Msec(30);
servo[Bucket_Release] = 100;
wait10Msec(30);
servo[Bucket_Release] = 0;
wait10Msec(30);
servo[Spring_Release] = 100;
wait10Msec(30);
servo[Spring_Release] = 0;
wait10Msec(30);
servo[IRS_1] = 100;
wait10Msec(30);
servo[IRS_1] = 0;
wait10Msec(30);
}
//=========================================
// Main Program
//=========================================
task main()
{
initializeRobot();
waitForStart(); // wait for FCS to tell us to go!
if(done)
{
StartTask(lightDiagnostic);
}
if(calibrate != 2) // GYRO calibration hasn't been run during the wait time
{
gyroCalTime = 3; // so setup the default calibrate time
calibrate = 1; // start the calibration going
while(calibrate != 2) // and wait for it to complete before moving the robot
{
EndTimeSlice();
}
}
constHeading = 0; // reset the GYRO headings to eliminate any drift while we waited
relHeading = 0; // same thing for relative heading
wait1Msec(Start_Delay*1000); // implement the user configurable delay before moving
PlaySound(soundBeepBeep); // tell everyone we're about to start going
step = MissionNumber*100; // this records the actual mission number that we ran within the data log file
LogData=true; // start saving data to the log file
servo[wrist] = WRIST_CLOSED;
servo[shoulder] = SHOULDER_DOWN;
servo[right_servo] = RIGHT_GRIPPER_START;
servo[left_servo] = LEFT_GRIPPER_START;
switch(MissionNumber) // now go run whichever mission we have been asked to run
{
//================================================
// start on the right side of the blue dispenser delivers to IR
//================================================
case 1:
IRside = position1();//go here
if(IRside < -75) column = 1;
else if(IRside < -25) column = 2;
else column = 3;
wait1Msec(800);
switch(column)
{
case 1://left
diagnosticBeeps(1);
leftPeg();
break;
case 2://center
diagnosticBeeps(2);
centerPeg();
break;
case 3://right
diagnosticBeeps(3);
rightPeg();
break;
}
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PEG
//================================================
case 2:
position1();
centerPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// LEFT PEG
//================================================
case 3:
position1();
leftPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// RIGHT PEG
//================================================
case 4:
position1();
rightPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// IR SECOND POSITION
//================================================
case 5:
IRside = position2();//go here
if(IRside < -75) column = 1;
else if(IRside < -25) column = 2;
else column = 3;
wait1Msec(800);
switch(column)
{
case 1://left
diagnosticBeeps(1);
leftPeg();
break;
case 2://center
diagnosticBeeps(2);
centerPeg();
break;
case 3://right
diagnosticBeeps(3);
rightPeg();
break;
}
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PED SECOND POSITION
//================================================
case 6:
position2();
centerPeg();
break;
//================================================
// LEFT PEG SECOND POSITION
//================================================
case 7:
position2();
leftPeg();
break;
//================================================
// RIGHT PED SECOND POSITION
//================================================
case 8:
position2();
rightPeg();
break;
//================================================
// DEFENSE RIGHT
//================================================
case 9:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,40,true,true,false,true);
break;
//================================================
// DEFENSE LEFT
//================================================
case 10:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,-40,true,true,false,true);
break;
//================================================
// OPPONENT IR
//================================================
case 11:
for (int i=0;i<90;i++)
{SetDrive(i,50,i);
wait1Msec(30);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// OPPONENT CENTER
//================================================
case 12:
opponentRight();
centerPeg();
break;
//================================================
// OPPONENT LEFT
//================================================
case 13:
opponentRight();
rightPeg();
break;
//================================================
// OPPONENT RIGHT
//================================================
case 14:
opponentRight();
leftPeg();
break;
//================================================
// OPPONENTL IR
//================================================
case 15:
opponentLeft();
leftPeg();
break;
//================================================
// OPPONENTL CENTER
//================================================
case 16:
opponentLeft();
centerPeg();
break;
//================================================
// OPPONENTL LEFT
//================================================
case 17:
opponentLeft();
rightPeg();
break;
//================================================
// OPPONENTL RIGHT
//================================================
case 18:
opponentLeft();
leftPeg();
break;
//================================================
// See Opponent test
//================================================
case 19:
sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
StopRobot();
if(sawOpponent) diagnosticBeeps(6);
break;
//================================================
// See Opponent test2
//================================================
case 20:
sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
StopRobot();
if(sawOpponent) diagnosticBeeps(6);
break;
}
LogData=false; // stop logging IR data and close the file
StopRobot();
}
