diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index 783d58a..9e300d5 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -31,7 +31,7 @@ public class Robot extends TimedRobot { public static final Intake INTAKE = new Intake(); public static final Climber CLIMBER = new Climber(); public static final ControlSystem CONTROLSYSTEM = new ControlSystem(); - + public static final OI OI = new OI(); // Must initialize after subsystems private Command autonomousCommand; @@ -45,6 +45,7 @@ public class Robot extends TimedRobot { public void robotInit() { chooser.addDefault("No Auto", null); chooser.addObject("Blind Baseline", new AutoBlindBaseline()); + // Add new autonomous routines here SmartDashboard.putData("Auto mode", chooser); } @@ -55,6 +56,7 @@ public void robotInit() { */ @Override public void disabledInit() { + // Puts robot into coast mode, allowing it to be pushed more easily new RelaxDrivetrain().start(); } diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java index 9043552..900c165 100644 --- a/src/org/usfirst/frc/team4737/robot/RobotMap.java +++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java @@ -39,14 +39,39 @@ public class RobotMap { /** * The voltage to give to the elevator for it to hold constant position with a * cube + * + * Increase this value if the elevator droops too much while holding the cube + */ + public static final double ELEVATOR_HOLD_V = 1; + /** + * The number of seconds to keep the elevator in place before letting the motor + * relax. To disable this, set the value to 0.1 */ - public static final double ELEVATOR_HOLD_V = 1; // TODO measure public static final double ELEVATOR_HOLD_TIME = 4; // TODO tune - public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6; // TODO tune - + /** + * The maximum speed the elevator is allowed to go down. This prevents + * unspooling too quickly, which causes the rope to fall off the spool. + */ + public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6; + + /** + * The value to multiply the throttle by when slow driving is enabled + */ public static final double DRIVE_SLOW_SCALE = 0.5; - + + /* + * Adjust these variables to tune autonomous + */ public static final double AUTO_BLIND_TIME = 5; public static final double AUTO_BLIND_SPEED = 0.3; + /** + * The steering value to use in autonomous to counteract any natural tendencies + * for the robot to veer left or right. A positive value (should) steer the + * robot to the right, and negative to the left. + * + * Tune this value accordingly. It may need to be adjusted throughout the + * competition. + */ + public static final double AUTO_BLIND_STEER = 0; } diff --git a/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java index 90bdeb5..c3ca684 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java +++ b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java @@ -37,4 +37,5 @@ protected void end() { // subsystems is scheduled to run protected void interrupted() { } + } diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java index 044d6c6..7a4bac8 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java @@ -1,69 +1,88 @@ 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; /** - * + * Autonomously accelerates/decelerates the robot to a target speed from a + * starting speed. */ 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() { - } - + + /** + * + * @param percentPerSecond + * The % speed to accelerate by per second. A value of 1 will + * accelerate the robot from a standstill to full speed in one + * second. + * @param startPercent + * The initial speed (between -1.0 and 1.0) of the robot. + * @param targetPercent + * The speed (between -1.0 and 1.0) to accelerate to. + */ + 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 based on time passed + 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; + + // Set drive speed + Robot.DRIVETRAIN.arcadeDrive(percent, RobotMap.AUTO_BLIND_STEER); + } + + // 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 bb36bc4..75f39e1 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java @@ -9,27 +9,39 @@ */ 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)); - } - + 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. + + /* + * Accelerates the robot to the configured speed, drives for the configured + * time, and decelerates to a stop. + * + * If the robot drives too far, decrease the time. If the robot drives too fast, + * decrease the speed. If the robot speeds up too fast, decrease the + * acceleration. Likewise, if it takes too long to speed up, increase + * acceleration. + * + * If the robot veers left or right, adjust the AUTO_BLIND_STEER value in RobotMap. + */ + double acceleration = 0.1; + addSequential(new AutoBlindAccelerate(acceleration, 0, RobotMap.AUTO_BLIND_SPEED)); + addSequential(new AutoBlindDrive(RobotMap.AUTO_BLIND_TIME, RobotMap.AUTO_BLIND_SPEED)); + addSequential(new AutoBlindAccelerate(acceleration, 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 index fc741dd..690ce04 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java @@ -1,39 +1,51 @@ 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.TimedCommand; /** - * + * Drives the robot forward autonomously without sensor input. Drives at a + * certain speed for a certain time. + * + * This should be used only in conjunction with AutoBlindAccelerate to get up to + * speed. If the robot does not accelerate first, it may jerk and tip over. */ 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() { - } - + + /** + * + * @param time + * The number of seconds to drive forward for. + * @param percent + * The percent output, or speed (between -1.0 and 1.0) to drive at. + */ + 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, RobotMap.AUTO_BLIND_STEER); + } + + // 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/RelaxDrivetrain.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java index 564a318..d2a04b2 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java @@ -5,7 +5,8 @@ import edu.wpi.first.wpilibj.command.InstantCommand; /** - * + * Configures the drivetrain to coast mode. This allows the robot to be pushed + * around more easily. */ public class RelaxDrivetrain extends InstantCommand { diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java index fe97da7..8ca2264 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java @@ -22,13 +22,10 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { // Control elevator movement, holding it in place by default + + // Grab input double input = Robot.OI.operator.getAxis("LS_Y").get(); - // Check if we're hitting the top - // if (input > 0 && Robot.ELEVATOR.isAtTop()) { - // input = 0; - // } - // Make sure we don't go down too fast if (input < RobotMap.ELEVATOR_MAX_DOWN_SPEED) { input = RobotMap.ELEVATOR_MAX_DOWN_SPEED; @@ -45,7 +42,6 @@ protected void execute() { // Set elevator speed Robot.ELEVATOR.setSpeed(input); - } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java index 1225ac0..cf0bc5a 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java @@ -28,7 +28,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - // If one second remaining, buzz controller + // If one second remaining, buzz controller to notify operator if (RobotMap.ELEVATOR_HOLD_TIME - this.timeSinceInitialized() < 1.0 && !startedBuzz) { new BuzzController(1, 0.3, Robot.OI.operator).start(); startedBuzz = true; diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java index 707e831..2e50df0 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java @@ -5,37 +5,38 @@ import edu.wpi.first.wpilibj.command.Command; /** - * + * Relaxes the elevator. Allows it to fall down on its own and disables any + * motor output. */ public class RelaxElevator extends Command { - public RelaxElevator() { - requires(Robot.ELEVATOR); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.ELEVATOR.setCoastMode(); - Robot.ELEVATOR.setHoldPosition(false); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.ELEVATOR.setSpeed(0); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // 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() { - } - + public RelaxElevator() { + requires(Robot.ELEVATOR); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.ELEVATOR.setCoastMode(); + Robot.ELEVATOR.setHoldPosition(false); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.ELEVATOR.setSpeed(0); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // 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/elevator/StopElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java index 6ada751..73a370f 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java @@ -5,7 +5,10 @@ import edu.wpi.first.wpilibj.command.Command; /** - * + * Default command for the elevator. Disables all motor output, but puts + * elevator in brake mode to prevent it from falling too fast. Keep in mind that + * enabling this command does not put the elevator in a safe state. That is, it + * does not stop it from chopping off fingers. */ public class StopElevator extends Command { diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java b/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java index 896678c..d02bd28 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java @@ -5,7 +5,8 @@ import edu.wpi.first.wpilibj.command.Subsystem; /** - * + * This subsystem currently does not exist on the robot. This class serves as a + * placeholder in case it is added. */ public class Climber extends Subsystem { diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java index a3b9936..5f7573a 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java @@ -30,6 +30,7 @@ public Elevator() { // Prevent motor from stalling too hard motor.configContinuousCurrentLimit(30, 30); motor.enableCurrentLimit(true); + // Use voltage compensation to keep inputs reliable // motor.configVoltageCompSaturation(12, 30); // motor.enableVoltageCompensation(true); diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java index 1e3d75a..1b6a2d1 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java @@ -12,9 +12,6 @@ */ public class Intake extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - private WPI_TalonSRX leftMotor; private WPI_TalonSRX rightMotor; @@ -31,7 +28,7 @@ public void initDefaultCommand() { /** * * @param speed - * ranges from -1 to 1. + * The speed (between -1.0 and 1.0) to run the intake motors at. */ public void setSpeed(double speed) { setLRSpeed(speed, speed);