API tools faq
paste
Advertisement
Guest User

Untitled

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