diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java new file mode 100644 index 0000000..1840ed3 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +public class AutoDrive extends CommandBase { + private double distance; + // Determines if we should start driving + // Will be false if we already started driving + private boolean startDriving = true; + public AutoDrive(double distance, double timeout) { + requires(driveBase); + this.distance = distance; + setTimeout(timeout); + } + public AutoDrive(double distance) { + requires(driveBase); + this.distance = distance; + // Make the default timeout 2s + setTimeout(2.0d); + } + protected void initialize() { + } + protected void execute() { + if (startDriving) { + driveBase.pidDrive(distance); + } + startDriving = false; + } + protected void end() { + startDriving = true; + driveBase.stopDriving(); + } + protected void interrupted() { + end(); + } + protected boolean isFinished() { + return isTimedOut(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index bc48b40..616ee83 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -16,6 +16,12 @@ public class DriveBase extends Subsystem { public void initDefaultCommand() { setDefaultCommand(new Drive()); } + public void stopDriving() { + leftMotorOne.set(0); + leftMotorTwo.set(0); + rightMotorOne.set(0); + rightMotorTwo.set(0); + } public void driveArcade(double x, double y, double z, double sensitivity) { leftMotorOne.set(-y + (x + z)); leftMotorTwo.set(-y + (x + z));