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; } }