Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- public void displaceGoldMineralv2(){
- double strafeSpeed = 0.2;
- double STRAFE_BOUND = 12 * COUNTS_PER_INCH;
- double initialPos = robot.leftFrontDrive.getCurrentPosition();
- int refreshTick = 0;
- int scanCount = 0;
- robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
- while(opModeIsActive() && !robot.cv.getAligned()){
- if(refreshTick != 0) refreshTick--;
- if(robot.cv.isFound()) {
- strafeSpeed /= 2;
- robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
- }
- if(Math.abs(robot.leftFrontDrive.getCurrentPosition() - initialPos) > STRAFE_BOUND && refreshTick == 0){
- strafeSpeed = -strafeSpeed;
- robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
- scanCount++;
- refreshTick = 5000;
- if(scanCount > 2) break;
- }
- }
- robot.setDrivePower(0,0);
- robot.cv.disable();
- encoderDrive(0.3, -26, -26, 5);
- encoderDrive(0.3, 25, 25, 5);
- sleep(100);
- // encoderTurn(TURN_POWER, LANDER_OFFSET, 5);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement