void armDown(int target, int speed) { while(SensorValue[rightLiftEncoder] > target) { liftMotors (-speed); } liftMotors (stillDown); }
void armUp(int target, int speed) { while(SensorValue[rightLiftEncoder] < target) { liftMotors (speed); } liftMotors (stillUp); }
//User Functions void lift(){ if (vexRT[Btn6U] == 1){ liftMotors(127); } else if (vexRT[Btn6D] == 1){ liftMotors(-127); } else liftMotors(0); }
/* Raises or lowers the lift by a certain change in height, measured with the shaft encoder */ void elevateLift(int deltaHeight){ int initialHeight = SensorValue[shaftEncoder]; if (deltaHeight > 0){ while (SensorValue[shaftEncoder] - initialHeight < deltaHeight){ liftMotors(1); } } else { while (SensorValue[shaftEncoder] - initialHeight > deltaHeight){ liftMotors(-1); } } stopLift(); }
//Returns the lift back until shaft encoder count is 0 //Requires "ground" position to be 0 for this to work! void returnLiftToPreload(){ while (abs(SensorValue[shaftEncoder]) > 45 && !killSwitch){ liftMotors(-1); killSwitch = (vexRT[Btn8U] || vexRT[Btn8UXmtr2]) ? true : killSwitch; } stopLift(); }
//Raises or lowers the lift by a certain change in height, //measured with the shaft encoder void elevateLift(int height){ height *= -1; int target = SensorValue[shaftEncoder] + height; if (height < 0){ while (SensorValue[shaftEncoder] > target){ liftMotors(1); } stopLift(); } else { while (SensorValue[shaftEncoder] < target){ liftMotors(-1); } stopLift(); } stopLift(); }
task usercontrol() { int LiftDownSelector=0; int stillspeed = 15; int stillspeedLeft = 15; while (1 == 1) { const int downSpeed = 15; const int feedstill = 2; rightDrive ( vexRT[Ch2]); leftDrive ( vexRT[Ch3]); if(vexRT[Btn7U] == 1) { SensorValue[latch] = 1; } if(vexRT[Btn7R] == 1) { SensorValue[latch] = 0; } if(vexRT[Btn7D] == 1) { SensorValue[conveyer] = 1; } if(vexRT[Btn7L] == 1) { SensorValue[conveyer] = 0; } if(vexRT[Btn8L] == 1) { stillspeed = -10; stillspeedLeft = 10; } if(vexRT[Btn8U] == 1) { stillspeed = 15; stillspeedLeft = 15; } if(vexRT[Btn8D] == 1) { SensorValue[Claw] = 1; } if(vexRT[Btn8R] == 1) { SensorValue[Claw] = 0; } if(vexRT[Btn5U] == 1) { liftMotors(127); } else if(vexRT[Btn5D] == 1) { liftMotors(-127); } else { if(SensorValue[potLeft] > 1700) { motor[leftArmTop] = stillspeedLeft; motor[leftArmBottom] = stillspeedLeft; motor[rightArmTop] = stillspeed; motor[rightArmBottom] = stillspeed; } else { liftMotors(-downSpeed); } } if(vexRT[Btn6U] == 1) { motor[rightFeed] = 127; motor[leftFeed] = 127; } else if(vexRT[Btn6D] == 1) { motor[rightFeed] = -127; motor[leftFeed] = -127; } else { motor[rightFeed] = feedstill; motor[leftFeed] = feedstill; } if (vexRT[Btn8L]==1) { LiftDownSelector=1; } // wait1Msec(25); //don't hog the cpu } }
task autonomous()///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// { int programmingSkills=0; int match=0; int red=0; int blue=0; int cube=0; int skyrise=0; SensorValue[leftLifttEncoder] = 0; SensorValue[rightLiftEncoder] = 0; if(SensorValue[autoPot2] > 1000) // competition or match { programmingSkills=1; } else match=1; if(SensorValue[autoPot] < 2500) //red or blue { red=1; } else blue=1; if(SensorValue[autoPot3] > 2000) // skyrise or cubes { skyrise=1; } else cube=1; // DriveLoop(-1, 800, 0.11, 0.000000, 0.001); // DriveLoop( 1, 800, 0.11, 0.000000, 0.001); if(match==1 && skyrise==1 && red==1)//------------------RED SKYRISE------- { SensorValue[latch] = 1; armDown(0, 90); wait1Msec(300); armUp(130, 90); autoFeed(300, -127); autoFeed(1,0); wait1Msec(500); armDown(105, 90); autoGrab(1); armUp(300, 90); liftMotors(-18); DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30); armDown(0, 90); autoGrab(0); //drop one armUp(160, 90); liftMotors(-10); DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30); armDown(105, 90); autoGrab(1); armUp(200, 90); DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(220, 90); autoGrab(0); //drop two armUp(290, 90); liftMotors(-15); DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(105, 90); autoGrab(1); armUp(390, 90); DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(320, 90); autoGrab(0); //drop three armUp(400, 90); Feed(-40); driveForward(127, -60, 160); Feed(-127); wait1Msec(1000); Feed(0); wait1Msec(5000000); } if(match==1 && skyrise==1 && blue==1)//---BLUE SKYRISE----- { SensorValue[latch] = 1; wait1Msec(300); armUp(160, 90); autoFeed(300, -127); autoFeed(1,0); wait1Msec(500); armDown(80, 90); autoGrab(1); armUp(185, 90); liftMotors(-20); DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(0, 90); autoGrab(0); //drop one armUp(180, 90); liftMotors(-10); DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30); armDown(80, 90); autoGrab(1); armUp(220, 90); DriveLoop(-1,-1, 925, 0.15, 0.00000001, 0.1, 120, 30); armDown(180, 90); autoGrab(0); //drop two armUp(250, 90); liftMotors(-15); DriveLoop(1,1, 925, 0.15, 0.00000001, 0.1, 120, 30); armDown(80, 90); autoGrab(1); armUp(340, 90); DriveLoop(-1,-1, 920, 0.15, 0.00000001, 0.1, 120, 30); armDown(320, 90); autoGrab(0); //drop three armUp(400, 90); driveForward(-60, 120, 100); Feed(-127); driveForward(-60, 120, 170); wait1Msec(5000); Feed(0); wait1Msec(5000000); } if(programmingSkills==1) //--------------------------------SKILLS------------- { SensorValue[latch] = 1; armDown(0, 90); wait1Msec(300); armUp(130, 90); autoFeed(300, -127); autoFeed(1,0); wait1Msec(500); armDown(90, 90); autoGrab(1); armUp(300, 90); liftMotors(-15); DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30); armDown(0, 90); autoGrab(0); //drop one armUp(160, 90); liftMotors(-10); DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30); armDown(90, 90); autoGrab(1); armUp(200, 90); DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(220, 90); autoGrab(0); //drop two armUp(290, 90); liftMotors(-10); DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(90, 90); autoGrab(1); armUp(380, 90); DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30); armDown(320, 90); autoGrab(0); //drop three armUp(370, 90); liftMotors(-15); DriveLoop(1,1, 905, 0.12, 0.00000001, 0.1, 80, 30); armDown(90, 90); autoGrab(1); armUp(560, 90); DriveLoop(-1,-1, 915, 0.12, 0.00000001, 0.1, 80, 30); armDown(530, 90); autoGrab(0); //drop four armUp(610, 90); liftMotors(-15); DriveLoop(1,1, 915, 0.12, 0.00000001, 0.1, 80, 30); armDown(90, 90); autoGrab(1); armUp(750, 90); DriveLoop(-1,-1, 925, 0.12, 0.00000001, 0.1, 80, 30); armDown(700, 90); autoGrab(0); //drop five armUp(750, 90); liftMotors(-15); DriveLoop(1,1, 950, 0.12, 0.00000001, 0.1, 80, 30); armDown(90, 90); autoGrab(1); armUp(950, 90); DriveLoop(-1,-1, 950, 0.11, 0.00000001, 0.1, 80, 30); armDown(900, 90); autoGrab(0); //drop six armUp(950, 90); liftMotors(-15); DriveLoop(1,1, 950, 0.11, 0.00000001, 0.1, 80, 30); armDown(90, 90); autoGrab(1); armUp(1220, 90); DriveLoop(-1,-1, 900, 0.12, 0.00000001, 0.1, 80, 30); armDown(1110, 90); autoGrab(0); //drop seven armUp(1220, 90); /*liftMotors(100); wait1Msec(500); liftMotors(15);*/ driveForward(70, -70, 225); driveForward(70, 70, 100); autoFeed(750, -127); SensorValue[conveyer] = 1; driveForward(70, -70, 825); armDown(20, 127); liftMotors(-100); wait1Msec(500); liftMotors(-15); driveForward(70, 70, 100); autoFeed(1000, 127); driveForward(70, -70, 225); driveForward(70, 70, 100); autoFeed(1000, 127); driveForward(70, -70, 1000); armUp(1220, 127); driveForward(70, 70, 225); autoFeed(750, -127); wait1Msec(500); driveForward(70, 70, 50); autoFeed(750, -127); driveForward(-70, -70, 200); /*DriveLoop(1,-1, 250, 0.17, 0.00000001, 0.05, 120, 43); DriveLoop(1,1, 140, 0.14, 0.00000001, 0.05, 80, 35); autoFeed(1500, -127); DriveLoop(-1,-1, 140, 0.14, 0.00000001, 0.05, 80, 35); liftMotors(-50); DriveLoop(-1,1, 350, 0.17, 0.00000001, 0.05, 120, 43); liftMotors(-15); DriveLoop(1,1, 825, 0.12, 0.00000001, 0.05, 80, 35); DriveLoop(-1,1, 410, 0.12, 0.00000001, 0.05, 120, 43); liftMotors(-127); wait1Msec(1250); liftMotors(-15); DriveLoop(1,1, 175, 0.12, 0.00000001, 0.05, 80, 35); autoFeed(750, 127); wait1Msec(5000000);*/ } if(match==1 && cube==1 && red==1) //-----------RED CUBES Match { DriveLoop(-1,1, 400, 0.12, 0.00000001, 0.05, 120, 50); DriveLoop( 1,-1, 400, 0.12, 0.00000001, 0.05, 120, 50); wait1Msec(50000000000); /*SensorValue[latch] = 1; driveStop(300); armUp(1550, 90); liftMotors(-10); autoFeed(600, -127); autoFeed(1,0); wait1Msec(50); armDown(1200, 120); liftMotors(-127); wait1Msec(250); liftMotors(-15); driveForward(100, 100, 425); autoFeed(900, 127); //feed in red1 stopRobot(10); liftMotors(60); driveForward(100, 100, 350); armUp(2300, 90); driveForward(90, -60, 250); autoFeed(800, -127); //feed out red1 autoFeed(1,0); driveForward(-90, 60, 420); armDown(1200, 110); driveForward(100, 100, 100); wait1Msec(50); driveForward(70, -70, 125); liftMotors(-127); wait1Msec(450); liftMotors(-15); driveForward(100, 100, 650); autoFeed(700, 127); //pick up blue autoFeed(1, 0); driveForward(-70, 70, 15); autoFeed(375, 127); //move blue autoFeed(1, 0); driveForward(100, 100, 530); autoFeed(600, 127); //pick up red2 driveForward(-70, 70, 10); autoFeed(1, 0); armUp(1800, 90); driveForward(90, -60, 250); wait1Msec(75); autoFeed(550, -127); //drop red2 autoFeed(1,0); driveBack(-90, -90, -100); driveForward(-70, 70, 420); armDown(1390, 90); liftMotors(-10); driveForward(90, 90, 400);*/ } if(match==1 && cube==1 && blue==1) //-----------BLUE CUBES------------- { SensorValue[latch] = 1; wait1Msec(300); armUp(1550, 90); liftMotors(-10); autoFeed(600, -127); autoFeed(1,0); wait1Msec(50); armDown(1425, 120); liftMotors(-127); wait1Msec(100); liftMotors(-15); SensorValue[conveyer] = 1; driveForward(100, 100, 425); autoFeed(925, 127); //feed in blue1 stopRobot(10); liftMotors(40); driveForward(100, 100, 415); armUp(2300, 90); driveForward(-90, 60, 265); // driveForward(90, 90, 20); autoFeed(800, -127); //feed out blue1 autoFeed(1,0); // driveBack(-90, -90, -20); driveForward(90, -60, 430); liftMotors(-40); driveForward(100, 100, 200); driveForward(-70, 70, 90); armDown(1425, 120); liftMotors(-127); wait1Msec(150); liftMotors(-15); driveForward(100, 100, 470); autoFeed(850, 127); //pick up red autoFeed(1, 0); // driveForward(-70, 70, 15); autoFeed(250, 127); //move red autoFeed(1, 0); driveForward(100, 100, 590); autoFeed(650, 127); //pick up blue2 // driveForward(-70, 70, 10); autoFeed(1, 0); armUp(1800, 90); driveForward(-90, 60, 290); driveForward(90, 90, 40); autoFeed(550, -127); //drop blue2 autoFeed(1,0); driveBack(-90, -90, -100); driveForward(-70, 70, 420); armDown(1390, 90); liftMotors(-10); driveForward(90, 90, 400); SensorValue[latch] = 1; wait1Msec(300); armUp(1550, 90); liftMotors(-10); autoFeed(600, -127); autoFeed(1,0); wait1Msec(50); armDown(1200, 120); liftMotors(-127); wait1Msec(250); liftMotors(-15); driveForward(100, 100, 425); autoFeed(900, 127); //feed in red1 stopRobot(10); liftMotors(60); driveForward(100, 100, 350); armUp(2300, 90); driveForward(-90, 60, 250); autoFeed(800, -127); //feed out red1 autoFeed(1,0); driveForward(90, -60, 420); armDown(1450, 110); driveForward(100, 100, 100); wait1Msec(50); driveForward(-70, 70, 85); armDown(1200, 110); liftMotors(-127); wait1Msec(450); liftMotors(-15); driveForward(100, 100, 650); autoFeed(700, 127); //pick up blue autoFeed(1, 0); // driveForward(-70, 70, 15); autoFeed(375, 127); //move blue autoFeed(1, 0); driveForward(100, 100, 530); autoFeed(600, 127); //pick up red2 // driveForward(-70, 70, 10); autoFeed(1, 0); armUp(1800, 90); driveForward(-90, 60, 260); wait1Msec(75); autoFeed(550, -127); //drop red2 autoFeed(1,0); driveBack(-90, -90, -100); driveForward(-70, 70, 420); armDown(1390, 90); liftMotors(-10); driveForward(90, 90, 400); } }
task autonomous(){ scoredSections = 0; direction = 1; SensorValue[pneuVal] = 0; if (SensorValue[disableAut] == 1){ //If we are on the blue side, preload and scoring are switched //To fix this, switch the direction of all horizontal motor movements if (SensorValue[nextToSection] == 0){ //If its plugged in //Switch direction so preload is now on the other side and we can use the same code direction = -1; } bool firstTime = true; while(scoredSections < 6){ if (firstTime){ //Release the arm only on the first time if (direction == -1){ motor[arm] = armSpeedAutonomous; wait1Msec(200); motor[arm] = 0; } //Raise the lift slightly (for 0.8 sec) to expand arm liftMotors(1); wait1Msec(600); //Open the clamp SensorValue[pneuVal] = 1; stopLift(); motor[arm] = direction * -armSpeedAutonomous; wait1Msec(250); motor[arm] = 0; } //Move arm to pick up section pickUpSection(); //Start lifting process //This needs to be our '0' spot so we come back to here SensorValue[shaftEncoder] = 0; //Regression Quadratic Equation int calculatedHeight; if (firstTime){ calculatedHeight = 235; } else{ calculatedHeight = (289.0 * scoredSections) + 85.33; } elevateLift(calculatedHeight); //Scoring moveArmToScore(direction * -1 * armSpeedUserControl); //if (scoredSections >= 2){ // wait1Msec(100); //} int nestValue = (firstTime) ? -330 : (int) (-867/7); elevateLift(nestValue); wait1Msec(50); SensorValue[pneuVal] = 1; int clearValue = (firstTime) ? 200 : 230; firstTime = false; elevateLift(clearValue); //Return to starting position //get the arm going in that direction for a bit so it doesn't get caught on anything //motor[arm] = (direction * armSpeedAutonomous); //wait1Msec(400); //motor[arm] = 0; clearSkyrise(); returnLiftToPreload(); scoredSections++; } } }
//Returns the lift back until shaft encoder count is 0 //Requires "ground" position to be 0 for this to work! void returnLiftToPreload(){ while (abs(SensorValue[shaftEncoder]) > 45){ liftMotors(-1); } stopLift(); }
//Uses the button sensor void returnLiftToGround(){ while (SensorValue[shaftEncoder] > 0){ //While its not pressed liftMotors(-1); } stopLift(); }