Guest User

Untitled

a guest
Dec 8th, 2019
80
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. public void encoderDrive(double inches, String direction, double timeoutS, double topPower)
  2. {
  3. stopAndReset();
  4. int TargetFL = 0;
  5. int TargetFR = 0;
  6. int TargetBL = 0;
  7. int TargetBR = 0;
  8. double errorFL = 0;
  9. double errorFR = 0;
  10. double errorBL = 0;
  11. double errorBR = 0;
  12. double powerFL = 0;
  13. double powerFR = 0;
  14. double powerBL = 0;
  15. double powerBR = 0;
  16.  
  17.  
  18. String heading = direction;
  19.  
  20. // Ensure that the opmode is still active
  21. if (opModeIsActive()) {
  22. if(heading == "f")
  23. {
  24. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  25. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  26. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  27. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  28.  
  29. }
  30.  
  31. else if(heading == "b")
  32. {
  33. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  34. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  35. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  36. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  37.  
  38.  
  39. }
  40.  
  41. else if(heading == "r")
  42. {
  43. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  44. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  45. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  46. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH); //weird should be +
  47.  
  48.  
  49. }
  50.  
  51. else if(heading == "l")
  52. {
  53. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  54. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  55. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH); // weird should be +
  56. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  57.  
  58. }
  59.  
  60. else
  61. {
  62. telemetry.addData("not a valid direction", heading );
  63. }
  64.  
  65.  
  66.  
  67. // Determine new target position, and pass to motor controller
  68.  
  69. robot.fLMotor.setTargetPosition(TargetFL);
  70. robot.fRMotor.setTargetPosition(TargetFR);
  71. robot.bRMotor.setTargetPosition(TargetBR);
  72. robot.bLMotor.setTargetPosition(TargetBL);
  73.  
  74.  
  75. // Turn On RUN_TO_POSITION
  76. robot.fLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  77. robot.fRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  78. robot.bRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  79. robot.bLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  80.  
  81.  
  82. // reset the timeout time and start motion.
  83. runtime.reset();
  84. /*robot.fLMotor.setPower(Speed);
  85. robot.fRMotor.setPower(Speed);
  86. robot.bRMotor.setPower(Speed);
  87. robot.bLMotor.setPower(Speed);*/
  88.  
  89.  
  90. // keep looping while we are still active, and there is time left, and both motors are running.
  91. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  92. // its target position, the motion will stop. This is "safer" in the event that the robot will
  93. // always end the motion as soon as possible.
  94. // However, if you require that BOTH motors have finished their moves before the robot continues
  95. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  96. while (opModeIsActive() &&
  97. (runtime.seconds() < timeoutS) && ((robot.fLMotor.isBusy() && robot.fRMotor.isBusy()) && robot.bLMotor.isBusy() && robot.bRMotor.isBusy()))
  98. {
  99. errorFL = TargetFL - robot.fLMotor.getCurrentPosition();
  100. errorFR = TargetFR - robot.fRMotor.getCurrentPosition();
  101. errorBL = TargetBL - robot.bLMotor.getCurrentPosition();
  102. errorBR = TargetBR - robot.bRMotor.getCurrentPosition();
  103.  
  104. powerFL = topPower * pidMultiplierDriving(errorFL);
  105. powerFR = topPower * pidMultiplierDriving(errorFR);
  106. powerBL = topPower * pidMultiplierDriving(errorBL);
  107. powerBR = topPower* pidMultiplierDriving(errorBR);
  108.  
  109. robot.fLMotor.setPower(Math.abs(powerFL));
  110. robot.fRMotor.setPower(Math.abs(powerFR));
  111. robot.bRMotor.setPower(Math.abs(powerBL));
  112. robot.bLMotor.setPower(Math.abs(powerBR));
  113. telemetry.addData("Path1", "Running to %7d :%7d :%7d :%7d", TargetFL, TargetFR, TargetBL, TargetBR);
  114.  
  115. telemetry.addData("Path2", "Running at %7d :%7d :%7d :%7d", robot.fLMotor.getCurrentPosition(), robot.fRMotor.getCurrentPosition(), robot.bLMotor.getCurrentPosition(), robot.bRMotor.getCurrentPosition());
  116. //telemetry.addData("speeds", "Running to %7f :%7f :%7f :%7f", speedfL, speedfR, speedfL, speedbR);
  117. telemetry.update();
  118. }
  119.  
  120. // Stop all motion;
  121. robot.fLMotor.setPower(0);
  122. robot.bLMotor.setPower(0);
  123. robot.fRMotor.setPower(0);
  124. robot.bRMotor.setPower(0);
  125.  
  126. // Turn off RUN_TO_POSITION
  127. robot.bRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  128. robot.bLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  129. robot.fRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  130. robot.fLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  131. // sleep(250); // optional pause after each move
  132. }
  133. stopAndReset();
  134. }
RAW Paste Data