SHARE
TWEET

Untitled

a guest Dec 8th, 2019 66 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
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top