API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 26th, 2020
101
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 13.93 KB | None | 0 0
raw download clone embed print report
  1.   public void drive (char direction, double power, int inches, int angle) {
  2.  
  3.         robot.resetDriveEncoders();
  4.         robot.runWithEncoders();
  5.         setBrake();
  6.  
  7.         ElapsedTime looptime = new ElapsedTime();
  8.         double startLoop;
  9.         double endLoop;
  10.  
  11.         final double MAX_DRIVE = power;
  12.         final double MIN_DRIVE = 0.1;
  13.  
  14.         int totalDistance = robot.inchesToTicks(inches);
  15.         double accPhase = totalDistance / 3;
  16.         int offset;
  17.  
  18.         double MAX_ACC = (0.5 - 0.1) / (50/4);
  19.  
  20.         double topSpeed = (totalDistance/3)*MAX_ACC + MIN_DRIVE;
  21.         double decSlope = (-topSpeed / (2*totalDistance/3));
  22.         int currentPosition;
  23.         int currentDrive;
  24.  
  25.         boolean longDistance = false;
  26.         if (inches >= 40)
  27.         {
  28.             longDistance = true;
  29.         }
  30.  
  31.  
  32.  
  33.         double startAngle = angle;
  34.  
  35.         double currentAngle = robot.getCurrentAngle();
  36.  
  37.         telemetry.addData("Info", "Angle to turn " + angle + " current angle " + currentAngle);
  38.         telemetry.update();
  39.  
  40.         int angleTurn = (int) (angle - currentAngle);
  41.         if (angleTurn > 10)
  42.         {
  43.             if (angleTurn > 0)
  44.             {
  45.                 turn(1, 0.25, angleTurn);
  46.                 telemetry.addData("Turn", "turning left " + angleTurn);
  47.                 telemetry.update();
  48.             }
  49.             else if (angleTurn < 0)
  50.             {
  51.                 turn(2, 0.25, angleTurn);
  52.                 telemetry.addData("Turn", "turning right " + angleTurn);
  53.                 telemetry.update();
  54.             }
  55.         }
  56.  
  57.  
  58.         robot.currentAngle = startAngle;
  59.  
  60.         telemetry.addData("Angle: ", "Start Angle: " + startAngle);
  61.         telemetry.update();
  62.  
  63.  
  64.         RobotCore.frontLeftBeforeTravel = RobotCore.frontLeftDrive.getCurrentPosition();
  65.         RobotCore.frontRightBeforeTravel = RobotCore.frontRightDrive.getCurrentPosition();
  66.         RobotCore.backLeftBeforeTravel = RobotCore.backLeftDrive.getCurrentPosition();
  67.         RobotCore.backRightBeforeTravel = RobotCore.backRightDrive.getCurrentPosition();
  68.  
  69.         RobotCore.frontLeftCurrentPosition = 0;
  70.         RobotCore.frontRightCurrentPosition = 0;
  71.         RobotCore.backLeftCurrentPosition = 0;
  72.         RobotCore.backRightCurrentPosition = 0;
  73.  
  74.         currentPosition = RobotCore.frontLeftBeforeTravel;
  75.         currentDrive = 0;
  76.  
  77.  
  78.         switch (direction) {
  79.             case 'f':
  80.             case 'F':
  81.                 RobotCore.frontLeftTargetPosition = RobotCore.frontLeftBeforeTravel + totalDistance;
  82.                 RobotCore.frontRightTargetPosition = RobotCore.frontRightBeforeTravel + totalDistance;
  83.                 RobotCore.backLeftTargetPosition = RobotCore.backLeftBeforeTravel + totalDistance;
  84.                 RobotCore.backRightTargetPosition = RobotCore.backRightBeforeTravel + totalDistance;
  85.  
  86.                 double target = (RobotCore.frontLeftTargetPosition + RobotCore.frontRightTargetPosition + RobotCore.backLeftTargetPosition + RobotCore.backRightTargetPosition ) / 4;
  87.                 looptime.reset();
  88.                 while ( (currentPosition < target) && (Math.abs(currentPosition - target) > 50) && opModeIsActive()) {
  89.                     startLoop = looptime.milliseconds();
  90.                     robot.bulkData = robot.expansionHub.getBulkInputData();
  91.                     currentPosition = robot.bulkData.getMotorCurrentPosition(robot.frontLeftDrive);
  92.                     currentDrive = Math.abs(currentPosition - robot.frontLeftBeforeTravel);
  93.  
  94.                     if (longDistance)
  95.                     {
  96.                         if (currentPosition > (3*totalDistance/4))
  97.                         {
  98.                             robot.frontLeftPower = 0.15;
  99.                             robot.frontRightPower = 0.15;
  100.                             robot.backLeftPower = 0.15;
  101.                             robot.backRightPower = 0.15;
  102.  
  103.                             robot.frontLeftDrive.setVelocity(robot.frontLeftPower*2600);
  104.                             robot.frontRightDrive.setVelocity(robot.frontRightPower*2600);
  105.                             robot.backLeftDrive.setVelocity(robot.backLeftPower*2600);
  106.                             robot.backRightDrive.setVelocity(robot.backRightPower*2600);
  107.                             endLoop = looptime.milliseconds() - startLoop;
  108.                             NUM_END_LOOP++;
  109.                             AVG_END_LOOP = AVG_END_LOOP + endLoop;
  110.                             continue;
  111.                         }
  112.                     }
  113.                     offset = (int) (robot.getCurrentAngle());
  114.  
  115.  
  116.                     robot.frontLeftCurrentPosition = Math.abs(currentPosition - robot.frontLeftBeforeTravel);
  117.                     robot.frontRightCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.frontRightDrive) - robot.frontRightBeforeTravel);
  118.                     robot.backLeftCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.backLeftDrive) - robot.backLeftBeforeTravel);
  119.                     robot.backRightCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.backRightDrive) - robot.backRightBeforeTravel);
  120.  
  121.  
  122.  
  123.                     if (currentDrive < accPhase)
  124.                     {
  125.                         robot.frontLeftPower = (robot.frontLeftCurrentPosition*MAX_ACC) + MIN_DRIVE;
  126.                         robot.frontRightPower = (robot.frontRightCurrentPosition*MAX_ACC) + MIN_DRIVE;
  127.                         robot.backLeftPower = (robot.backLeftCurrentPosition*MAX_ACC) + MIN_DRIVE;
  128.                         robot.backRightPower = (robot.backRightCurrentPosition*MAX_ACC) + MIN_DRIVE;
  129.                     }
  130.                     else if (currentDrive > accPhase)
  131.                     {
  132.                         robot.frontLeftPower = (decSlope*(robot.frontLeftCurrentPosition - totalDistance) + MIN_DRIVE);
  133.                         robot.frontRightPower = (decSlope*(robot.frontRightCurrentPosition - totalDistance) + MIN_DRIVE);
  134.                         robot.backLeftPower = (decSlope*(robot.backLeftCurrentPosition - totalDistance) + MIN_DRIVE);
  135.                         robot.backRightPower = (decSlope*(robot.backRightCurrentPosition - totalDistance) + MIN_DRIVE);
  136.                         telemetry.addData("Phase", "Decelerate");
  137.                     }
  138.                     else
  139.                     {
  140.                         robot.frontLeftPower = MAX_DRIVE;
  141.                         robot.frontRightPower = MAX_DRIVE;
  142.                         robot.backLeftPower = MAX_DRIVE;
  143.                         robot.backRightPower = MAX_DRIVE;
  144.                         telemetry.addData("Phase", "Max");
  145.                     }
  146.  
  147.                     robot.frontLeftPower = Range.clip(robot.frontLeftPower, MIN_DRIVE, MAX_DRIVE);
  148.                     robot.frontRightPower = Range.clip(robot.frontRightPower, MIN_DRIVE, MAX_DRIVE);
  149.                     robot.backLeftPower = Range.clip(robot.backLeftPower, MIN_DRIVE, MAX_DRIVE);
  150.                     robot.backRightPower = Range.clip(robot.backRightPower, MIN_DRIVE, MAX_DRIVE);
  151.  
  152.  
  153.                     robot.frontLeftPower = robot.frontLeftPower + (offset - startAngle) / 100;
  154.                     robot.frontRightPower = robot.frontRightPower - (offset - startAngle) / 100;
  155.                     robot.backLeftPower = robot.backLeftPower + (offset - startAngle) / 100;
  156.                     robot.backRightPower = robot.backRightPower - (offset - startAngle) / 100;
  157.  
  158.  
  159.                     robot.velocityDrive();
  160.  
  161.  
  162.  
  163.                     endLoop = looptime.milliseconds() - startLoop;
  164.                     TOTAL_LOOP_TIME = TOTAL_LOOP_TIME + endLoop;
  165.                     NUMBER_OF_LOOPS++;
  166.                     if (endLoop > 70)
  167.                     {
  168.                         NUMBER_OF_SLOW_LOOPS++;
  169.                     }
  170.                     if (endLoop > MAX_LOOP_TIME)
  171.                     {
  172.                         MAX_LOOP_TIME = endLoop;
  173.                     }
  174.  
  175.                     telemetry.addData("Loop time: ", endLoop );
  176.                     telemetry.addData("Moving forward", robot.frontLeftPower);
  177.                 }
  178.                 break;
  179.             case 'b':
  180.             case 'B':
  181.                 RobotCore.frontLeftTargetPosition = RobotCore.frontLeftBeforeTravel - totalDistance;
  182.                 RobotCore.frontRightTargetPosition = RobotCore.frontRightBeforeTravel - totalDistance;
  183.                 RobotCore.backLeftTargetPosition = RobotCore.backLeftBeforeTravel - totalDistance;
  184.                 RobotCore.backRightTargetPosition = RobotCore.backRightBeforeTravel - totalDistance;
  185.  
  186.  
  187.                 target = (RobotCore.frontLeftTargetPosition + RobotCore.frontRightTargetPosition + RobotCore.backLeftTargetPosition + RobotCore.backRightTargetPosition ) / 4;
  188.                 looptime.reset();
  189.                 while ((currentPosition > target) && (Math.abs(currentPosition - target) > 50) && opModeIsActive()) {
  190.                     startLoop = looptime.milliseconds();
  191.                     robot.bulkData = robot.expansionHub.getBulkInputData();
  192.                     currentPosition = robot.bulkData.getMotorCurrentPosition(robot.frontLeftDrive);
  193.  
  194.                     currentDrive = Math.abs(currentPosition - robot.frontLeftBeforeTravel);
  195.                     if (longDistance)
  196.                     {
  197.                         if (currentDrive > (3*totalDistance/4))
  198.                         {
  199.                             robot.frontLeftPower = -0.15;
  200.                             robot.frontRightPower = -0.15;
  201.                             robot.backLeftPower = -0.15;
  202.                             robot.backRightPower = -0.15;
  203.  
  204.                             robot.frontLeftDrive.setVelocity(robot.frontLeftPower*2600);
  205.                             robot.frontRightDrive.setVelocity(robot.frontRightPower*2600);
  206.                             robot.backLeftDrive.setVelocity(robot.backLeftPower*2600);
  207.                             robot.backRightDrive.setVelocity(robot.backRightPower*2600);
  208.                             endLoop = looptime.milliseconds() - startLoop;
  209.                             NUM_END_LOOP++;
  210.                             AVG_END_LOOP = AVG_END_LOOP + endLoop;
  211.                             continue;
  212.                         }
  213.                     }
  214.  
  215.                     offset = (int) (robot.getCurrentAngle());
  216.  
  217.  
  218.  
  219.                     robot.frontLeftCurrentPosition = Math.abs(currentPosition - robot.frontLeftBeforeTravel);
  220.                     robot.frontRightCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.frontRightDrive) - robot.frontRightBeforeTravel);
  221.                     robot.backLeftCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.backLeftDrive) - robot.backLeftBeforeTravel);
  222.                     robot.backRightCurrentPosition = Math.abs(robot.bulkData.getMotorCurrentPosition(robot.backRightDrive) - robot.backRightBeforeTravel);
  223.  
  224.  
  225.                     if (currentDrive < accPhase)
  226.                     {
  227.                         robot.frontLeftPower = (robot.frontLeftCurrentPosition*MAX_ACC) + MIN_DRIVE;
  228.                         robot.frontRightPower = (robot.frontRightCurrentPosition*MAX_ACC) + MIN_DRIVE;
  229.                         robot.backLeftPower = (robot.backLeftCurrentPosition*MAX_ACC) + MIN_DRIVE;
  230.                         robot.backRightPower = (robot.backRightCurrentPosition*MAX_ACC) + MIN_DRIVE;
  231.                     }
  232.                     else if (currentDrive > accPhase)
  233.                     {
  234.                         robot.frontLeftPower = (decSlope*(robot.frontLeftCurrentPosition - totalDistance) + MIN_DRIVE);
  235.                         robot.frontRightPower = (decSlope*(robot.frontRightCurrentPosition - totalDistance) + MIN_DRIVE);
  236.                         robot.backLeftPower = (decSlope*(robot.backLeftCurrentPosition - totalDistance) + MIN_DRIVE);
  237.                         robot.backRightPower = (decSlope*(robot.backRightCurrentPosition - totalDistance) + MIN_DRIVE);
  238.                         telemetry.addData("Phase", "Decelerate");
  239.                     }
  240.                     else
  241.                     {
  242.                         robot.frontLeftPower = MAX_DRIVE;
  243.                         robot.frontRightPower = MAX_DRIVE;
  244.                         robot.backLeftPower = MAX_DRIVE;
  245.                         robot.backRightPower = MAX_DRIVE;
  246.                         telemetry.addData("Phase", "Max");
  247.                     }
  248.  
  249.                     robot.frontLeftPower = Range.clip(robot.frontLeftPower, MIN_DRIVE, MAX_DRIVE);
  250.                     robot.frontRightPower = Range.clip(robot.frontRightPower, MIN_DRIVE, MAX_DRIVE);
  251.                     robot.backLeftPower = Range.clip(robot.backLeftPower, MIN_DRIVE, MAX_DRIVE);
  252.                     robot.backRightPower = Range.clip(robot.backRightPower, MIN_DRIVE, MAX_DRIVE);
  253.  
  254.  
  255.                     robot.frontLeftPower = -(robot.frontLeftPower - (offset - startAngle) / 100);
  256.                     robot.frontRightPower = -(robot.frontRightPower + (offset - startAngle) / 100);
  257.                     robot.backLeftPower = -(robot.backLeftPower - (offset - startAngle) / 100);
  258.                     robot.backRightPower = -(robot.backRightPower + (offset - startAngle) / 100);
  259.  
  260.  
  261.                     robot.frontLeftDrive.setVelocity(robot.frontLeftPower*2600);
  262.                     robot.frontRightDrive.setVelocity(robot.frontRightPower*2600);
  263.                     robot.backLeftDrive.setVelocity(robot.backLeftPower*2600);
  264.                     robot.backRightDrive.setVelocity(robot.backRightPower*2600);
  265.  
  266.  
  267.  
  268.  
  269.                     endLoop = looptime.milliseconds() - startLoop;
  270.                     TOTAL_LOOP_TIME = TOTAL_LOOP_TIME + endLoop;
  271.                     NUMBER_OF_LOOPS++;
  272.                     telemetry.addData("Loop time: ", endLoop );
  273.                     telemetry.addData("Moving backward", robot.frontLeftPower);
  274.                     telemetry.update();
  275.  
  276.  
  277.                 }
  278.                 break;
  279.             default:
  280.                 break;
  281.  
  282.         }
  283.  
  284.         robot.drive(0);
  285.         robot.runWithEncoders();
  286.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!