Advertisement
Guest User

LeftRight Encoder function

a guest
Jan 23rd, 2020
99
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.81 KB | None | 0 0
  1. public void leftRight(double power, double leftDist, double rightDist, int seconds) {
  2.  
  3. int leftFrontDist, rightFrontDist, leftBackDist, rightBackDist;
  4. double correction;
  5.  
  6. // if (opmode.opModeIsActive()) {
  7.  
  8. int moveLeftTicks = (int) (leftDist * forwardTicks);
  9. int moveRightTicks = (int) (rightDist * forwardTicks);
  10.  
  11. // DOES THIS DO ANYTHING
  12.  
  13. /* robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  14. robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  15. robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  16. robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); */
  17.  
  18. leftFrontDist = robot.leftFront.getCurrentPosition() + moveLeftTicks;
  19. rightFrontDist = robot.rightFront.getCurrentPosition() + moveRightTicks;
  20. leftBackDist = robot.leftBack.getCurrentPosition() + moveLeftTicks;
  21. rightBackDist = robot.rightBack.getCurrentPosition() + moveRightTicks;
  22.  
  23. setTarget(power, leftFrontDist, rightFrontDist, leftBackDist, rightBackDist);
  24. motionTimer.reset();
  25. // how long the motors should run
  26. while (opmode.opModeIsActive()&&(motionTimer.seconds() < seconds)
  27. && (robot.leftFront.isBusy()
  28. && robot.rightFront.isBusy()
  29. && robot.leftBack.isBusy()
  30. && robot.rightBack.isBusy())) {
  31.  
  32. encoderData(leftFrontDist, rightFrontDist);
  33. opmode.telemetry.update();
  34.  
  35. // find how much we should correct by
  36.  
  37.  
  38. // corrections
  39. robot.leftFront.setPower(power);
  40. robot.rightFront.setPower(power);
  41. robot.leftBack.setPower(power);
  42. robot.rightBack.setPower(power);
  43. }
  44.  
  45. robot.stop();
  46. robot.setEncoders(true);
  47. opmode.sleep(stopTime);
  48. // }
  49. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement