diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index daaa0c9..980f151 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -7,6 +7,7 @@ package org.usfirst.frc.team4737.robot; +import org.usfirst.frc.team4737.robot.commands.drivetrain.AutoBlindBaseline; import org.usfirst.frc.team4737.robot.commands.drivetrain.RelaxDrivetrain; import org.usfirst.frc.team4737.robot.subsystems.Climber; import org.usfirst.frc.team4737.robot.subsystems.Drivetrain; @@ -34,8 +35,8 @@ public class Robot extends TimedRobot { public static final Climber CLIMBER = new Climber(); public static final OI OI = new OI(); - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + private Command autonomousCommand; + private SendableChooser chooser = new SendableChooser<>(); /** * This function is run when the robot is first started up and should be used @@ -43,9 +44,9 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // m_chooser.addDefault("Default Auto", new ExampleCommand()); - // chooser.addObject("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); + chooser.addDefault("No Auto", null); + chooser.addObject("Blind Baseline", new AutoBlindBaseline()); + SmartDashboard.putData("Auto mode", chooser); } /** @@ -77,7 +78,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); + autonomousCommand = chooser.getSelected(); /* * String autoSelected = SmartDashboard.getString("Auto Selector", "Default"); @@ -87,8 +88,8 @@ public void autonomousInit() { */ // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); + if (autonomousCommand != null) { + autonomousCommand.start(); } } @@ -106,8 +107,8 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java index ec7dec5..9043552 100644 --- a/src/org/usfirst/frc/team4737/robot/RobotMap.java +++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java @@ -45,5 +45,8 @@ public class RobotMap { public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6; // TODO tune public static final double DRIVE_SLOW_SCALE = 0.5; + + public static final double AUTO_BLIND_TIME = 5; + public static final double AUTO_BLIND_SPEED = 0.3; } diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java new file mode 100644 index 0000000..ae9e3c4 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java @@ -0,0 +1,43 @@ +package org.usfirst.frc.team4737.robot.commands.drivetrain; + +import org.usfirst.frc.team4737.robot.Robot; +import org.usfirst.frc.team4737.robot.RobotMap; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class AutoBlindBaseline extends Command { + + public AutoBlindBaseline() { + requires(Robot.DRIVETRAIN); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.DRIVETRAIN.setBrakeMode(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.DRIVETRAIN.arcadeDrive(RobotMap.AUTO_BLIND_SPEED, 0); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return this.timeSinceInitialized() > RobotMap.AUTO_BLIND_TIME; + } + + // Called once after isFinished returns true + protected void end() { + Robot.DRIVETRAIN.arcadeDrive(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + this.end(); + } + +}