task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35; // In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;
wait10Msec(1500);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin)
{
nxtDisplayCenteredTextLine(1, "%d",USreadDist(SONAR_1));
nxtDisplayCenteredTextLine(2, "%d",SensorValue[IR]);
#if 1
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
#endif
}//End of while.
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
servo[Block_Chuck] = 255;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
while( USreadDist(SONAR_1) < 60)
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
}//End of while
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
Turn(65);
wait10Msec(55);
Move(25,100);
wait10Msec(55);
Turn(75);
wait10Msec(55);
Move(30 ,100);
wait10Msec(55);
servo[Block_Chuck] = 250;
wait10Msec(55);
servo[Spring_Release] = 250;
wait10Msec(55);
servo[Spring_Release] = 0;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
/* 1. We will need a sweep task.
2. Pathfinding move function.
A. Pythagorean theorum to find the sides
B. Try both sides of the obsticle if one side is obstructive.
C. 45 degrees.
*/
/*Scan = true;
while (Scan)
{
if (servo[IRS_1] == 0)
{
servo[IRS_1] = 255;
}
else if (servo[IRS_1] == 255)
{
servo[IRS_1] = 0;
}
}//End of while*/
} // End main task
task main()
{
short right_y;
short left_y;
initializeRobot();
waitForStart(); // wait for start of tele-op phase
StartTask(endGameTimer);
displayForward();
while (true)
{
getJoystickSettings(joystick);
if (!debounce) {
if (joy2Btn(RIGHT_TRIGGER_UP)) {
handle_event(RIGHT_TRIGGER_UP);
} else if (joy2Btn(RIGHT_TRIGGER_DOWN)) {
handle_event(RIGHT_TRIGGER_DOWN);
} else if (joy2Btn(LEFT_TRIGGER_UP)) {
handle_event(LEFT_TRIGGER_UP);
} else if (joy2Btn(LEFT_TRIGGER_DOWN)) {
handle_event(LEFT_TRIGGER_DOWN);
} else if (joy2Btn(BUTTON_ONE)) {
handle_event(BUTTON_ONE);
} else if (joy2Btn(BUTTON_FOUR)) {
handle_event(BUTTON_FOUR);
} else if (joy1Btn(BUTTON_ONE)) {
handle_joy1_event(BUTTON_ONE);
} else if (joy1Btn(BUTTON_FOUR)) {
handle_joy1_event(BUTTON_FOUR);
}
}
//if (drive_multiplier) {
//right_y = joystick.joy1_y2;
//left_y = joystick.joy1_y1;
//} else {
right_y = joystick.joy1_y1;
left_y = joystick.joy1_y2;
//}
if (abs(right_y) > 20) {
motor[driveFrontRight] = drive_multiplier * right_y;
motor[driveRearRight] = drive_multiplier * right_y;
}
else {
motor[driveFrontRight] = 0;
motor[driveRearRight] = 0;
}
if (abs(left_y) > 20) {
motor[driveFrontLeft] = drive_multiplier * left_y;
motor[driveRearLeft] = drive_multiplier * left_y;
}
else
{
motor[driveFrontLeft] = 0;
motor[driveRearLeft] = 0;
}
}
}
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
while (true)
{
getJoystickSettings(joystick);
//drive code (right or left?)
if(joystick.joy1_y2 > -15 && joystick.joy1_y2 < 15)//if joysticks are near center power is 0
{
motor[leftDrive] = 0;
}
else if(joy1Btn(1) == 1){//extra control to go straight forward
motor[leftDrive] = 50;
}
else
{
motor[leftDrive] = joystick.joy1_y2*04;//otherwise power is half of the joystick value
}
//same for other drive wheels (right or left?)
if(joystick.joy1_y1 > -15 && joystick.joy1_y1 < 15)
{
motor[rightDrive] = 0;
}
else if(joy1Btn(1) == 1){//extra control to go straight forward
motor[rightDrive] = 70;
}
else
{
motor[rightDrive] = joystick.joy1_y1*0.65;
}
//lid controls
if(joy2Btn(5) == 1){//servo is almost all the way down to help with positioning (prev. 32)
servo[lidServo] = 223;
}
else if(joy2Btn(7) == 1){//
servo[lidServo] = 200;
}
else if(joy2Btn(6) == 1){//lid is shut
servo[lidServo] = 180;
}
//flipper servo controls
if(joy2Btn(1) == 1){
servo[flipperServo] = 255;//flips the box over to score in the center goal.
}
else if(joy2Btn(3) == 1){
servo[flipperServo] = 0;//moves the box so it is on the flat of the gear, so it can be folded into the arm.
}
else if(joy2Btn(2) == 1){
servo[flipperServo] = 100;//moves the box in position to score in the highest rolling goal.
}
//Harvester controls (button 6 is high speed, button 8 is low speed)
if(joy1Btn(6) == 1)//harvester only runs when arm is down
{
motor[harvester] = 40;
}
else if(joy1Btn(8) == 1)//reverses harvester
{
motor[harvester] = -40;
}
else
{
motor[harvester] = 0;
}
//arm controls (btn 10 up, btn 9 down)
if(joy2Btn(10) == 1 && getArmPosition() != ARM_EXTENDED)//when the arm isn't up it can go up
{
motor[arm] = 100;
}
else if(joy2Btn(9) == 1 && getArmPosition() != ARM_FOLDED)//when arm isn't down it can go down
{
motor[arm] = -80;
}
else
{
motor[arm] = 0;
}
//goal grabber controls
if(joy1Btn(7) == 1){
servo[goalServo] = 100;
}
else if(joy1Btn(5) == 1){
servo[goalServo] = 64;
}
//else{
//}
}//end while
}
task main()
{
initializeRobot();
int countline = 0;
waitForStart(); // Wait for the beginning of autonomous phase.
wait1Msec(10000);
motor[slide] = -20;//slide over
wait1Msec(750);
motor[slide] = 0;
ClearTimer(T1);
SensorValue[irSeek] = 0;
motor[leftDrive] = 70;
motor[rightDrive] = 70;
servo[rightgrab] = grabDownPosition;//set to start position
while(time1[T1] < 9000)
{
if(atLine())
{
countline++;
if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6)//checks if at the ir beacon
{
// at Correct Line
break;
}
else if(countline==1)// if hit the 1st line turn slightly
{
motor[rightDrive]= 0;
motor[leftDrive] = 9;
wait1Msec(500);
}
else if(countline == 2){//if hit the 2nd line turn slightly
motor[rightDrive] = 0;
motor[leftDrive] = 1;
wait1Msec(500);
}
motor[leftDrive] = 75;//drive
motor[rightDrive] = 75;
wait1Msec(500);
}
}
if(time1[T1] < 9000)//while less thatn 9 seconds
{
nMotorEncoder[leftDrive] = 0;
nMotorEncoder[rightDrive] = 0;
if(countline == 1){
forward(8.9); //Distance after line till stop
stopDrive();
wait1Msec(500);
servo[rightgrab] = grabReleasePositionpeg1;//put grabber in position to place ring
wait1Msec(500);
motor[slide] = 40;//move slide over
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[rightgrab] = grabReleasePositionpeg1 + 10;//move slightly to help put ring on
wait1Msec(1000);
forward(7);//go forward to pull ring off
}
else if(countline == 2)
{
forward(8.1);//Distance after line till stop
stopDrive();
wait1Msec(500);
servo[rightgrab] = grabReleasePositionpeg2;//put grabber in position to place ring
wait1Msec(500);
motor[slide] = 40;//move over to put ring on
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[rightgrab] = grabReleasePositionpeg2 -8;//move slightly to put ring on
wait1Msec(1000);
forward(7);//go forward to pull ring off
}
else if(countline == 3){
forward(8.97);//Distance after line till stop
stopDrive();
wait1Msec(500);
servo[rightgrab] = grabReleasePositionpeg3;//move grabber to position to place ring on 3 peg
wait1Msec(500);
motor[slide] = 40;//move over to place ring on
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[rightgrab] = grabReleasePositionpeg3 +5;//move slightly to help put ring on
wait1Msec(1000);
forward(7);//go forward to pull ring off
}
}
else
stopDrive();//if not less than 9 seconds stop
nMotorEncoder[leftDrive] = 0;//clea encoders
nMotorEncoder[rightDrive] = 0;
while (true) {}
}
task main()
{
// The IR signal strengh in all 5 directions.
int IRdirA = 0;
int IRdirB = 0;
int IRdirC = 0;
int IRdirD = 0;
int IRdirE = 0;
waitForStart();
initializeRobot();
// The amount of time the robot...
const int forwardTimeAA = 25;
const int turnTimeA = 50;
const int forwardTimeA = 170;
const int turnTimeB = 110;
const int forwardTimeB = 100;
const int liftTimeB = 45;
const int forwardTimeCA = 110; //TODO
const int forwardTimeCB = 40; //TODO
const int turnTimeD = 152;
const int forwardTimeD = 110;
const int liftTimeD = 135;
const int forwardTimeE = 95; //TODO
const int turnTimeF = 112;
const int forwardTimeF = 80;
const int liftTimeF = 47;
const int liftTimeG = 30; //TODO
const int backwardTimeG = 100; //TODO
const int turnTimeG = 70; //TODO
const int forwardTimeG = 20; //TODO
const int liftTimeH = 50; //TODO
const int backwardTimeH = 90; //TODO
const int turnTimeH = 100; //TODO
const int forwardTimeH = 70; //TODO
const int liftTimeI = 30; //TODO
const int backwardTimeI = 130; //TODO
const int turnTimeI = 70; //TODO
const int forwardTimeI = 170; //TODO
const int forwardTimeJ = 50; //TODO
const int turnTimeK = 90; //TODO
const int liftTimeK = 30; //TODO
const int forwardTimeK = 50; //TODO
Move_Forward (forwardTimeAA, g_AccurateMotorPower);
Turn_Left (turnTimeA, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeA, g_AccurateMotorPower);
Time_Wait(50);
HTIRS2readAllDCStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
{
Turn_Right (turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeB, g_AccurateMotorPower);
Move_Forward (forwardTimeB, g_AccurateMotorPower);
Lift_Down (liftTimeG, g_AccurateMotorPower);
Move_Backward (backwardTimeG, g_AccurateMotorPower);
Turn_Right (turnTimeG, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeG, g_AccurateMotorPower);
}
else
{
Move_Forward (forwardTimeCA, g_AccurateMotorPower);
Time_Wait(50);
HTIRS2readAllACStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
Move_Forward (forwardTimeCB, g_AccurateMotorPower);
if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
{
Turn_Right (turnTimeD, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeD, g_AccurateMotorPower);
Move_Forward (forwardTimeD, g_AccurateMotorPower);
Lift_Down (liftTimeH, g_AccurateMotorPower);
Move_Backward (backwardTimeH, g_AccurateMotorPower);
Turn_Right (turnTimeH, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeH, g_AccurateMotorPower);
}
else
{
Move_Forward (forwardTimeE, g_AccurateMotorPower);
Turn_Right (turnTimeF, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeF, g_AccurateMotorPower);
Move_Forward (forwardTimeF, g_AccurateMotorPower);
Lift_Down (liftTimeI, g_AccurateMotorPower);
Move_Backward (backwardTimeI, g_AccurateMotorPower);
Turn_Right (turnTimeI, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeI, g_AccurateMotorPower);
}
}
Move_Forward (forwardTimeJ, g_AccurateMotorPower);
Turn_Right (turnTimeK, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeK, g_AccurateMotorPower);
Move_Forward (forwardTimeK, g_AccurateMotorPower);
while (true)
{
PlaySoundFile("moo.rso");
while(bSoundActive == true)
{
Time_Wait(1);
}
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
int IRvalue = SensorValue[IR];
wait10Msec(140);
IRvalue = 7;
if(IRvalue < 5 || IRvalue > 7)
{
IRvalue = 0;
}
//Robot takes reading of the IR
if(IRvalue==5)//Far
{
wait10Msec(25);
Move(52,75);
wait10Msec(55);
Turn(42);
wait10Msec(55);
Arm_move(149);
wait10Msec(55);
motor[Lift] = 75;
wait10Msec(25);
motor[Lift] = 0;
Move(28.5,75);
wait10Msec(55);
Arm_move(1);
wait10Msec(55);
Move(-1,75);
wait10Msec(55);
Arm_move(1);
wait10Msec(55);
Move(-1,75);
wait10Msec(55);
Arm_move(3);
wait10Msec(55);
Move(-15,75);
wait10Msec(55);
}
if(IRvalue==6 || IRvalue==0)//Middle
{
wait10Msec (55);
Move(27,75);
wait10Msec(55);
Turn(51);
wait10Msec(55);
Arm_move(154);
wait10Msec(55);
motor[Lift] = 75;
wait10Msec(32);
motor[Lift] = 0;
Move(42.5,80);
wait10Msec(55);
Arm_move(12);
wait10Msec(55);
Move(-15,75);
wait10Msec(55);
Turn(50);
wait10Msec(55);
Move(-10,75);
wait10Msec(55);
}
if(IRvalue==7)//Close
{
wait10Msec(55);
Move(4,60);
wait10Msec(55);
Turn(80);
wait10Msec(55);
Move(25,75);
wait10Msec(55);
Turn(-36);
wait10Msec(55);
Arm_move(149);
wait10Msec(55);
motor[Lift] = 75;
wait10Msec(25);
motor[Lift] = 0;
Move(26,75);
wait10Msec(10);
Arm_move(22);
wait10Msec(55);
Move(-7,75);
wait10Msec(55);
}
}
// the starting point of the program
task main()
{
// prompt the user for a delay at the start of autonomous
selectTime();
// prompt whether the robot should wait for FCS to start
// They can disable it when testing the program at practice.
startSelect();
// wait for FCS to start if the user has chosen to
if(waitSelected)
{
waitForStart();
}
// perform the aforementioned delay
wait1Msec(1000*waitTime);
//holds which beacon the robot is at
int beacon = 0;
//holds the IR sensor value
int i = 0;
//loops until the sensor finds the beacon or until it gets to the final beacon.
while(i != 5 && beacon < 4)
{
i=0;
//this section prevents the reading of random IR values and makes sure the value is constant.
//when the value is the constant we want, the beacon is in front of the sensor.
for(int a = 0; a <100; a++)
{
if(a == 0)
{
//gets original ir value
i = SensorValue[IR];
}
else
{
int b = SensorValue[IR];
if(b != i)
{
//if a different value is sensed, then the original value was not constant; thus, we restart the checking
//to try and gete a constant value.
i = 0;
a = 0;
}
}
}
if(i != 5)
{
if(beacon == 0)
{
//the distance to get to the first beacon
ModForward(2200,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 1)
{
//the distance to get to the second beacon
ModForward(900,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 2)
{
//the distance to get to the third beacon
ModForward(1600,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 3)
{
//the distance to get to the fourth beacon
ModForward(1000,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
beacon++;
}
}
//code that runs after we stop in front of a beacon.
servo[AutoHook] = 255; //dispenses the cube
wait1Msec(900);
servo[AutoHook] = 0;
wait1Msec(1000);
//pick where to go based on beacon #.
//if the robot is at rings 1 or 4, it turns, goes back, then turns to face the ramp.
//if the robots is at rings 2 or 3, it goes backwards, turns, goes backwards again, and then turns to face the ramp.
if(beacon == 1) //beacon was on first box
{
//goes backwards from the box
ModBackward(1760,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns to be parallel to the ramp
ModTurn(1840, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes backwards and towards the ramp
ModBackward(4000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns towards the ramp
ModTurn(2100, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes onto the ramp
ModBackward(4500,100);
}
else if (beacon == 2) //beacon was on second box
{
//goes backwards from the box
ModBackward(3400,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns to be parallel to the ramp
ModTurn(1840, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes backwards and towards the ramp
ModBackward(4000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns towards the ramp
ModTurn(2100, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes onto the ramp
ModBackward(4500,100);
}
else if (beacon == 3) //beacon was on third box
{
//goes backwards from the box
ModBackward(5000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns to be parallel to the ramp
ModTurn(1840, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes backwards and towards the ramp
ModBackward(4000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns towards the ramp
ModTurn(2100, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes onto the ramp
ModBackward(4500,100);
}
else if (beacon == 4) //beacon was on fourth ring
{
//goes backwards from the box
ModBackward(6000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns to be parallel to the ramp
ModTurn(1840, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes backwards and towards the ramp
ModBackward(4000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//turns towards the ramp
ModTurn(2100, 100, right);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
//goes onto the ramp
ModBackward(4500,100);
}
}
//=============================================================================================================================================
//---------------------------------------------------BEGIN INITIALIZATION CODE-----------------------------------------------------------------
task main() {
//Initialize the display with the program choices
chooseProgram();
switch (PROGID) {
case 1:
FORWARD_SCORE_FORWARD_LINE_1 = true;
linesToFind = 1;
break;
case 2:
FORWARD_SCORE_FORWARD_LINE_2 = true;
linesToFind = 2;
break;
case 3:
FORWARD_SCORE_BACKWARD_LINE_1 = true;
linesToFind = 1;
break;
case 4:
FORWARD_SCORE_BACKWARD_LINE_2 = true;
linesToFind = 2;
break;
case 5:
useDummyAutonomous();
break;
case 6:
//useOriginalAutonomous();
PlaySoundFile("Woops.rso");
break;
}
//---------------------------------------------------------END INITIALIZATION CODE-------------------------------------------------------------
//=============================================================================================================================================
//if (PROGID == 1 || PROGID == 2 || PROGID == 3 || PROGID == 4) {
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
//PlaySound(soundBlip);
//wait1Msec((2 * PI) * 1000); //TAUUUU
//wait1Msec(10000);//wait 10 seconds for other teams who **might** have better autonomous code
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
servo[servoLift] = 123;
servo[servoSweep] = 199;
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
//float irval = acS3L;
//StringFormat(irvalres, "%3.0f", irval);
//WriteText(irFileHandle, IOResult, "Test");
//WriteString(irFileHandle, IOResult, irvalres);
//WriteByte(irFileHandle, IOResult, 13);
//WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
//if it doesnt find the beacon, dont bother returning to start if it has been set to
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
if (time1[T1] < 2000) {
wait1Msec(600);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
//irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
//Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
driveToEnd(-80, timeToEnd - roundTime);//drive to end of ramp
}
if (FORWARD_SCORE_BACKWARD_LINE_1 || FORWARD_SCORE_BACKWARD_LINE_2) {
driveToEnd(80, roundTime);
}
}
wait1Msec(300);
//=======================================================================================================================================
//------------------------BEGIN 90 DEGREE TURNS------------------------------------------------------------------------------------------
//HTGYROstartCal(HTGYRO);
ClearTimer(T3);
while (true) {
wait1Msec(20);
//if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degFirstTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
//---------------LINE DETECTOR--------------------------
LSsetActive(LEGOLS);
while (linesFound < linesToFind) {
motor[motorLeft] = -50;
motor[motorRight] = -50;
wait1Msec(10);
if (LSvalNorm(LEGOLS) >= WHITE_LINE_VALUE) {
linesFound++;
}
if (linesFound >= linesToFind) { //ever-present failsafe
break;
LSsetInactive(LEGOLS);
}
}
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degSecondTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
} else {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = -60;
motor[motorRight] = 60;
if (heading <= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
}
break;
}
}
//==================================================================================
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
float heading = 92;
HTGYROstartCal(HTGYRO);
while (koth) {
//wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
MissionImpossible();
/*
}//END MAIN IF PROGIDS THING
else if (PROGID == 5) {
useDummyAutonomous();
}
*/
}//task main
task main()
{
int threshold = 40;
/*//removed for testing
servo[Winch]=127;
*/
//
// going straight
//
int straight_FRS = 136;
int straight_BRS = 146;
int straight_FLS = 132;
int straight_BLS = 134;
// mandible servo positions
//
int openwideR = 142;
int openwideL = 100;
int shutR = 0;
int shutL = 228;
// mouth
int mouthup=60;
int mouthdown=255;
//
int elevatorposition=0;
int uppositionscore=62;
int downpositionscore= 5;
// gate
int gateup = 5;
int gatedrive = 50;
int gatedown = 202;
//
int lockup = 0;
int lockdown = 85;
//
int scoreopen = 5;
int scoreclose = 245;
int FRS =0;
int BRS = 0;
int FLS = 0;
int BLS = 0;
// servo initialization
servo[leftmandible] = openwideL;
servo[rightmandible] = openwideR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
//
// wheels go straight to start
//
servo[frontRS]= straight_FRS;
servo[backRS]= straight_BRS;
servo[frontLS]= straight_FLS;
servo[backLS]= straight_BLS;
int lastservopos = 0; // 1 = straight, 2 = spin , 3 = turn 45 deg to right, 4 = turn 45degrees to left, 5 = turn wheels to side
waitForStart();
//turnServos(1, lastservopos);
servo[gate] = gatedrive;
while(true)
{
getJoystickSettings(joystick); //Constantly updates values on the joystick
// left joystick
if((abs(joystick.joy1_y1) > threshold)|| (abs(joystick.joy1_x1) > threshold) || (abs(joystick.joy1_x2) > threshold))
{
if ((abs(joystick.joy1_y1) > threshold) && (abs (joystick.joy1_x1) > threshold)) // going 45 degrees
{
BRS = FRS = BLS = FLS = (abs(joystick.joy1_y1) * 100)/joystick.joy1_y1; // TAKE POWER AND DIRECTION from Y axis
if (((joystick.joy1_y1 * joystick.joy1_x1)) > 0) // if both positive or both negative go to right
{
turnServos(3, lastservopos); // turn wheels to right to go 45 degrees
lastservopos = 3;
} else
{
turnServos(4, lastservopos); // turn wheels to left to go 45 degrees
lastservopos = 4;
}
} else if (((abs(joystick.joy1_y1) > threshold)) &&(abs(joystick.joy1_x1) <= threshold)) // go straight
// if going forward or back
{
BRS = FRS = BLS = FLS = (abs(joystick.joy1_y1)*100)/joystick.joy1_y1;
// servos
turnServos(1, lastservopos);
lastservopos = 1;
// motors
} else if ((abs(joystick.joy1_x1) > threshold) && (abs(joystick.joy1_y1) <= threshold)) // go sideways
{
BRS = FRS = BLS = FLS = (abs(joystick.joy1_x1)*100)/joystick.joy1_x1; // positive = go right, negative = go left.
// servos
turnServos(5, lastservopos); // turn wheels to right to go sideways
lastservopos = 5;
// motors
} else if (abs(joystick.joy1_x2) > threshold) // spin
{
FRS = BRS = -(abs(joystick.joy1_x2)*100)/joystick.joy1_x2; // positive = go right, negative = go left.
BLS = FLS = -FRS; // have to switch direction to make it spin.
// servos
turnServos(2, lastservopos); // turn wheels to right to go sideways
lastservopos = 2;
}
}else
{
BRS = FRS = BLS = FLS = 0;
}
// Now tell motors to go
motor[RightFront] = FRS;
motor[LeftFront] = FLS;
motor[RightBack] = BRS;
motor[LeftBack] = BLS;
// Mandible controls
if(joy1Btn(8))
{
servo[rightmandible] = shutR;
servo[leftmandible] = shutL;
}
if(joy1Btn(6))
{
servo[rightmandible] = openwideR;
servo[leftmandible] = openwideL;
}
/*
// Mouth controls
if(joy1Btn(4))
{
servo[mouth]=mouthup;
wait1Msec(30);
servo[rightmandible] = openwideR; // open mandibles so they don't hit elevator
servo[leftmandible] = openwideL;
}
if(joy1Btn(2))
{
servo[mouth]=mouthdown;
}
///////////// JOYSTICK 2 //////////////////
// elevator controls
if(abs(joystick.joy2_y2) > threshold)// absolute value so can be negative or positive
{
motor[elevator] = 100*joystick.joy2_y2/abs(joystick.joy2_y2);//Raises the elevator
}
else
{
motor[elevator] = 0;
}
*/
// scoring
if(joy2Btn(8))
{
servo[score] = scoreopen;
}
else
{
servo[score]=scoreclose;
}
// rolling goal
if(joy2Btn(2)) // gate down to capture rolling goal
{
servo[gate] = gatedown;
}
if (joy2Btn(4)) // gate up
{
servo[lock] = lockup;
wait1Msec(5);
servo[gate] = gatedrive;
}
if (joy2Btn(3)) // lock on
{
servo[lock] = lockdown;
}
if (joy2Btn(1)) // lock up - release rolling goal
{
servo[lock] = lockup;
}
}
}
task main()
{
motor[lift] = 0;
waitForStart();
//irSeeker.acDirection;
initializeRobot();
//int centerGoal;
moveBackward(7);
stopBot();
/*wait10Msec(150);
turnLeft(105);
stopBot();
wait10Msec(100);
moveBackward(2.45);
stopBot();
wait10Msec(100);
readSensor(&irSeeker);
centerGoal = irSeeker.acDirection;
//IRseeker();
displayTextLine(1, "D:%4d", irSeeker.acDirection);
if(centerGoal == 0)
{
stopBot();
}
else if(centerGoal <= 3 && centerGoal > 0)
{
playSound(soundBeepBeep);
moveForward(1);
wait10Msec(50);
turnLeft(100);
wait10Msec(50);
moveBackward(3.3);
wait10Msec(50);
turnRight(80);
wait10Msec(50);
moveBackward(2);
wait10Msec(50);
}
else if(centerGoal >= 3 && centerGoal < 5)
{
playSound(soundDownwardTones);
moveForward(1);
wait10Msec(50);
turnLeft(100);
wait10Msec(50);
moveBackward(2);
wait10Msec(50);
turnRight(70);
wait10Msec(50);
moveBackward(1.5);
wait10Msec(50);
}
else if(centerGoal >= 5 && centerGoal <= 9)
{
playSound(soundFastUpwardTones);
moveForward(.75);
wait10Msec(50);
turnLeft(90);
wait10Msec(50);
moveBackward(1.5);
wait10Msec(50);
turnRight(80);
wait10Msec(50);
moveBackward(2);
wait10Msec(50);
}
else
{
stopBot();
}
wait10Msec(1000);
*/
}
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
// Initialize variables
int gear = 1;
float turnFactor;
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.
getJoystickSettings(joystick);
// Determine how much to slow down one motor to turn
turnFactor = abs(joystick.joy1_x1) / 127;
if (joy1Btn(7) == 1)
{
// Moving forward
if (joystick.joy1_x1 > 5) {
// Turn right
motor[leftMotor] = gear * 20;
motor[rightMotor] = gear * 20 * turnFactor;
} else {
if (joystick.joy1_x1 < -5) {
// Turn left
motor[leftMotor] = gear * 20 * turnFactor;
motor[rightMotor] = gear * 20;
} else {
// Move straight
motor[leftMotor] = gear * 20;
motor[rightMotor] = gear * 20;
}
}
} else {
// Stop
motor[leftMotor] = 0;
motor[rightMotor] = 0;
}
}
}
task main()
{
waitForStart();
moveForward(6.5, 80);
wait10Msec(50);
stopMotors();
wait10Msec(30);
leftTwoWheelTurn(45, 50);
wait10Msec(50);
stopMotors();
wait10Msec(30);
motor[tiltingMotor] = 75;
//robot going to score
wait10Msec(70);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
//robot ready to roll out blocks
motor[conveyorMotor] = 100;
wait10Msec(20);
motor[conveyorMotor] = 0;
wait10Msec(20);
//robot done scoring
motor[tiltingMotor] = -50;
wait10Msec(57);
motor[tiltingMotor] = -25;
wait10Msec(10);
motor[tiltingMotor] = 0;
wait10Msec(4);
//robot's tilting motor comes back to normal state
moveBackward(2, 80);
wait10Msec(50);
//robot moves backward, so it will be able to sense the ir beacon underneath first bucket
stopMotors();
wait10Msec(30);
//keep going forward until ir sensor senses ir beacon
StartTask(irLeftTesting);
wait10Msec(800);
//wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum
while (SensorValue[sonarSensor] < 75)
{
motor[frontLeftMotor] = 100;
motor[frontRightMotor] = 100;
motor[backRightMotor] = 100;
motor[backLeftMotor] = 100;
}
stopMotors();
wait10Msec(30);
moveForward(8, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(60);
moveForward(25, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(58);
moveForward(28.5, 80);
wait10Msec(50);
leftTwoWheelTurn(53, 50);
wait10Msec(120);
moveBackward(43.5, 80);
wait10Msec(50);
//robot is parked in the middle of the ramp
}
task main()
{
// These will be used later and are declared here to save from having to
// declare them every single loop.
int powerL = 0;
int powerR = 0;
int powerPopcorn = 0;
MotorState isMotorStateL = MOTOR_JOYSTICK;
MotorState isMotorStateR = MOTOR_JOYSTICK;
waitForStart();
initializeRobot();
while (true)
{
// Currently does (at least) 7 checks and 3 assignments per loop.
Joystick_UpdateData();
// These should be zeroed after every loop. In the case that there
// isn't input, the motors won't keep moving at the last speed it had.
powerL = 0;
powerR = 0;
powerLift = 0;
powerPopcorn = 0;
// POPCORN!!! (This comes first, obviously.)
if ( Joystick_Button(BUTTON_B, CONTROLLER_2)==true )
{
powerPopcorn = g_FullDrivePower;
}
else if ( Joystick_Button(BUTTON_A, CONTROLLER_2)==true )
{
powerPopcorn = (-1)*g_FullDrivePower;
}
// See if a direction is being pressed, then test for the direction.
// This is inside an `if` statement to optimize speed (less checking).
// `JoystickController` arguments are not passed to increase speed.
// Input from CONTROLLER_2 will be used to control the lift in
// conjunction with CONTROLLER_1, but shouldn't override the driver,
// since driver #1's input is processed last.
// This is the code for CONTROLLER_2:
if ( abs(joystick.joy2_y1)>g_JoystickThreshold )
{
isLiftState = LIFT_JOYSTICK;
//powerLift = Math_ToLogarithmic(joystick.joy2_y1);
powerLift = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y, CONTROLLER_2));
}
if ( ( Joystick_Button(BUTTON_LB, CONTROLLER_2) ||
Joystick_Button(BUTTON_RB, CONTROLLER_2)) ==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift /= g_FineTuneFactor;
}
// This is the code for CONTROLLER_1, along with two unimplemented
// functions for putting rings on and taking rings off.
if ( Joystick_Direction() != DIRECTION_NONE )
{
switch ( Joystick_Direction() )
{
// Operate lift at full power if F/B.
case DIRECTION_F:
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower;
break;
case DIRECTION_B:
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower;
break;
case DIRECTION_L:
sub_PutRingOn();
break;
case DIRECTION_R:
StartTask(sub_TakeRingOff);
break;
}
}
// See if a button (not masked) is being pressed, then react.
// This is inside an `if` statement to optimize speed (less checking).
// The argument to this first `if` statement is a masked version
// of the "bitmap" of buttons directly from the controller.
// Everything other than the buttons used are masked off, to increase
// processing speed (possibly, just speculation). Reasoning:
// `&` compares all bits of the variables, so we might as well mask
// everything we won't need, in case something irrelevant is pressed.
// A `0` value means no buttons (that we are testing for) are pressed.
// Directly using the struct since this is the only possible time to
// use it, and this is very low-level anyways.
//if ( joystick.joy1_Buttons != false )
//if ( (g_ControllerMaskA & joystick.joy1_Buttons) != false )
{
// Buttons Y/B/A will control lift height.
if ( Joystick_Button(BUTTON_Y)==true )
{
isLiftState = LIFT_TOP;
}
if ( Joystick_Button(BUTTON_B)==true )
{
isLiftState = LIFT_MIDDLE;
}
if ( Joystick_Button(BUTTON_A)==true )
{
isLiftState = LIFT_BOTTOM;
}
// If only X is pressed, weigh the ring.
// If JOYR is pressed as well, deploy ramp.
if ( Joystick_Button(BUTTON_X)==true )
{
if ( Joystick_Button(BUTTON_JOYR) == true )
{
Servo_Rotate(servo_ramp, g_rampServoDeployed);
}
else
{
StartTask(sub_WeighRings);
}
}
// Buttons LT/RT will fine-tune the lift.
if ( Joystick_Button(BUTTON_RT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower/g_FineTuneFactor;
}
if ( Joystick_Button(BUTTON_LT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower/g_FineTuneFactor;
}
}
// L/R motor code. Only triggered when a joystick returns a
// value greater than the "drive" threshold (`global vars.h`).
// Logarithmic control probably won't be implemented anytime soon.
// Also need to stop using the `joystick` struct and switch to the
// encapsulated version (Joystick_Joystick(...)).
// Y-axis code:
if ( abs(Joystick_Joystick(JOYSTICK_L, AXIS_Y)) > g_JoystickThreshold ||
abs(Joystick_Joystick(JOYSTICK_R, AXIS_Y)) > g_JoystickThreshold )
{
powerL = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y));
powerR = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_R, AXIS_Y));
}
// Last check: if LB/RB is pressed, fine-tune the power level.
if ( (Joystick_Button(BUTTON_LB)||Joystick_Button(BUTTON_RB)) ==true )
{
powerL /= g_FineTuneFactor;
powerR /= g_FineTuneFactor;
}
// CONTROLLER_2 has the same masking implementation as CONTROLLER_1.
// For a detailed explanation of the mechanism, see those comments.
// CONTROLLER_2 is only tested for button X (currently).
//if ( joystick.joy2_Buttons != false )
//if ( (g_ControllerMaskB & joystick.joy2_Buttons) != false )
{
// If X is pressed, the MOO shall be released!
if ( Joystick_Button(BUTTON_X, CONTROLLER_2)==true )
{
//StartTask(sub_MOO);
PlaySoundFile("moo.rso");
}
if ( Joystick_Button(BUTTON_Y, CONTROLLER_2)==true )
{
//StopTask(sub_MOO);
sub_CowsWithGuns();
}
}
switch (isLiftState)
{
case LIFT_BOTTOM:
sub_LiftToBottom();
break;
case LIFT_MIDDLE:
sub_LiftToMiddle();
break;
case LIFT_TOP:
sub_LiftToTop();
break;
}
// Flush the controller input buffer periodically (every 1/4 sec?)
Motor_SetPower(motor_L, powerL);
Motor_SetPower(motor_R, powerR);
Motor_SetPower(motor_lift, powerLift);
Motor_SetPower(motor_popcorn, powerPopcorn);
}
}
task main()
{
waitForStart();
while(true)
{
getJoystickSettings(joystick);
if(abs(joystick.joy1_y1) < 75 && abs(joystick.joy1_y2) > 75)
{
if(joystick.joy1_y1 < 75)
{
straightNoWait(100);
}
else if(joystick.joy1_y1 > -75)
{
straightNoWait(-100);
}
else
{
straightNoWait(joystick.joy1_y1);
}
}
else if(joystick.joy1_y2 < 75)
{
if(joystick.joy1_y2 > 75)
{
turnNoWait(100, 1);
}
else if(joystick.joy1_y2 > -75)
{
turnNoWait(-100, 1);
}
else if(joystick.joy1_y2 == 0)
{
motor[rf] = 0;
motor[rr] = 0;
}
else
{
turnNoWait(joystick.joy1_y2, 1);
}
}
else if(joystick.joy1_y1 < 75)
{
if(joystick.joy1_y1 > 100)
{
turnNoWait(100, 0);
}
else if(joystick.joy1_y1 < -100)
{
turnNoWait(-100, 0);
}
else if(joystick.joy1_y1 == 0)
{
motor[lf] = 0;
motor[lr] = 0;
}
else
{
turnNoWait(joystick.joy1_y1, 0);
}
}
else
{
motor[lf] = 0;
motor[lr] = 0;
motor[rf] = 0;
motor[rr] = 0;
}
if(joystick.joy2_y1 < 75)
{
if(joystick.joy2_y1 > 100)
{
armNoWait(100);
}
else if(joystick.joy1_y1 < -100)
{
armNoWait(-100);
}
else
{
armNoWait(joystick.joy2_y1);
}
}
else
{
motor[E] = 0;
}
if(joy1Btn(7) == 1)
{
hand();
}
else
{
stopHand();
}
if(joy1Btn(8) == 1)
{
handBack();
}
else
{
stopHand();
}
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
StartTask( Zelda_Theme );
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;
// In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[Block_Chuck] = 0;
servo[IRS_1] = 160;
wait10Msec(400);
ClearTimer(T1);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 )
{
if ((SensorValue[SONAR_1] < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((SensorValue[SONAR_1] > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
}//End of while.
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
servo[Block_Chuck] = 255;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
ClearTimer(T1);
while( SensorValue[SONAR_1] < 60)
{
if ((SensorValue[SONAR_1] < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((SensorValue[SONAR_1] > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
}//End of while
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
Turn(65);
wait10Msec(55);
Move(25,100);
wait10Msec(55);
Turn(75);
wait10Msec(55);
Move(30 ,100);
wait10Msec(55);
servo[Block_Chuck] = 250;
wait10Msec(55);
servo[Spring_Release] = 250;
wait10Msec(55);
servo[Spring_Release] = 0;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
}
task main ()
{
waitForStart(); // wait for start of tele-op phase
StartTask (update_gyro);
nMotorEncoder[leftmotor] = 0;
nMotorEncoder[rightmotor] = 0;
servo[Claw]=110;
start_motor (25, BACKWARD);
wait10Msec(200);
start_motor (25, FORWARD);
wait10Msec(60);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
while (nMotorEncoder[slideL]<18000)
{
motor[slideL]=slide_speed;
motor[slideR]=slide_speed;
}
motor[slideL]=0;
motor[slideR]=0;
//start_motor (40, TURNRIGHT); //not reading gyro while running start_motor (0.4 seconds).
while (happy_angle>-90) //ramp up to 40, skip to 100 and run until 40 degrees. TURNED TOO FAR
{
nxtDisplayTextLine (3,"%f",happy_angle);
motor[leftmotor]=-70;
motor[rightmotor]=70;
}
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
servo[Claw]=50;
motor[leftmotor]=60;
motor[rightmotor]=60;
wait10Msec(80);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
while (happy_angle>-95) //ramp up to 40, skip to 100 and run until 40 degrees.
{
nxtDisplayTextLine (3,"%f",happy_angle);
motor[leftmotor]=-50;
motor[rightmotor]=50;
}
motor[leftmotor]=0;
motor[rightmotor]=0;
motor[leftmotor]=100;
motor[rightmotor]=100;
wait10Msec(260);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(300);
while (nMotorEncoder[slideL]>0)
{
motor[slideL]=-slide_speed;
motor[slideR]=-slide_speed;
}
motor[slideL]=0;
motor[slideR]=0;
}
void InGame::loadGame(std::unique_ptr<StartInfo>& startInfo)
{
//graphics in loading screen
sf::RenderWindow& window = config.window;
tgui::Gui gui;
gui.setWindow(window);
gui.setFont(tgui::Font(config.fontMan.get("Carlito-Bold.ttf")));
tgui::Picture::Ptr background = std::make_shared<tgui::Picture>();
background->setTexture(config.texMan.get("Book.png"));
background->setSize(gui.getSize());
gui.add(background);
tgui::ProgressBar::Ptr progressBar = std::make_shared<tgui::ProgressBar>();
gui.add(progressBar);
progressBar->setPosition(50, 700);
progressBar->setSize(930, 20);
progressBar->setMinimum(0);
progressBar->setMaximum(100);
progressBar->setText("loading...0%");
unsigned int percent = 0;
tgui::Panel::Ptr panel = std::make_shared<tgui::Panel>();
gui.add(panel);
panel->setSize(820, 200);
panel->setPosition(100, 450);
panel->setBackgroundColor(tgui::Color(192, 192, 192, 150));
tgui::Label::Ptr tips = std::make_shared<tgui::Label>();
panel->add(tips);
tips->setPosition(20, 20);
tips->setTextSize(24);
tips->setText("Tips : You can find Burny Sanders in the deep of the desert ruin.");
//*************************************************************************
//the render loop
bool loading_complete = false; //leave the loop when loading completed
Connection* temp_networkPtr = new Connection;
if (!temp_networkPtr)
{
throw "Failed to create network module!";
}
while (window.isOpen() && !loading_complete)
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed)
window.close();
gui.handleEvent(event);
}
config.cursor.update();
//if still loading, update percent
if (percent < 99)
{
if (clock.getElapsedTime() > sf::seconds(0.05))
{
percent++;
std::stringstream ss;
ss << percent;
progressBar->setText(sf::String("loading...") + sf::String(ss.str()) + sf::String("%"));
progressBar->setValue(percent);
clock.restart();
}
}
else //else, client: send "ready" signal to server for every 5s; server: start anyway
{
progressBar->setText(sf::String("waiting for server..."));
loading_complete = waitForStart(startInfo, temp_networkPtr);
}
window.clear();
gui.draw();
window.draw(config.cursor);
window.display();
}
delete temp_networkPtr;
//*************************************************************************
//if it is server, start server system...
if (startInfo->type == StartInfo::TYPE::Server)
{
systemPtr = new Gameplay::ServerSystem(config, startInfo);
}
else //else it is client, start client system
{
systemPtr = new Gameplay::ClientSystem(config, startInfo);
}
//create network and interface which is pointing to the game system
networkPtr = new Gameplay::GameNetwork(systemPtr, startInfo);
systemPtr->setNetworkPtr(networkPtr);
interfacePtr = new Gameplay::GameInterface(systemPtr);
systemPtr->setInterfacePtr(interfacePtr);
if (startInfo->type == StartInfo::TYPE::Server)
{
//send ready signal to every player
//...
}
else //if it is a client
{
//wait for server's signal
}
}
task main() {
waitForStart();
/*int raw = 0;
int nrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "LIGHT");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayClearTextLine(7);
nxtDisplayTextLine(5, "Press [enter]");
nxtDisplayTextLine(6, "to toggle light");
wait1Msec(2000);*/
//while (true) {
// The enter button has been pressed, switch
// to the other mode
/*if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", raw);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
wait1Msec(50);
}*/
int ser=0,fs1=0;
servo[servo1]=ser;
servo[servo2]=215-ser;
bool rightOfLine = true,touch=false;
LSsetActive(LEGOLS);
touch = TSreadState(LEGOTS);
lsVal = LSvalRaw(LEGOLS);
nMotorEncoder[arms] = 0; //Initiate Encoder Pos
wait1Msec(2500);
while(lsVal<360){
lsVal = LSvalRaw(LEGOLS);
forward(15);
}
stop();
//reverse(50);
wait1Msec(500);
stop();
wait1Msec(1000);
while(nMotorEncoder[arms]<30){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
wait1Msec(1000);
while(lsVal<210){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
lsVal = LSvalRaw(LEGOLS);
left(50);
}
stop();
wait1Msec(1000);
fs1 = HTFreadSensor(HTFS1);
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
ClearTimer(T1);
while(time1[T1]<2500){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
fs1 = HTFreadSensor(HTFS1);
lsVal = LSvalRaw(LEGOLS);
touch = TSreadState(LEGOTS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
if(lsVal>210){
while(time1[T1]<2500){
forward(20);
lsVal = LSvalRaw(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
}
rightOfLine=!rightOfLine;
}
else if(lsVal<210){
if(rightOfLine){
rotateLeft(50);
}else{
rotateRight(50);
}
}
}
stop();
ser=100;
servo[servo1]=ser;
servo[servo2]=215-ser;
while(nMotorEncoder[arms]>5){
lowerArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
reverse(50);
wait1Msec(2500);
}
task main(){
//Code: drive(auto_command,time);
//replace auto_command and time with values necessary (if you don't replace them, code won't run)
//auto_command values are: "rpoint", "lpoint", "up", "down", "rswing", "lswing", "rswingback", "lswingback"
//time values is the time, in milliseconds, you want the robot to be doing the action specified in auto_command
//Info:
//Motor at 75% power travels at 2 feet per second = 60.96 cm
//Lpoint turn takes 750 ms to turn 90 degrees
//Use the sonar sensor by: SensorValue[sonarSensor]. It can get a distance from 0cm to 80cm
//drive("up", 10000); move forward 10000 milliseconds (10sec)
//drive("lpoint", 1000); left point turn for 1000 milliseconds (1sec)
//drive("up", 10000); move forward again for 10000 milliseconds (10sec)
waitForStart();
long distance = 0;
/*
int count = 0;
string BatteryLevel = externalBatteryAvg;
string selection = "";
nxtDisplayCenteredBigTextLine (3, BatteryLevel);
wait1Msec(3000);
*/
//0) Move off of home zone
distance = 1650;
drive("up", distance);
//1) Measure offset from walls
//Convert offset distance into milliseconds
// Turn left 90 degrees and measure
distance = 870;
drive("rpoint", distance);
//offset_right = (SensorValue[sonarSensor]/60.96) * 1000;
//Remeasure
//offset_back = ((SensorValue[sonarSensor]/60.96) * 1000) - 1958.3333;
//2) Move forward ((Four Feet Seven Inches=139.7 cm)-offset_back)
distance = 2000;
drive("up", distance);
//3) Point turn 90 degrees
distance = 420;
drive("rpoint", distance);
distance = 2000;
drive("up", distance);
/*
//4) Move forward ((Four Feet Eight Inches=142.24 cm)-(the robot's length)-offset_right)
/////
distance = 2333.333333 - robot_length - offset_right;
drive("up", distance);
//5) Point turn -33.7 degrees
distance = 280.833333;
drive("lpoint", distance);
//6) Move forward ((Seven Feet 2.5 Inches=219.71 cm)-(the robot's length))
distance = 3604.1666666 - robot_length;
drive("up", distance);
//7) Pause 1 second
wait1Msec(1000);
//8) Move backward (Three Inches=7.62 cm)
distance = 125;
drive("down", distance);
//9) Point turn 33.7 degrees
distance = 280.833333;
drive("rpoint", distance);
//10) Measure offset from RED base wall (should be from 5' - 7' = 152.4-213.36 cm)
offset_right = (SensorValue[sonarSensor]/60.96) * 1000;
//11) Move backward until (offset=(Two Feet Six Inches = 76.2 cm))
//OR Move backward (Two Feet Four Inches = 71.12 cm)
//Robot will move into RED Low Zone
//while offset_right > 857 {
// drive("down", 500);
// offset_right = SensorValue[sonarSensor]/60.96;
//}
//OR
distance = 857;
drive("down", distance);
//12) Point Turn 90 degrees
distance = 750;
drive("rpoint", distance);
//13) Measure offset
offset_back = (SensorValue[sonarSensor]/60.96) * 1000;
//14) Move until (offset=(Ten Feet Six Inches = 320.04 cm)-(half the robot's length))
//OR Move forward (Nine Feet Eight Inches = 294.64 cm)
//while offset_back > 5250 {
// drive("up", 500);
// offset_right = SensorValue[sonarSensor]/60.96;
//}
//OR
distance = 4833.33333;
drive("up", distance);
//15) Point Turn 90 Degrees
distance = 750;
drive("rpoint", distance);
//16) Move forward (Five Feet Six Inches = 167.64 cm)
distance = 2750;
drive("up", distance);*/
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//////////////////////////////////BEGIN AUTONOMOUS CODE/////////////////////////////////////////////////////////
int dir;
int strength1, strength2, strength3, strength4, strength5;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode mode = DSP_1200;
// You can switch between the two different DSP modes by pressing the
// orange enter button
while (true)
{
// set the DSP to the new mode
if (HTIRS2setDSPMode(IRSeeker, mode))
break; // Sensor initialized
PlaySound(soundShortBlip);
nxtDisplayCenteredTextLine(6, "Connect Sensor");
nxtDisplayCenteredTextLine(7, "to Port S2");
wait1Msec(100);
}
eraseDisplay();
while (true)
{
// read the current modulated signal direction
dir = HTIRS2readACDir(IRSeeker);
if (dir < 0)
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(IRSeeker, strength1, strength2, strength3, strength4, strength5 ))
break; // I2C read error occurred
string tmpStr;
StringFormat(tmpStr, "Direction: %d", dir);
nxtDisplayTextLine(3, tmpStr);
StringFormat(tmpStr, "Strength: %d", strength3);
nxtDisplayTextLine(4, tmpStr);
if (dir < 5) {
// turn right
motor[driveRight] = 100;
motor[driveLeft] = -100;
} else if (dir > 5) {
// turn left
motor[driveRight] = -100;
motor[driveLeft] = 100;
} else if (strength3 < 110) {
motor[driveRight] = 100;
motor[driveLeft] = 100;
} else {
motor[driveRight] = 0;
motor[driveLeft] = 0;
}
}
while (true)
{}
}
task main()
{
int elevatorHeightTicks;
// Set the following variables to false to hide the standard NXT LCD information
bDisplayDiagnostics = false;
bNxtLCDStatusDisplay = false;
////////// INITIALIZATIONS //////////
initializeRobot(); // Execute robot initialization routine
/////////////// Variables to be used later /////////////////
//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;
// User selected Autonomous information
{
selectStartLocation(); // Get the starting location of the robot
selectAutoActions(); // Get Autononmous actions
switch(AutoActions)
{
case 0: // No Autonomous
break;
case 1: // IR Beacon
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 2: // Left Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 3: // Center Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 4: // Right Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 5: // Play Defense
selectAutoStartDelay(); // select the autonomous start delay
break;
case 6: // Left Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
default: // Not a valid selection
eraseDisplay();
nxtDisplayCenteredTextLine(1,"DITU SAYS");
nxtDisplayCenteredTextLine(2,"INPUT ERROR");
nxtDisplayCenteredTextLine(4,"TRY AGAIN");
wait1Msec(6000);
break;
}
}
eraseDisplay();
// Set the following variables to false to hide the standard NXT LCD information
// Show the default FTC Display Information
bDisplayDiagnostics = true;
bNxtLCDStatusDisplay = true;
//bDisplayDiagnostics = false;
//bNxtLCDStatusDisplay = false;
// Determine the autonomous start delay based on delayAutoStart
switch(delayAutoStart)
{
case 0: // delay start = 0 seconds
delayAutoStartValue = 0;
break;
case 1: // delay start = 1 second
delayAutoStartValue = 1000;
break;
case 2: // delay start = 5 seconds
delayAutoStartValue = 5000;
break;
case 3: // delay start = 10 seconds
delayAutoStartValue = 10000;
break;
case 5: // delay start = 15 seconds
delayAutoStartValue = 15000;
break;
default: // delay start = 0 seconds
delayAutoStartValue = 0;
break;
}
// Process robot starting location selection
switch(robotStartLocation)
{
case 0: // Left Wall
leftWall = true;
break;
case 1: // Corner
corner = true;
break;
case 2: // Right Wall
rightWall = true;
break;
default: // Not a valid starting location
break;
}
// Process robot starting location selection
switch(Row)
{
case 0: // Bottom Row
pegHeightCmd = 1;
break;
case 1: // Middle Row
pegHeightCmd = 2;
break;
case 2: // Top Row
pegHeightCmd = 3;
break;
default: // Not a valid starting location
break;
}
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();
switch(AutoActions)
{
case 0: // No Autonomous
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 1 || Score Ring on the column designated by the IR Beacon===============================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 1: // Score Ring on the column designated by the IR Beacon
wait1Msec(delayAutoStartValue);
// INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE
// move forward, turn 45 degrees, move forward
move_forward(24, 2000, 90, 90);
wait10Msec(50);
//move(1, 0, 1000);
clearTimer(T1);
//HTIRS2setDSPMode(InfraredSensor, DSP_1200);
while (time1[T1] < 500)
{
irSensorVal = SensorValue[IR];
nxtDisplayBigTextLine(2, "IR: %d", irSensorVal);
}
//////////////////////////////////////////////////////////////////
switch(irSensorVal)
{
case 1:
turngyro_left(-90.0, 35);
wait10Msec(100);
move_forward(24, 4000, 90, 90);
wait10Msec(100);
turngyro_left(83.0, 30);
wait10Msec(100);
move_forward(.5, 2000, 90, 90);
wait1Msec(50);
break;
//------------------------------------------------------------------
case 2:
wait10Msec(100);
move_backwards(2, 1000, 90, 90);
turngyro_left(-45.0, 50);
wait10Msec(50);
ClearTimer(T1);
nMotorEncoder[Right2] = 0;
nMotorEncoder[Left2] = 0;
move_forward(56, 4000, 90, 90);
ClearTimer(T1);
move_backwards(12.5, 2000, 90, 90);
wait1Msec(50);
break;
//-----------------------------------------------------
case 3:
move_forward(43, 2000, 90, 90);
wait10Msec(100);
turngyro_left(-90.0, 50);
wait10Msec(100);
move_forward(2, 1000, 90, 90);
wait1Msec(50);
break;
}
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 2 || Score Ring on Left Column==========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 2: // Score Ring on Left Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 3 || Score Ring on Center Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 3: // Score Ring on Center Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
move_forward(60-5, 5000, 90, 90);
/////The alignment from the wall that could work.../////
//move_forward(24, 4000, 80, 80);
//turngyro_left(45, 60, 60);
//move_forward(4, 2000, 80, 80);
RAM_down();
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
/*ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -20;
motor[Left2] = -20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}*/
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 4 || Score Ring on Right Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 4: // Score Ring on Right Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
//asdfjkl;
wait1Msec(500);
move_forward(12, 5000, 90, 90);
turngyro_right(45, 100);
move_forward(20, 7000, 80, 80);
//RAM_down();
motor[Right1] = 0;
motor[Right2] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -20;
motor[Left2] = -20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 5 || Play Defense=======================================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 5: // Play Defense
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
//wait1Msec(500);
move_backwards(84.0, 10000, 100, 100);
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 6 || Score Center & Line Up with the Dispsenser=========================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 6: // Score on the center column & line up with the dispenser
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
move_forward(60-5, 5000, 90, 90);
RAM_down();
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
move_backwards(40-5, 5000, 90, 90);
turngyro_right(135, 100);
move_forward(20-2, 5000, 90, 90);
turngyro_left(90, 100);
move_forward(10-2, 5000, 90, 90);
turngyro_left(90, 100);
move_backwards(10-2, 5000, 90, 90);
break;
default: // Not a valid autonomous action
break;
}
servo[colorSensorServo] = colorServoDefault;
wait10Msec(200);
// All Done, time to stop all tasks
StopAllTasks();
} // End Main Bracket
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[handJoint] = 240;
wait1Msec(100);
black_threshold = LSvalNorm(middle_light);
white_threshold = black_threshold + 5;
writeDebugStreamLine("black_threshold: %d", black_threshold);
writeDebugStreamLine("white_threshold: %d", white_threshold);
//
// Move to other side of field
//
moveStraight(50,500);
//
// Find the line
//
// Continue forward until the middle sensor detects the white line
//
writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));
while (LSvalNorm(middle_light) < white_threshold) {
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
setAllMotorVals(20);
}
allStop();
//
// Turn 90 degrees
//
// Move forward a little more and then turn until the middle sensor detects the white line
//
bool isLeft = true; // If this boolean is true, the 90 degree turn is toward the left
moveStraight(30,200);
//moveStraight(15,1000); // with long arm
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter1 = 0;
while (LSvalNorm(middle_light) < white_threshold) {
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", middle_nrm);
temp_left_nrm = left_nrm;
}
//--!
if (isLeft) {
if (counter1 > 500) {
setDriveMotorVals(60,-60);
}
else {
setDriveMotorVals(40,-40);
}
}
else {
if (counter1 > 100) {
setDriveMotorVals(-20,20);
}
else {
setDriveMotorVals(-15,15); // with long arm
}
}
counter1 += 1;
}
//
// Follow the line until touch sensor is triggered
//
// 1. Left on white, middle on white (or black), right on black: turn toward the left
// 2. Left on black, middle on white (or black), right on white: turn toward the right
// 3. Left on black, middle on white, right on black: move straight
// 4. Left on black, middle on black, right on black: lost...
//
// Set up touch sensor variables
int nButtonsMask;
bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
// If one has, this changes to true.
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter2 = 0;
//deploy the spear
deploySpear(true);
while (isTouch == false) {
writeDebugStreamLine("-----------------counter2 val: %d", counter2);
// Read light sensor values
left_nrm = LSvalNorm(left_light);
middle_nrm = LSvalNorm(middle_light);
right_nrm = LSvalNorm(right_light);
//!--Code for debugging
//if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", left_nrm);
//temp_left_nrm = left_nrm;
//}
//if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
//temp_middle_nrm = middle_nrm;
//}
//if (right_nrm != temp_right_nrm) {
writeDebugStreamLine("middle light sensor value: %d", right_nrm);
//temp_right_nrm = right_nrm;
//}
//--!
if (left_nrm < black_threshold) {
// left sensor is black
if (middle_nrm < black_threshold) {
// middle sensor is black
if (right_nrm > black_threshold) {
// right sensor is white or gray
// turn right
writeDebugStreamLine("Case 1: turned right");
if (counter2 > 600) {
setDriveMotorVals(30,0);
}
else {
setDriveMotorVals(20,0);
}
}
else {
// right sensor is black
// lost, so just turn right a lot
setDriveMotorVals(30,-30);
}
}
else {
// middle sensor is not black
if (middle_nrm > white_threshold) {
// middle sensor is white
// assume right sensor is black
// move straight
writeDebugStreamLine("Case 2: went straight");
if (counter2 > 600) {
setAllMotorVals(30);
}
else {
setAllMotorVals(20);
}
}
else {
// middle sensor is gray
// assume right sensor is gray
// turn right
writeDebugStreamLine("Case 3: turned right");
if (counter2 > 600) {
setDriveMotorVals(30,0);
}
else {
setDriveMotorVals(20,0);
}
}
}
}
else {
// left sensor is not black
if (left_nrm > white_threshold) {
// left sensor is white
// assume middle sensor is black
// assume the right sensor is black too
// turn left
writeDebugStreamLine("Case 4: turned left");
if (counter2 > 600) {
setDriveMotorVals(0,30);
}
else {
setDriveMotorVals(0,20);
}
}
else {
// left sensor is gray
// middle sensor is gray
// assume right sensor is black
// turn left
writeDebugStreamLine("Case 5: turned left");
if (counter2 > 600) {
setDriveMotorVals(0,30);
}
else {
setDriveMotorVals(0,20);
}
}
}
counter2 += 1;
// Read touch sensor values
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
}
allStop();
wait1Msec(1000);
//
// More later....scoring stuff
//
// 1. Move backward a little
// 2. Unfold arm
// 3. Move forward a little
// 4. Move arm down to score
// 5. Back up
// 6. Reset arm
// 7. Move toward our rings?
//
// back up a little
moveStraight(-20,800);
// unfold arm
fold_arm(false);
// move forward a little
moveStraight(20,600);
// move arm down
motor[shoulderJoint] = -50;
wait1Msec(800);
motor[shoulderJoint] = 0;
// back up
moveStraight(-40,500);
// reset arm
fold_arm(true);
//retract spear
//deploySpear(false);
while (true) {
return;
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//save IR state here
//determine what position the beacon is at
int _dir =HTIRS2readACDir(IRseek);
//To do test values for range
if (_dir == 1)
{
position =1;
}
else if(_dir == 2)
{
position =2;
}
else if(_dir == 3)
{
position =3;
}
else if(_dir == 4)
{
position =4;
}
else if(_dir == 5)
{
position =5;
}else if(_dir == 0)
{
position =0;
}
if (position ==1)
{
if ((_dir)!=1)
{
motor[Right]=75;
motor[Left]=75;
}
else{
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(200);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
if (position ==2)
{
if ((_dir)!=1)
{
motor[Right]=75;
motor[Left]=75;
}
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(250);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
if (position ==3)
{
if ((_dir)!=1)
{
motor[Right]=75;
motor[Left]=75;
}
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(300);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
if (position ==4)
{
if ((_dir)!=1)
{
motor[Right]=75;
motor[Left]=75;
}
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(350);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
if (position ==5)
{
if ((_dir)!=1)
{
motor[Right]=75;
motor[Left]=75;
}
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(400);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
if (position ==0)
{
motor[Right]=0;
motor[Left]=0;
wait1Msec(500);
motor[Arm]=75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
servo[Claw]=150;
motor[Right]=-75;
motor[Left]=-75;
wait10Msec(150);
motor[Right]=75;
motor[Left]=-75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(200);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(50);
motor[Right]=-75;
motor[Left]=75;
wait10Msec(100);
motor[Right]=75;
motor[Left]=75;
wait10Msec(150);
}
}
}
task main()
{
short right_y;
short left_y;
short left_dock_y;
debounce = false;
initializeRobot();
waitForStart(); // wait for start of tele-op phase
// StartTask(endGameTimer);
while (true) {
getJoystickSettings(joystick);
if (!debounce) {
if (joy2Btn(Btn1)) {
handle_joy2_event(BUTTON_ONE);
} else if (joy2Btn(Btn2)) {
handle_joy2_event(BUTTON_TWO);
} else if (joy2Btn(Btn3)) {
handle_joy2_event(BUTTON_THREE);
} else if (joy2Btn(Btn4)) {
handle_joy2_event(BUTTON_FOUR);
} else if (joy2Btn(Btn5)) {
handle_joy2_event(LEFT_TRIGGER_UP);
} else if (joy2Btn(Btn6)) {
handle_joy2_event(RIGHT_TRIGGER_UP);
} else if (joy2Btn(Btn7)) {
handle_joy2_event(LEFT_TRIGGER_DOWN);
} else if (joy2Btn(Btn8)) {
handle_joy2_event(RIGHT_TRIGGER_DOWN);
}
}
//if (drive_multiplier) {
//right_y = joystick.joy1_y2;
//left_y = joystick.joy1_y1;
//} else {
left_dock_y = joystick.joy2_y1;
right_y = joystick.joy1_y1;
left_y = joystick.joy1_y2;
//}
if (abs(right_y) > 20) {
motor[driveFrontRight] = drive_multiplier * left_y;
motor[driveRearRight] = drive_multiplier * left_y;
}
else {
motor[driveFrontRight] = 0;
motor[driveRearRight] = 0;
}
if (abs(left_y) > 20) {
motor[driveFrontLeft] = drive_multiplier * right_y;
motor[driveRearLeft] = drive_multiplier * right_y;
}
else
{
motor[driveFrontLeft] = 0;
motor[driveRearLeft] = 0;
}
if (abs(left_dock_y) > 20) {
if (left_dock_y > 0) {
servo[dockarm] = SERVO_DOCK_ARM_FORWARD;
} else {
servo[dockarm] = SERVO_DOCK_ARM_BACKWARD;
}
} else {
servo[dockarm] = SERVO_DOCK_ARM_STOPPED;
}
}
}
task main()
{
waitForStart();
int beacon = 0; //holds which beacon the robot is at.
int i = 0; //holds the IR sensor value.
//loops until the sensor finds the beacon or until it gets to the final beacon.
while(i != 5 && beacon < 4)
{
i=0;
//this section prevents the reading of random IR values and makes sure the value is constant.
//when the value is the constant we want, the beacon is in front of the sensor.
for(int a = 0; a <100; a++)
{
if(a == 0)
{
//gets original ir value
i = SensorValue[IR];
}
else
{
int b = SensorValue[IR];
if(b != i)
{
//if a different value is sensed, then the original value was not constant; thus, we restart the checking
//to try and gete a constant value.
i = 0;
a = 0;
}
}
}
if(i != 5)
{
if(beacon == 0)
{
//the distance to get to the first beacon
ForwardWithModTime(1200,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 1)
{
//the distance to get to the third beacon
ForwardWithModTime(450,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 2)
{
//the distance to get to the third beacon
ForwardWithModTime(900,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
else if(beacon == 3)
{
//the distance to get to the third beacon
ForwardWithModTime(470,35);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(1000);
}
beacon++;
}
}
//code that runs after we stop in front of a beacon.
servo[AutoDispenser] = 255; //dispenses the cube
wait1Msec(900);
servo[AutoDispenser] = 0;
wait1Msec(1000);
//pick where to go based on beacon #.
//if the robot is at rings 1 or 4, it turns, goes back, then turns to face the ramp.
//if the robots is at rings 2 or 3, it goes backwards, turns, goes backwards again, and then turns to face the ramp.
if(beacon == 1) //beacon was on first ring
{
BackwardWithModTime(800,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = -100;
motor[Left] = 100;
wait1Msec(700);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
BackwardWithModTime(1600,50);
motor[Right] = 100;
motor[Left] = -100;
wait1Msec(900);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
ForwardWithModTime(1400,100);
}
else if (beacon == 2) //beacon was on second ring
{
BackwardWithModTime(1200,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = -100;
motor[Left] = 100;
wait1Msec(700);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
BackwardWithModTime(1600,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = 100;
motor[Left] = -100;
wait1Msec(900);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
ForwardWithModTime(1400,100);
}
else if (beacon == 3) //beacon was on third ring
{
ForwardWithModTime(1000,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = 100;
motor[Left] = -100;
wait1Msec(900);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
ForwardWithModTime(1600,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = -100;
motor[Left] = 100;
wait1Msec(900);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
BackwardWithModTime(1400,100);
}
else if (beacon == 4) //beacon was on fourth ring
{
ForwardWithModTime(800,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = 100;
motor[Left] = -100;
wait1Msec(700);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
ForwardWithModTime(1600,50);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
motor[Right] = -100;
motor[Left] = 100;
wait1Msec(800);
motor[Right] = 0;
motor[Left] = 0;
wait1Msec(500);
BackwardWithModTime(1500,100);
}
}
task main()
{
waitForStart();
initializeRobot();
while(true)
{
getJoystickSettings(joystick);
int jLeft = (int)joystick.joy1_y1;
int jRight = -(int)joystick.joy1_y2;
//Driver Controls//////////////////////////////////////////////////////////////////////////
//////Drivetrain///////////////////////////////////////////////////////////////////////
//Speed Control//
if(joy1Btn(5))
{
speed = true;
}
//------------------------------
if(joy1Btn(7))
{
speed = false;
}
//-------------------------------
if (speed)
{
if (abs(jLeft) < 10) ///< core out the noise for near zero settings
motor[LeftMotor] = 0; ///< sets the left motor to 0% power
else
motor[LeftMotor] = jLeft; ///< set motors to joystick settings
if (abs(jRight) < 10) ///< core out the noise for near zero settings
motor[RightMotor] = 0; ///< sets the right motor to 0% power
else
motor[RightMotor] = jRight; ///< sets motors to joystick settings
}
//--------------------------------
else
{
if (abs(jLeft) < 10) ///< core out the noise for near zero settings
motor[LeftMotor] = 0; ///< sets the left motor to 0% power
else
motor[LeftMotor] = (jLeft/3); ///< set motors to joystick settings
if (abs(jRight) < 10) ///< core out the noise for near zero settings
motor[RightMotor] = 0; ///< sets the right motor to 0% power
else
motor[RightMotor] = (jRight/3); ///< sets motors to joystick settings
}
//CURRENTLY NULL//
/*
//////BAM////////////////////////////////////////////////////////////////////////////////
//up
if(joy1Btn(8))
{
servoTarget[bamLeft] = 255;
servoTarget[bamRight] = 0;
}
//down
else if(joy1Btn(6))
{
servoTarget[bamLeft] = 0;
servoTarget[bamRight] = 255;
}
//Idle
else
{
servoTarget[bamLeft] = 127;
servoTarget[bamRight] = 127;
}
*/
//Manipulator Controls/////////////////////////////////////////////////////////////////////
//////lifter///////////////////////////////////////////////////////////////////////////////
int touchVal;
touchVal = SensorValue(touch);
int lifterEncoderVal;
lifterEncoderVal = nMotorEncoder[lifter];
nxtDisplayCenteredBigTextLine(2, "%d", touchVal);
nxtDisplayCenteredBigTextLine(5, "%d", lifterEncoderVal);
//up
if(joy2Btn(7) && (touchVal == 0))
{
motor[lifter] = -50;
}
//down
else if(joy2Btn(5))
{
motor[lifter] = 50;
}
//Idle
else
{
motor[lifter] = 0;
}
//OVERRIDE
//down
if(joy2Btn(4))
{
motor[lifter] = 50;
}
/*
float encoderVal;
nMotorEncoder[lifter] = encoderVal;
nxtDisplayBigTextLine(2, "encoder val: %lf", encoderVal);
*/
/*
//////Mechanical Stop/////////////////////////////////////////////////////////////////////////
//Down////
if(joy2Btn(2))
{
nMotorEncoderTarget[stopLeft] = 90;
nMotorEncoderTarget[stopRight] = 90;
ClearTimer(T1);
while ((nMotorEncoder[stopLeft] < 90) && (time1[T1] < 500))
{
motor[stopLeft] = -30;
motor[stopRight] = -30;
}
}
//Up////
if(joy2Btn(4))
{
nMotorEncoderTarget[stopLeft] = 0;
nMotorEncoderTarget[stopRight] = 0;
ClearTimer(T1);
while ((nMotorEncoder[stopLeft] > 0) && (time1[T1] < 500))
{
motor[stopLeft] = 30;
motor[stopRight] = 30;
}
motor[stopLeft] = 0;
motor[stopRight] = 0;
}
*/
//Ramp/////////////////////////////////////////////////////////////////////////
if(joy2Btn(10))
{
ClearTimer(T1);
while(time1[T1] < 1000)
{
servoTarget[ramp] = 255;
}
}
}
}
task main()
{
//use NXT buttons to select autonomous route and time delay
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
UnlockRightHook();
UnlockLeftHook();
int LeftIRVal = HTIRS2readACDir(LeftIR); //Initialize variable storing left IR sensor value
int timeElapsed = 0; //define variable timeElapsed which will store time values
wait10Msec(500);
time1[T1] = 0; //reset the timer
timeElapsed = time1[T1] //set time elapsed to the timer value
while(LeftIRVal != 5) //move forward until robot is perpendicular to IR beacon
{
motor[LeftDrive] = 100;
motor[RightDrive] = 100;
wait10Msec(1);
LeftIRVal = HTIRS2readACDir(LeftIR); //refresh the current IR value
}
timeElapsed = time1[T1] - timeElapsed;
motor[LeftDrive] = 0; //Brake Motors
motor[RightDrive] = 0;
nxtDisplayCenteredTextLine(3, "%d", nMotorEncoder[RightDrive]);//display current encoder values for drive train motors on screen
////////////////////////////
//deploy autonomous block://
////////////////////////////
//Move(70, 70, 1000, 1000);
if(timeElapsed < 1500) // if time elapsed to find beacon is less than 2000 ms, then probably we mov a little more
{
motor[LeftDrive] = 50;
motor[RightDrive] = 50;
wait1Msec(350);
motor[LeftDrive] = 0;
motor[RightDrive] = 0;
}
else //Else just deploy right there
{
motor[LeftDrive] = 0;
motor[RightDrive] = 0;
}
ServoRotate (AutonomousBlock,30); //Rotating Servo to put block in basket
ServoRotate (AutonomousBlock,31);
ServoRotate (AutonomousBlock,130);//Rotate Servo inside robot
wait10Msec(10);
/////////////////
//go onto ramp://
/////////////////
while(1)
{
if(nMotorEncoder[RightDrive] < 500) // Go forward until a certain total distance is reached
{
motor[RightDrive] = 70;
motor[LeftDrive] = 70;
}
else //once that distance is reached, stop the robot
{
motor[RightDrive] = 0;
motor[LeftDrive] = 0;
}
}
Move(-60, 60, 2150, 2150); //turn robot so that backside faces ramp
Move(60, 60, 3100, 3100); //Move forward toward ramp
Move(-60, 60, 2150, 2150); //Turn perpendicular to ramp
Move(100, 100, 4000, 4000); //Drive on to ramp at full force
motor[RightDrive] = 0; //stop robot and park on ramp
motor[LeftDrive] = 0;
while(1){wait10Msec(10);} //wait until end of autonomous time period
}
//=========================================
// Main Program
//=========================================
task main()
{
disableDiagnosticsDisplay();
alive();
initializeRobot();
StartTask(sensors);
StartTask(selector);
StartTask(display);
motor[lightMotor] = 50;
wait1Msec(500);
motor[lightMotor] = 0;
waitForStart();
constHeading = 0;
relHeading = 0;
if(calibrate != 2)
{
gyroCalTime = 3;
calibrate = 1;
while(calibrate != 2)
{
EndTimeSlice();
}
}
constHeading = 0;
relHeading = 0;
wait1Msec(time_selector*1000);
PlaySound(soundBeepBeep);
switch(MissionNumber)
{
//================================================
// start on the right side of the blue dispencer delivers to IR, then stops
//================================================
case 1:
GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(1000);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,18,true,true,false,false);
if(currDir >= 4 && currDir <= 6) column = 2;
if(currDir < 4) column = 1;
if(currDir > 6) column = 3;
switch(column)
{
case 1:
GyroTimeS_moveV2(300,30,false,false,true,REL);
GyroTimeS_moveV2(8000,15,true,true,true,REL);
motor[RB_motor] = 0;
motor[RF_motor] = 0;
motor[LF_motor] = 0;
motor[LB_motor] = 0;
GyroTime45_V2(340,-30,true,true,REL,true);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(800,-30,true,false,REL);
motor[LB_motor] = 50;
motor[LF_motor] = 50;
wait1Msec(700);
motor[RF_motor] = 50;
wait1Msec(100);
motor[LB_motor] = 0;
motor[LF_motor] = 0;
motor[RF_motor] = 0;
wait1Msec(1000);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(800);
break;
case 2:
GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(3000);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(3000);
servo[shoulder] = 255;
wait1Msec(3000);
servo[wrist] = 130;
wait1Msec(800);
case 3:
GyroTimeS_moveV2(300,-15,false,true,true,false);
GyroTimeS_moveV2(8000,-15,true,true,true,false);
GyroTimeS_moveV2(260,-15,false,true,false,false);
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(3000);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(3000);
servo[shoulder] = 255;
wait1Msec(3000);
servo[wrist] = 130;
wait1Msec(800);
break;
}
break;
//================================================
// start on the right side of the blue dispencer and delivers to the center column, then stops
//================================================
case 2:
GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(1000);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(220,-25,false,true,false,false);//was 15
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(3000);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(3000);
servo[shoulder] = 255;
wait1Msec(3000);
break;
//================================================
// start on the right side of the blue dispencer and delvivers to B and returns to the blue dispencer
//================================================
case 3:
GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(1000);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(1500);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(1000);
servo[shoulder] = 255;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(800);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
//================================================
// start on the right side of the blue dispencer and delvivers to B and returns to the red dispencer
//================================================
case 4:
GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(1000);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
servo[right_servo] = 90;
servo[left_servo] = 90;
wait1Msec(1500);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(1000);
servo[shoulder] = 255;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(800);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
//================================================
// start on the right side of the blue dispencer and delivers to the left column, then stops
//================================================
case 5:
GyroSonar_moveV2(0,SIDE_BACK,125,110,-60,true,true,CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(500);
GyroTime_moveV2(1200,-30,true,false,REL);
motor[motorA] = -50;
wait1Msec(500);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,25,true,true,false,REL);
GyroTimeS_moveV2(300,30,false,false,true,REL);
GyroTimeS_moveV2(8000,15,true,true,true,REL);
motor[RB_motor] = 0;
motor[RF_motor] = 0;
motor[LF_motor] = 0;
motor[LB_motor] = 0;
GyroTime45_V2(340,-30,true,true,REL,true);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(800,-30,true,false,REL);
motor[LB_motor] = 50;
motor[LF_motor] = 50;
wait1Msec(700);
motor[RF_motor] = 50;
wait1Msec(100);
motor[LB_motor] = 0;
motor[LF_motor] = 0;
motor[RF_motor] = 0;
wait1Msec(1000);
servo[shoulder] = 0;
wait1Msec(2000);
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(800);
break;
//================================================
// start on the right side of the blue dispencer and delivers to the right column
//================================================
case 6:
GyroSonar_moveV2(0,SIDE_BACK,125,110,-60,true,true,CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(500);
GyroTime_moveV2(1200,-30,true,false,REL);
motor[motorA] = -50;
wait1Msec(500);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,25,true,true,false,REL);
GyroTimeS_moveV2(8000,-25,true,true,false,REL);
GyroTimeS_moveV2(300,-30,false,false,true,REL);
GyroTimeS_moveV2(8000,-15,true,true,true,REL);
GyroTimeS_moveV2(350,-30,false,false,true,REL);
motor[RB_motor] = 0;
motor[RF_motor] = 0;
motor[LF_motor] = 0;
motor[LB_motor] = 0;
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(800,-30,true,false,REL);
GyroTimeS_moveV2(200,30,false,true,true,REL);
motor[LB_motor] = 45;
motor[LF_motor] = 45;
wait1Msec(800);
motor[LF_motor] = 0;
motor[LB_motor] = 0;
wait1Msec(1500);
servo[shoulder] = 0;
wait1Msec(2000);
servo[wrist] = 130;
wait1Msec(800);
break;
case 7:
GyroTime_moveV2(800,-90,true,false,REL);
GyroTime45_V2(400,90,false, true, REL, false);
break;
case 8:
while(true)
{}
break;
}
}
task main()
{
RegisterDriveMotors(LeftMid, LeftSides, RightMid, RightSides);
InitializeTeleOP();
waitForStart();
StartTask(DriveTank);
while(true)
{
if (joy2Btn(3) == 1)
{
servo[goalGrabber] = 150;
}
else if (joy2Btn(2) == 1)
{
servo[goalGrabber] = 45;
}
//Andymark motor on the right side of the robot has a 512 ticks per rotation value.
//split the motors
//arm raiser
if (joy2Btn(8) == 1)
{
motor[Arm1] = 70;
motor[Arm2] = 70;
}
else if (joy2Btn(7) == 1)
{
motor[Arm1] = 40;
motor[Arm2] = -40;
}
else
{
motor[Arm1] = 0;
motor[Arm2] = 0;
}
if (abs(joystick.joy2_y2) > 15)
{
servo[wrist] = joystick.joy2_y2;
}
//arm extender
/*if (joy2Btn(8) == 1)
{
servo[leftExtender] = 255;
servo[rightExtender] = 0;
}
else if (joy2Btn(7) == 1)
{
servo[leftExtender] = 0;
servo[rightExtender] = 255;
}
else
{
servo[leftExtender] = 128;
servo[rightExtender] = 128;
}*/
//tube code
if (joy2Btn(4) == 1)
{
servo[ballCollecter] = 150;
}
else if (joy2Btn(1) == 1)
{
servo[ballCollecter] = 45;
}
/*while (joy2Btn(7) == 1 && nMotorEncoder[Arm] <= 1110)
{
motor[Arm] = 50;
}
nMotorEncoder[Arm] = 0;
while(joy2Btn(6) == 1 && nMotorEncoder[Arm] >= -1110)
{
motor[Arm] = -50;
}
nMotorEncoder[Arm] = 0;*/
}
}
task main()
{
waitForStart();
initializeRobot();
// The amount of time the robot...
// ...moves forward at an angle.
const int forwardTimeA = 150;
// ...turns to line up perpendicular to the center rack.
const int turnTimeB = 40;
// ...drives up to the peg before lifting the lift up.
const int forwardTimeC = 155;
// ...lifts the claw to put a ring on (row 2).
const int liftTimeF = 79;
// ...moves forward, putting the ring onto the peg.
const int forwardTimeG = 65;
// ...lowers its lift to get rid of the ring.
const int liftTimeH = 55;
// ...backs up and gets ready to go to a dispenser.
const int backwardTimeI = 300;
// ...turns to face parallel to the walls.
const int turnTimeJ = 40;
// ...moves forward to align itself with a dispenser.
const int forwardTimeK = 100;
// ...turns to face the dispenser.
const int turnTimeL = 80;
// ...moves forward to be under the dispenser.
const int forwardTimeM = 50;
Move_Forward (forwardTimeA, g_AccurateMotorPower);
Turn_Right (turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeC, g_AccurateMotorPower);
Lift_Up (liftTimeF, g_AccurateMotorPower);
Move_Forward (forwardTimeG, g_AccurateMotorPower);
// Lift power is negative so that the lift goes DOWN, not UP.
Lift_Down (liftTimeH, g_AccurateMotorPower);
Move_Backward (backwardTimeI, g_AccurateMotorPower);
// Turn power doesn't need to be negative (turns in-place).
Turn_Right (turnTimeJ, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeK, g_AccurateMotorPower);
Turn_Right (turnTimeL, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeM, g_AccurateMotorPower);
while (true)
{
PlaySoundFile("moo.rso");
while(bSoundActive == true)
{
Time_Wait(1);
}
}
}