-
-
Save shzcuber/1086325cca43de130e1773f8092c1094 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// Copyright (c) FIRST and other WPILib contributors. | |
// Open Source Software; you can modify and/or share it under the terms of | |
// the WPILib BSD license file in the root directory of this project. | |
package frc.robot.commands; | |
import edu.wpi.first.wpilibj2.command.CommandBase; | |
import frc.robot.subsystems.DriveTrain; | |
public class DriveDistance extends CommandBase { | |
private DriveTrain driveTrain; | |
private double distanceToTravel; | |
/** Creates a new DriveStraight. */ | |
public DriveDistance(DriveTrain driveTrain, double distanceToTravel) { | |
// Use addRequirements() here to declare subsystem dependencies. | |
addRequirements(driveTrain); | |
this.driveTrain = driveTrain; | |
this.distanceToTravel = distanceToTravel; | |
driveTrain.selectTurningPIDSlot(); | |
driveTrain.zeroSensors(); | |
} | |
// Called every time the scheduler runs while the command is scheduled. | |
@Override | |
public void execute() { | |
// driveTrain.arcadeDrive(speed, 0); | |
driveTrain.driveStraightToPosition(distanceToTravel); | |
} | |
// Returns true when the command should end. | |
@Override | |
public boolean isFinished() { | |
return this.driveTrain.getRightMotorsDistance() >= this.distanceToTravel; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment