daily pastebin goal
69%
SHARE
TWEET

Untitled

a guest Mar 13th, 2018 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2.         Waypoint[] point = new Waypoint[] {
  3.                 new Waypoint(2.5,-1,45),
  4.                 new Waypoint(2,0,0),
  5.                 new Waypoint(1.5,0.5,34),
  6.                 new Waypoint(1,0,0),
  7.                 new Waypoint(0,0,0)
  8.         };
  9.        
  10.         matchLogger.writeClean("PATHFINDER STARTING");
  11.        
  12.         Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0);
  13.         Trajectory trajectory = Pathfinder.generate(point, config);
  14.        
  15.         TankModifier modifier = new TankModifier(trajectory).modify(0.627);
  16.        
  17.         EncoderFollower left = new EncoderFollower(modifier.getLeftTrajectory());
  18.         EncoderFollower right = new EncoderFollower(modifier.getRightTrajectory());
  19.        
  20.         left.configureEncoder(sensorInput.getLeftDriveEncoder(), 83, 0.127);
  21.         right.configureEncoder(sensorInput.getRightDriveEncoder(), 83, 0.127);
  22.        
  23.         left.configurePIDVA(1.0, 0.0, 0.0, 1 / 3, 0);
  24.         right.configurePIDVA(1.0, 0.0, 0.0, 1 / 3, 0);
  25.        
  26.         double l, r, turn;
  27.        
  28.         while(!left.isFinished() || !right.isFinished()) {
  29.             l = left.calculate(sensorInput.getLeftDriveEncoder());
  30.             r = right.calculate(sensorInput.getRightDriveEncoder());
  31.            
  32.             turn = -(0.8 * (-1.0/80.0) * (Pathfinder.d2r(left.getHeading()) - sensorInput.getAHRSYaw()));
  33.            
  34.             matchLogger.writeClean("PATHFINDER left = " + l + " right = " + r + " turn = " + turn);
  35.            
  36.             robotOutput.tankDrive(l + turn, r - turn);
  37.         }
  38.         robotOutput.tankDrive(0, 0);
  39.         robotOutput.setIntake(1.0);
  40.         Thread.sleep(10000);
  41.        
  42.     }
  43. }
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
 
Top