diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index 980f151..f44e0da 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -33,7 +33,8 @@ public class Robot extends TimedRobot { public static final Elevator ELEVATOR = new Elevator(); public static final Intake INTAKE = new Intake(); public static final Climber CLIMBER = new Climber(); - public static final OI OI = new OI(); + + public static final OI OI = new OI(); // Must initialize after subsystems private Command autonomousCommand; private SendableChooser chooser = new SendableChooser<>(); diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java new file mode 100644 index 0000000..044d6c6 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java @@ -0,0 +1,69 @@ +package org.usfirst.frc.team4737.robot.commands.drivetrain; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class AutoBlindAccelerate extends Command { + + private double percentPerSecond; + private double startPercent; + private double targetPercent; + + private boolean startLow; + + private double lastTime; + private double percent; + + public AutoBlindAccelerate(double percentPerSecond, double startPercent, double targetPercent) { + requires(Robot.DRIVETRAIN); + this.percentPerSecond = percentPerSecond; + this.startPercent = startPercent; + this.targetPercent = targetPercent; + } + + // Called just before this Command runs the first time + protected void initialize() { + lastTime = 0; + percent = startPercent; + startLow = startPercent < targetPercent; + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + double time = this.timeSinceInitialized(); + double delta = time - lastTime; + lastTime = time; + + // Adjust speed + if (startLow) percent += percentPerSecond * delta; + else percent -= percentPerSecond * delta; + + // Check bounds + if (startLow && percent > targetPercent) percent = targetPercent; + if (!startLow && percent < targetPercent) percent = targetPercent; + if (percent < -1) percent = -1; + if (percent > 1) percent = 1; + + // Apply speed + Robot.DRIVETRAIN.arcadeDrive(percent, 0); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return percent == targetPercent; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + +} diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java index ae9e3c4..bb36bc4 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java @@ -1,43 +1,35 @@ 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; +import edu.wpi.first.wpilibj.command.CommandGroup; /** * */ -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(); - } - +public class AutoBlindBaseline extends CommandGroup { + + public AutoBlindBaseline() { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + + addSequential(new AutoBlindAccelerate(0.1, 0, RobotMap.AUTO_BLIND_SPEED)); + addSequential(new AutoBlindDrive(RobotMap.AUTO_BLIND_TIME, RobotMap.AUTO_BLIND_SPEED)); + addSequential(new AutoBlindAccelerate(0.1, RobotMap.AUTO_BLIND_SPEED, 0)); + } + } diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java new file mode 100644 index 0000000..fc741dd --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4737.robot.commands.drivetrain; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.TimedCommand; + +/** + * + */ +public class AutoBlindDrive extends TimedCommand { + + private double percent; + + public AutoBlindDrive(double time, double percent) { + super(time); + requires(Robot.DRIVETRAIN); + + this.percent = percent; + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.DRIVETRAIN.arcadeDrive(percent, 0); + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + +}