void rushBlue()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 630; //turn to goal + enclose it
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
//driveBackDead(); // push them
//delay (300);
stopDrive();
delay(7000); // wait till auto end
driveBackDead(); // push them
delay(800);
stopDrive();
delay(100);
driveForwardDead(); // driveback towards goal
delay(300);
stopDrive();
intakeDead();
armUp(goalHeight); //arm up
scrapeLeft(encoder4); //encase the goal
driveForwardSlowDead();
delay(700);
stopDrive();
outtake(3000);
stopAll();
}
//rotates(point turn) the robot to reach a certain degree
void rotate(int goal, int cutOff)
{
//more variables for later
float error = 1800;
proportion = 0;
float Kp = 5;
//prepares goal for use with gyro since gyro measures 10 increments for each degree
goal *= 10;
//carrier for the sensor value
int gyroValue;
//buffer in gyro units
int buffer = 175;
//main loop. will continue adjusting until set at right angle
while(!((error<buffer) && (error> -buffer)))
{
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = adjustGyro(gyroValue);
//error = centerGyro(goal, gyroValue);
error = goal - gyroValue;
if(gyroValue < goal)
{
setRightDrive(-127, cutOff);
setLeftDrive(127, cutOff);
}
else if(gyroValue >goal)
{
setRightDrive(127, cutOff);
setLeftDrive(-127, cutOff);
}
else
{
stopDrive();
}
/*
proportion = 127//(int)(abs(error) * Kp);
if(error > 0)
{
//when error is positive robot needs to rotate counter clockwise and if it is negative it is reversed
setRightDrive(-proportion, cutOff);
setLeftDrive(proportion, cutOff);
}
else
{
setRightDrive(proportion, cutOff);
setLeftDrive(-proportion, cutOff);
}
*/
//if the motors arn't moving due to low power input then check the timer else clear it
}
stopDrive();
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart();
disableDiagnosticsDisplay();
//Initialize the gyro and turning
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
//Start actual movement code
moveForwardInches(60,43);//initial forwards
turn(g_PidTurn,45,20);
clearEncoders();
wait1Msec(50);
const int totalTics = 6806;//total tics from before IR to end- CHANGED FOR LESSENED AMOUNT OF FORWARDS
const int ticsToCenter = 3663;//tics from start to central beam
const int ticsToSubtract = 1665;//failsafe, may still need testing
//finding IR
while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //finds the beacon zone 4 (rough)
//nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
startBackward(27);
}
stopDrive();
wait1Msec(300);
while(HTIRS2readACDir(IR) != 5 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //slow down to look for basket (fine)
startForward(15);
}
int currentPosition = abs(nMotorEncoder[leftDrive]);
if (currentPosition > ticsToCenter)//check where we are
moveForwardInchesNoReset(20, 6);//move forwards 5 inches (buckets 1 and 2)
else
moveForwardInchesNoReset(20, 3);//forwards 3 inches (buckets 3 and 4)
stopDrive();//stops robot
servo[dumper] = 30;//dumps the block
motor[lift]= 50;//starts the lift up
wait1Msec(700);
motor[lift]= 0;//stops lift
servo[dumper] = servoRestPosition;//resets servo
int ticsToMove= totalTics - abs(nMotorEncoder[leftDrive]);//tics left after IR
displayCenteredTextLine(0,"TTM: %d", ticsToMove);
moveBackwardTics(90, ticsToMove); //move to end after IR
turn(g_PidTurn, -87,40); //turn to go towards ramp
moveForwardInches(90, 44, false, LEFTENCODER); //forwards to ramp
turn(g_PidTurn, 95, 40); //turn to face ramp
moveForwardInches(90, 45, false, LEFTENCODER);//onto ramp
motor[lift]= -50;//starts the lift down
wait1Msec(500);
motor[lift]= 0;//stops lift
while(true){}
}
void findLineLeft ()
{
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
int black = 2000;
while ( midLine > black ) // black
{
printf("%d\r\n", midLine);
motorSet (motor1, 25); // drive slow until hit
motorSet (motor2, 25);
motorSet (motor9, 25);
motorSet (motor10, 25);
midLine = analogRead(2);
}
motorSet (motor1, -15); // no inertia
motorSet (motor2, -15);
motorSet (motor9, -15);
motorSet (motor10, -15);
delay(50);
stopDrive();
driveForwardSlow(60); // make sure it will be centered
stopDrive();
delay(160);
rightLine = analogRead(3);
midLine = analogRead(2);
leftLine = analogRead(1);
while (rightLine > black) // left is black
{
motorSet (motor1, 7); // turn left until mid line is set
motorSet (motor2, 7);
motorSet (motor9, 55);
motorSet (motor10, 55);
leftLine = analogRead(1);
midLine = analogRead(2);
rightLine = analogRead(3);
}
motorSet (motor1, -15); // no inertia
motorSet (motor2, -15);
motorSet (motor9, -15);
motorSet (motor10, -15);
delay(50);
stopDrive();
stopDrive();
delay(200);
}
//Activates motors required to vertically move
//@param encoderCounts change in counts
void driveEncoderVertical(int encoderCounts){
int direction = (encoderCounts > 0) ? 1 : -1;
int initialEncoder = getMotorEncoder(backRight);
while (abs(getMotorEncoder(backRight) - initialEncoder) < abs(encoderCounts)){
verticalMotors(direction);
}
stopDrive();
}
void worldsBlue ()
{
// variables
int armMin = 300;
int wallHeight = 100;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
int encoder1 = 1500; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 900; // backwards to the opponets goal
int encoder4 = 400; //turn to goal + enclose it
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
driveBackDead(); // push them
delay (200);
stopDrive ();
delay (7000); // wait till auto end
driveBackDead ();// push them
delay (600);
stopDrive();
delay(100);
driveForwardDead(); // driveback towards goal
delay(400);
stopDrive ();
armUp(goalHeight); //arm up
scrapeLeft (encoder4); //encase the goal
outtake (300);
stopAll ();
}
//Activates the motors required to strafe robot
//@param direction: 1 is right, -1 is left
void sideMotors(int encoderCounts){
int direction = (encoderCounts > 0) ? 1 : -1;
int initialEncoder = getMotorEncoder(backRight);
while (abs(getMotorEncoder(backRight) - initialEncoder) < abs(encoderCounts)){
motor[frontLeft] = -1 * direction * 127;
motor[frontRight] = direction * 127;
motor[backLeft] = direction * 127;
motor[backRight] = -1 * direction * 127;
}
stopDrive();
}
task main()
{
StartTask(intakeStart);
while(SensorValue[bumperLeft]==0)
{
}
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
wait10Msec(50);
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(200);
intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,600);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveFirstTierUp(127,1800);
moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 500);
if (time1[T4] < 10000)
{
moveSharpRight(127,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
}
StopTask(intakeStart);
}
void ram()
{
intakeDead(); // unfold
driveForwardDead();
delay(5000);
stopDrive();
stopIntake();
stopAll();
}
//Activates the motors required to rotate robot
//@param direction: 1 is clockwise, -1 is counterclockwise
void turnMotors(int direction, int encoderCounts){
//int initialEncoder = getMotorEncoder(backRight);
//while (abs(getMotorEncoder(backRight) - initialEncoder) < abs(encoderCounts)){
if (direction == 1){
motor[frontRight] = 63;
motor[backRight] = 63;
} else{
motor[frontLeft] = -63;
motor[backLeft] = -63;
}
//}
wait1Msec(encoderCounts);
stopDrive();
}
task main()
{
initializeRobot();
waitForStart();
turnLeft(_45DEGREES - 35);
delayStop(10);
moveBackward(32);
delayStop(10);
turnLeft(_90DEGREES);
delayStop(10);
timedMove(5, 100, -1); // 5 seconds, 100 power, backwards
stopDrive();
delayStop(0);
timedMove(3, 100, 1); // 3 seconds, 100 power, forward
stopDrive();
delayStop(0);
timedMove(4, 100, -1); // 4 seconds, 100 power, backwards
stopDrive();
delayStop(0);
gotoIR_FORWARD();
storeEncoderValues();
resetEnc();
delayStop(300);
turnRight(_90DEGREES - 35);
resetEnc();
delayStop(0);
}
void reverse15Red()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int pot = analogRead (8);
int encoder1 = 1200; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 1300; // backwards to the opponets goal
int encoder4 = 900; //turn to bridge
int encoder5 = 200; // knock their wallball
int encoder6 = 200; // turn to other ball
int encoder7 = 400; // knock their center ball
int encoder8 = 400; // positioning
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnRight (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
driveBackDead(); // 90 deg align
delay(400);
stopDrive ();
armUp(wallHeight);// arm up
scrapeLeft (encoder4); // turn to wall ball
driveForward (encoder5); // knock off wall ball
scrapeLeftBack(encoder6); // turn to middle ball
driveForward(encoder7); // knock off middle ball
driveBack (encoder8); // positioning
stopAll ();
}
void rejectionBlue()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 100; //turn to goal + enclose it
int encoder5 = 450; //hit 1st blue ball off
int encoder6 = 350; // drive back
int encoder7 = 175; // turn to 2nd blue ball
int encoder8 = 550; // hit 2nd ball off
int encoder9 = 250; // drive back to position
int encoder10 = 370; // rotate to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
armUp(wallHeight); // arm up
turnRight(encoder4); // turn towards center blue ball
driveForward(encoder5); // hit 1st ball off
delay(700);
driveBack(encoder6); // back
delay(700);
turnRight(encoder7); // turn towards 2nd blue ball
delay(700);
driveForward(encoder8); // hit off 2nd blue ball
delay(700);
intakeDead();
driveBack(encoder9); // position
delay(700);
turnLeft(encoder10); // encase opponet goal
delay(700);
armUp(goalHeight);
findLineRight();
//followLine(300);
driveForwardDead(); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll();
delay(60000);
}
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()
{
initializeRobotAUTO();
resetEnc();
waitForStart();
determineFirst();
while(firstTest == true){
rotateServo();
}
while(secondTest == false){
stopDrive();
}
while(secondTest == true){
gotoIR_FORWARD();
storeEncoderValues();
resetEnc();
delayStop(500);
//switch case
moveForwardif(1.75);
delayStop(200);
turnLeft(_90DEGREES);
delayStop(500);
moveBackward(7);
resetEnc();
scoreAuto();
moveForward(3.5);
resetEnc();
delayStop(300);
turnRight(_90DEGREES);
resetEnc();
stopDrive();
delayStop(300);
gotoEnd_for();
delayStop(300);
turnLeft(_135DEGREES + 215);
delayStop(300);
moveBackward(28.9);
delayStop(300);
turnRight(_90DEGREES + 135);
delayStop(200);
moveBackward(39.5);
delayStop(200);
moveForward(3.5);
delayStop(350);
moveBackward(3.5);
delayStop(350);
retractServo();
stopDrive();
secondTest = false;
}
for(;;){}//loop forever
}
void classic15Red()
{
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
//encoder values
int encoder1 = 1125; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 275; //back up
int encoder5 = 800; //back up across the barrier again
int encoder6 = 90; // turn towards center large ball
int encoder7 = 220; // hit it off the barrier
int encoder8 = 350; // drive back
int encoder9 = 110; // turn towards other large ball
int encoder10 = 681; // hit it off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 150; // ass to wall
int encoder13 = 800;
int encoder14 = 400;
int encoder15 = 700;
int encoder16 = 400;
int encoder17 = 100;
int encoder18 = 400;
int encoder19 = 700;
int encoder20 = 400;
int encoder21 = 100;
int encoder22 = 400;
int encoder23 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
driveForward(encoder1); // drive to goal
armUp (goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead (dead1); //drive slow, adjust to 90 degrees
outtake (1500);
driveBack(encoder4); // back up
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
turnRight(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
driveBack(encoder8); // drive back
turnLeft(encoder9); // turn towards other large ball
driveForward(encoder10); // hit it off the barrier
driveBackSlowDead (dead2); // drive back to square
turnLeftArc (encoder12); // ass to wall
driveBackSlowDead (dead2) ; // wall align 90degrees
armDown (armMin); // arm down to floor
intakeDead (); // start intaking
driveForwardSlow (encoder13) ;// drive towards barf and intake whatever
turnRightArc (encoder14); // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder15); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //hump da bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder16); //face the barf
driveForwardSlow (encoder17) ;// drive towards barf and intake whatever
turnRightArc (encoder18) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder19); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //drive to and align on bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder20); //face the barf
driveForwardSlow (encoder21) ;// drive towards barf and intake whatever
turnRightArc (encoder22) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder23); // drive to barrier
outtake (1500); //drop the barf
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
task main()
{
initializeRobot();
int countline = 0;
waitForStart(); // Wait for the beginning of autonomous phase.
motor[slide] = 20;
wait1Msec(750);
motor[slide] = 0;
ClearTimer(T1);
SensorValue[irSeek] = 0;
motor[leftDrive] = 70;
motor[rightDrive] = 70;
servo[leftgrab] = grabDownPosition;
while(time1[T1] < 9000)//stays in loop while less than 9 seconds
{
if(atLine())
{
countline++; // if at line check and see if we are at the IR beacon if break out of loop and go to next loop
if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6)
{
// at Correct Line
PlayTone(100,100);
break;
}
else if(countline==1)
{
motor[rightDrive]= 5;//if it isnt at the beacon and its the first line turn to the right slightly
motor[leftDrive] = 0;
wait1Msec(500);
}
else if(countline == 2)/*may need to change*/{
motor[rightDrive] = 4;//if it isn't at the beacon and is at line 2
motor[leftDrive] = 0;
wait1Msec(500);
}
motor[leftDrive] = 75;// drive with 75% power
motor[rightDrive] = 75;
wait1Msec(500);
}
}
if(time1[T1] < 9000)//while less than 9 seconds and boolean atgood is true
{
nMotorEncoder[leftDrive] = 0;
nMotorEncoder[rightDrive] = 0;
if(countline == 1)
{//if it is at the beacon and it is at the first line
forward(6); //Distance after line till stop
stopDrive();
wait1Msec(500);
servo[leftgrab] = grabReleasePositionpeg1;//put grabber at position
wait1Msec(500);
motor[slide] = -50;
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[leftgrab] = grabReleasePositionpeg1 - 15;// move slightly to possible help
wait1Msec(1000);
motor[leftDrive] = -30;
motor[rightDrive] = -30;
wait1Msec(1000);
motor[leftDrive] = 0;
motor[rightDrive] = 0;
}
else if(countline == 2)//
{//if at line 2 and at the beacon
forward(7);//Distance after line till stop
stopDrive();
wait1Msec(500);
servo[leftgrab] = grabReleasePositionpeg2;//move to position for the 2nd peg
wait1Msec(500);
motor[slide] = -50;//slide over to put the ring on
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[leftgrab] = grabReleasePositionpeg2 + 10;//move slightly to help put the ring on
wait1Msec(1000);
motor[leftDrive] = -50;
motor[rightDrive] = -50;
wait1Msec(2000);
motor[leftDrive] = 0;
motor[rightDrive] = 0;
}
else if(countline ==3)
{//if at line 3 and at beacon
forward(6.2);//Distance after line till stop
stopDrive();
wait1Msec(500);
servo[leftgrab] = grabReleasePositionpeg3;//put grabber to position for 3rd peg
wait1Msec(500);
motor[slide] = -50;//move to slide the ring on
wait1Msec(1000);
motor[slide] = 0;
wait1Msec(1000);
servo[leftgrab] = grabReleasePositionpeg3 + 25;
//move slightly to help put ring on
wait1Msec(1000);
motor[leftDrive] = -50;
motor[rightDrive] = -50;
wait1Msec(6400);
forward(7);
}
}
else
stopDrive();//if not below 9 seconds stop
nMotorEncoder[leftDrive] = 0;
nMotorEncoder[rightDrive] = 0;//clear encoders
while (true) {}
}
void driveForTicks(int encoderSide, int encoderTicks, int speed)
{
switch(encoderSide)
{
case Encoder_Left:
default:
{
int leftDest = SensorValue[encoder_Left] + encoderTicks;
if(leftDest < SensorValue[encoder_Left])
{
while(SensorValue[encoder_Left] < leftDest)
{
motor[driveLeftBack] = speed;
motor[driveRightBack] = speed;
motor[driveLeftFront] = speed;
motor[driveRightFront] = speed;
}
}
else
{
while(SensorValue[encoder_Left] > leftDest)
{
motor[driveLeftBack] = -speed;
motor[driveRightBack] = -speed;
motor[driveLeftFront] = -speed;
motor[driveRightFront] = -speed;
}
}
}
break;
case Encoder_Right:
{
int rightDest = SensorValue[encoder_Right] + encoderTicks;
if(rightDest < SensorValue[encoder_Right])
{
while(SensorValue[encoder_Right] < rightDest)
{
motor[driveLeftBack] = speed;
motor[driveRightBack] = speed;
motor[driveLeftFront] = speed;
motor[driveRightFront] = speed;
}
}
else
{
}
}
break;
case Encoder_Both:
{
int leftDest = SensorValue[encoder_Left] + encoderTicks;
int rightDest = SensorValue[encoder_Right] + encoderTicks;
while((SensorValue[encoder_Left] != leftDest) || (SensorValue[encoder_Right] != rightDest))
{
if(SensorValue[encoder_Left] < leftDest)
{
motor[driveLeftBack] = speed;
motor[driveLeftFront] = speed;
}
else if(SensorValue[encoder_Left] > leftDest)
{
motor[driveLeftBack] = -speed;
motor[driveLeftFront] = -speed;
}
else
{
motor[driveLeftBack] = 0;
motor[driveLeftFront] = 0;
}
if(SensorValue[encoder_Right] < rightDest)
{
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
else if(SensorValue[encoder_Right] > rightDest)
{
motor[driveRightBack] = -speed;
motor[driveRightFront] = -speed;
}
else
{
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
}
}
break;
}
stopDrive();
}
void driveForTicks(int leftTicks, int rightTicks, int speed, bool scaleSpeed)
{
int leftDest = SensorValue[encoder_Left] + leftTicks;
int rightDest = SensorValue[encoder_Right] + rightTicks;
while((abs(SensorValue[encoder_Left] - leftDest) > 100) || (abs(SensorValue[encoder_Right] - rightDest) > 100))
{
if(SensorValue[encoder_Left] < leftDest)
{
if(abs(SensorValue[encoder_Left]) - leftDest < 200)
{
motor[driveLeftBack] = speed / 2;
motor[driveLeftFront] = speed / 2;
}
else
{
motor[driveLeftBack] = speed;
motor[driveLeftFront] = speed;
}
}
else if(SensorValue[encoder_Left] > leftDest)
{
if(abs(SensorValue[encoder_Left]) - leftDest < 200)
{
motor[driveLeftBack] = -speed / 2;
motor[driveLeftFront] = -speed / 2;
}
else
{
motor[driveLeftBack] = -speed;
motor[driveLeftFront] = -speed;
}
}
else
{
motor[driveLeftBack] = 0;
motor[driveLeftFront] = 0;
}
if(SensorValue[encoder_Right] < rightDest)
{
if(abs(SensorValue[encoder_Right]) - rightDest < 200)
{
motor[driveRightBack] = speed / 2;
motor[driveRightFront] = speed / 2;
}
else
{
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
}
else if(SensorValue[encoder_Right] > rightDest)
{
if(abs(SensorValue[encoder_Right]) - rightDest < 200)
{
motor[driveRightBack] = -speed / 2;
motor[driveRightFront] = -speed / 2;
}
else
{
motor[driveRightBack] = -speed;
motor[driveRightFront] = -speed;
}
}
else
{
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
}
stopDrive();
}
task autonomous()
{
// .....................................................................................
// Insert user code here.
// .....................................................................................
if(programs ==0)
{
StartTask(intakeStart);
/*while(SensorValue[bumperLeft]==0)
{
}
*/
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
motor[secondTier]=-127;
wait10Msec(50);
motor[secondTier]=0;
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(150);
//intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,-10,650);
moveStraightDistance(127,100);
turnRight(127,100);
stopPid(0.6,0.3);
wait10Msec(50);
//moveFirstTierUp(127,1800);
//moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 1000);
moveSharpRight(127,0,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
StopTask(intakeStart);
}
else if(programs ==1)
{
}
else if(programs ==2)
{
}
}
void kakitRed()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1650;
int pot = analogRead(8);
int encoder00 = 250; // back up abit to row
int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
int encoder1 = 162; // rotate towards 2 buckys
int encoder2 = 150; // drive towards buckys
int encoder3 = 400; // back up to bump
int encoder4 = 200; // back up towards bridge
int encoder5 = 360; // rotate 180 degrees to the large ball
int encoder6 = 900; // go under bridge and knock out large ball
int encoder7 = 90; // turn to goal
int encoder8 = 200; // drive to goal
int encoder9 = 75;
// begin routine
intake(300);
armDownTrim();
driveForwardDead(); //ram big balls
delay(1500);
stopDrive();
delay(500);
driveBack(encoder00); // back up to row abit
intakeDead();
turnLeft(encoder0); // turn 360 degrees
driveBackDead(); // drive back + wall align
delay(1300);
stopDrive();
delay(500);
turnRight(encoder1); // turn to two buckys
intakeDead();
followLine(300); // make sure ur straight
driveForwardSlowDead(); // drive towards buckys
delay(500);
stopDrive();
delay(600);
driveForwardDead(); //get the 2nd ball
delay(200);
stopDrive();
delay(600);
stopIntake();
driveBack(encoder3); // back up to bump
armUpDead(); // armup
delay(50);
stopArm();
stopIntake();
driveBackSlowDead(); // allign the bump
delay(300);
stopDrive();
delay(500);
driveBackDead(); // over the bump
delay(1000);
stopDrive();
delay(500);
driveForwardSlowDead(); // alighn to bump
delay(800);
stopDrive();
delay(500);
driveBackSlow(encoder4); // back up towards bridge
turnLeft(encoder5); // rotate 180 degrees to the large ball
armDown(armMin);
armDownTrim();
driveForward(encoder6); // go under the bridge + knock large ball
armUp(goalHeight);
turnRight(encoder7); // turn to goal
findLineRight();
followLine(300);
driveForwardSlowDead(); // drive to goal
delay(700);
stopDrive();
outtake(7000); // outtake 3
stopAll();
}
void followLine(int dylanSexxydistance) {
int lightDegree = 2000;
int aliSexxySpeed = 40;
int aliSpeed2 = 35;
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
int counts = 0;
imeReset(0); // reset right I.M.E
imeGet(0, &counts);
printf("%d\r\n", leftLine);
printf("%d\r\n", rightLine);
printf("%d\r\n", midLine);
while (abs(counts) < dylanSexxydistance) {
double leftLine = analogRead(1);
double midLine = analogRead(2);
double rightLine = analogRead(3);
//printf("%d\r\n ", leftLine);
//printf("%d\r\n ", rightLine);
//printf("%d\r\n \n ", midLine);
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//printf("\r\n ");
//if(leftLine>1300 && rightLine>1300 && midLine <1300)
//{
//}
imeGet(0, &counts); // keep getting the value
if (leftLine < 2000 || rightLine < 2000 || midLine < 2000) //(midLine>100) // Mid is black
{
printf("%f \t",
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10)); //right
printf("%f \n",
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10)); //left
motorSet(MOTOR1,
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10));
motorSet(MOTOR2,
aliSexxySpeed * ((((rightLine) / (leftLine)) + 5) / 10));
motorSet(MOTOR9,
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10));
motorSet(MOTOR10,
aliSexxySpeed * ((((leftLine) / (rightLine)) + 5) / 10));
}
/*else
{
printf("%f\t", aliSexxySpeed*((((rightLine)/(leftLine*1.5))+2)/5));//right
printf("%f\t", aliSexxySpeed*((((leftLine)/(rightLine*1.5))+2)/5));//left
motorSet (motor1, aliSexxySpeed*((((rightLine)/(leftLine*2))+2)/5));
motorSet (motor2, aliSexxySpeed*((((rightLine)/(leftLine*2))+2)/5));
motorSet (motor9, aliSexxySpeed*((((leftLine)/(rightLine*2))+2)/5));
motorSet (motor10, aliSexxySpeed*((((leftLine)/(rightLine*2))+2)/5));
}*/
/*
if (leftLine < 2000) // leftLine white
{
motorSet (motor1, 40);
motorSet (motor2, 40);
motorSet (motor9, 0);
motorSet (motor10, 0);
}
if (rightLine < 2000) // rightLine white
{
motorSet (motor1, 0);
motorSet (motor2, 0);
motorSet (motor9, 40);
motorSet (motor10, 40);
}
if (midLine < 2000)
{
motorSet (motor1, 40);
motorSet (motor2, 40);
motorSet (motor9, 40);
motorSet (motor10, 40);
}
*/
}
stopDrive();
delay(200);
}
//go forward for distance(in degrees) in a straight line at heading goal
//distance is always a positive integer = the distance in inches
//goal should be 0 if comming out of a turn or it should be the current gyro heading
//base power is positive or negative depending on desired direction
void forward(int distance, int goal, int basePower, int cutOff)
{
nMotorEncoder[BL] = 0;
nMotorEncoder[BR] = 0;
int direction;
//sets direction for use later
if(distance <= 0)
{
direction = -1;
}
else
{
direction = 1;
}
basePower*=direction;
//adjusts distance
distance = (int)(51 * distance -(277*direction));
//error variables for use later
float error;
float oldError = 0;
int change = 0;
//resets variables for use later
proportion = 0;
integral = 0;
derivative = 0;
//set constants for PID
float Kp = 0.5;
float Ki = 0.075;
float Kd = 6;
//prepares goal for use with gyro
goal *= 10;
//holds gyro value for future use
int gyroValue;
bool endLoop = false;
int encoderLeftVal = nMotorEncoder[BL];
int encoderRightVal = nMotorencoder[BR];
//will loop while the average of the sides is less than the direction
while(!endLoop)
{
encoderLeftVal = nMotorEncoder[BL];
encoderRightVal = nMotorencoder[BR];
if(direction > 0)
{
if(!(((nMotorEncoder[BL] + (nMotorEncoder[BR]*-1))/2) < distance))
{
endLoop = true;
}
}
else
{
if(!((nMotorEncoder[BL] + (nMotorEncoder[BR]*-1))/2 > distance))
{
endLoop = true;
}
}
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = adjustGyro(gyroValue);
error = centerGyro(goal, gyroValue);
change = PID(Kp, Ki, Kd, error, oldError);
//subtracts(or adds a negative since change will be negaive in that case/..) to the side that is ahead
//multiplies by direction so that change is reversed
//when both signs are the same left is affected, when signs are different right is effected
if((change * direction) > 0)//if change and direction are both positive or negative
{
setRightDrive(basePower, cutOff);
setLeftDrive((basePower - change), cutOff);
}
else if((change * direction) < 0)//if either change or direction but not both are negative
{
setRightDrive((basePower + change), cutOff);
setLeftDrive(basePower, cutOff);
}
else
{
setRightDrive(basePower, cutOff);
setLeftDrive(basePower, cutOff);
}
//sets the current error to be the old error for the next iteration
oldError = error;
//waits so that calculations are at even intervals
wait1Msec(50);
}
stopDrive();
}
//go sideways for distance(in degrees) in a straight line at heading goal(front of robot)
//distance is always a positive integer
//goal should be 0 coming out of a turn so it finishes adjustments it couldn't do because of the motor cutoff
//alternately the goal could be the current gyro heading
//base power is positive or negative depending on desired direction-negative = left; positive = right
void sideways(int distance, int goal, int basePower, int cutOff)
{
//adjusts distance
distance = abs(distance);
distance = (int)(distance*(360/sideConversion));
int direction;
//sets direction for use later
if(basePower <= 0)
{
direction = -1;
}
else
{
direction = 1;
}
//error variables for use later
float error;
float oldError = 0;
float change = 0;
//resets variables for use later
proportion = 0;
integral = 0;
derivative = 0;
//set constants for PID
float Kp = 0.5;
float Ki = 0.054;
float Kd = 1.5;
//prepares goal for use with gyro
goal *= 10;
//stores gyro value
int gyroValue;
//will loop while the average of the sides is less than the direction
while((direction * NMotorEncoder[BL] + direction * (-1*NMotorEncoder[BR]))/2 < distance)
{
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = centerGyro(goal, gyroValue);
error = adjustGyro(gyroValue);
change = PID(Kp, Ki, Kd, error, oldError);
change = change * -1;
weightMechanum(basePower, 0, change);
setFR(FRPower, cutOff);
setFL(FLPower, cutOff);
setBL(BLPower, cutOff);
setBR(BRPower, cutOff);
//sets the current error to be the old error for the next iteration
oldError = error;
//waits so that calculations are at even intervals
wait1Msec(50);
}
stopDrive();
}
void rejectionBlueSkills()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 270; //turn to center of large balls
int encoder5 = 200; //hit 1st blue ball off
int encoder55 = 300; // back a bit
int encoder6 = 450; //hit 2nd ball off
int encoder7 = 450; // drive back past line
int encoder8 = 160; // turn to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
armUp(wallHeight); // arm up
turnRight(encoder4); // turn towards center of large balls
driveForwardSlowDead(); // go between large balls
delay(1200);
stopDrive();
delay(300);
turnLeft(encoder5); //hit center ball
driveBackSlow(encoder55);
turnRight(encoder6); // hit far ball
driveBackSlow(encoder7); // back up past line
turnRight(encoder8); // tun to line
armUp(goalHeight);
findLineLeft();
//followLine(300);
driveForwardDead(); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll();
delay(60000);
}
task autonomous()
{
switch(selectedAuton)
{
case Autonomous_LeftScoreMatchLoads:
{
clearEncoders();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(700);
stopIntake();
positionArm(Arm_ScoreHeight);
stopArm();
wait1Msec(1000);
int leftDest = SensorValue[encoder_Left] + 2200;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(60);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait10Msec(200);
}
break;
case Autonomous_RightPickupScoreYellow:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 40;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 40;
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(1500);
stopIntake();
}
break;
case Autonomous_ProgrammingSkills:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(40);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(500);
stopIntake();
leftDest = SensorValue[encoder_Left] - 390;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-30);
}
stopDrive();
positionArm(990);
stopArm();
rightDest = SensorValue[encoder_Right] + 660;
while(SensorValue[encoder_Right] < rightDest)
{
motor[driveLeftBack] = -30;
motor[driveLeftFront] = -30;
}
stopDrive();
leftDest = SensorValue[encoder_Left] - 2050;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-50);
}
stopDrive();
}
break;
case Autonomous_None:
default:
{
}
break;
}
}
