diff --git a/src/org/usfirst/frc/team2521/robot/OI.java b/src/org/usfirst/frc/team2521/robot/OI.java index 8abeb6b..52832c4 100644 --- a/src/org/usfirst/frc/team2521/robot/OI.java +++ b/src/org/usfirst/frc/team2521/robot/OI.java @@ -2,8 +2,10 @@ import org.usfirst.frc.team2521.robot.commands.automation.DriveToGear; import org.usfirst.frc.team2521.robot.commands.base.RunAgitator; -import org.usfirst.frc.team2521.robot.commands.base.RunFeeder; -import org.usfirst.frc.team2521.robot.commands.groups.RunShooterAndAgitator; +import org.usfirst.frc.team2521.robot.commands.groups.AlignShooter; +import org.usfirst.frc.team2521.robot.commands.groups.RunShooterSubsystems; + +import java.util.stream.IntStream; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -13,31 +15,35 @@ * driver to control the robot. */ public class OI { + private static final int[] CUSTOM_BUTTONS = {2, 3, 4, 5}; + private final Joystick left; private final Joystick right; private final Joystick secondary; + private final Joystick custom; + private JoystickButton alignShooterButton; private JoystickButton driveToGearLeftButton; private JoystickButton driveToGearRightButton; - private JoystickButton runShooterAndAgitatorButton; - private JoystickButton runFeederButton; + private JoystickButton runShooterSubsystemsButton; private JoystickButton runAgitatorBackwardButton; private OI() { left = new Joystick(RobotMap.LEFT_STICK_PORT); right = new Joystick(RobotMap.RIGHT_STICK_PORT); secondary = new Joystick(RobotMap.SECONDARY_STICK_PORT); + custom = new Joystick(RobotMap.CUSTOM_STICK_PORT); // Right joystick buttons - driveToGearLeftButton = new JoystickButton(right, RobotMap.DRIVE_TO_GEAR_LEFT_PORT); - driveToGearRightButton = new JoystickButton(right, RobotMap.DRIVE_TO_GEAR_RIGHT_PORT); + alignShooterButton = new JoystickButton(left, RobotMap.ALIGN_SHOOTER_BUTTON_PORT); + driveToGearLeftButton = new JoystickButton(left, RobotMap.DRIVE_TO_GEAR_LEFT_PORT); + driveToGearRightButton = new JoystickButton(left, RobotMap.DRIVE_TO_GEAR_RIGHT_PORT); // Secondary joystick buttons - runShooterAndAgitatorButton = - new JoystickButton(secondary, RobotMap.RUN_SHOOTER_AND_AGITATOR_BUTTON_PORT); - runFeederButton = new JoystickButton(secondary, RobotMap.RUN_FEEDER_BUTTON_PORT); - runAgitatorBackwardButton = - new JoystickButton(secondary, RobotMap.RUN_AGITATOR_BACKWARD_BUTTON_PORT); + runShooterSubsystemsButton = new JoystickButton(secondary, + RobotMap.RUN_SHOOTER_SUBSYSTEMS_BUTTON_PORT); + runAgitatorBackwardButton = new JoystickButton(secondary, + RobotMap.RUN_AGITATOR_BACKWARD_BUTTON_PORT); setButtonListeners(); } @@ -49,6 +55,13 @@ public static OI getInstance() { return Holder.INSTANCE; } + public int getAutoMode() { + return IntStream.range(0, CUSTOM_BUTTONS.length) + .filter(i -> custom.getRawButton(CUSTOM_BUTTONS[i])) + .map(i -> (int) Math.pow(2, i)) + .sum(); + } + /** * @return the left joystick */ @@ -71,10 +84,10 @@ public Joystick getSecondaryStick() { } private void setButtonListeners() { + alignShooterButton.toggleWhenActive(new AlignShooter()); driveToGearLeftButton.toggleWhenActive(new DriveToGear(true)); driveToGearRightButton.toggleWhenActive(new DriveToGear(false)); - runShooterAndAgitatorButton.toggleWhenActive(new RunShooterAndAgitator()); - runFeederButton.whileActive(new RunFeeder()); + runShooterSubsystemsButton.toggleWhenActive(new RunShooterSubsystems()); runAgitatorBackwardButton.toggleWhenActive(new RunAgitator(false)); } diff --git a/src/org/usfirst/frc/team2521/robot/Robot.java b/src/org/usfirst/frc/team2521/robot/Robot.java index b5a5f44..87c0de4 100644 --- a/src/org/usfirst/frc/team2521/robot/Robot.java +++ b/src/org/usfirst/frc/team2521/robot/Robot.java @@ -38,6 +38,8 @@ public void robotInit() { agitator = new Agitator(); auto = new Auto(); + + sensors.setCVCamera(Sensors.Camera.REAR); } @Override @@ -47,6 +49,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { + Robot.sensors.setCVCamera(Sensors.Camera.FRONT); auto.start(); } diff --git a/src/org/usfirst/frc/team2521/robot/RobotMap.java b/src/org/usfirst/frc/team2521/robot/RobotMap.java index 4eb76b7..8ffde5a 100644 --- a/src/org/usfirst/frc/team2521/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2521/robot/RobotMap.java @@ -7,15 +7,16 @@ public class RobotMap { public static final int LEFT_STICK_PORT = 0; public static final int RIGHT_STICK_PORT = 1; public static final int SECONDARY_STICK_PORT = 2; + public static final int CUSTOM_STICK_PORT = 3; // Right joystick buttons - public static final int DRIVE_TO_GEAR_LEFT_PORT = 4; // On right stick - public static final int DRIVE_TO_GEAR_RIGHT_PORT = 5; // On right stick + public static final int ALIGN_SHOOTER_BUTTON_PORT = 7; // On Left stick + public static final int DRIVE_TO_GEAR_LEFT_PORT = 8; // On left stick + public static final int DRIVE_TO_GEAR_RIGHT_PORT = 9; // On left stick // Secondary joystick buttons - public static final int RUN_SHOOTER_AND_AGITATOR_BUTTON_PORT = 1; // On secondary stick - public static final int RUN_FEEDER_BUTTON_PORT = 2; // On secondary stick - public static final int RUN_AGITATOR_BACKWARD_BUTTON_PORT = 4; // On secondary stick + public static final int RUN_SHOOTER_SUBSYSTEMS_BUTTON_PORT = 1; // On secondary stick + public static final int RUN_AGITATOR_BACKWARD_BUTTON_PORT = 2; // On secondary stick public static final int FRONT_RIGHT_MOTOR = 10; public static final int FRONT_LEFT_MOTOR = 14; @@ -30,7 +31,17 @@ public class RobotMap { public static final int SHOOTER_MOTOR = 13; public static final int FEEDER_MOTOR = 12; - public static final int FRONT_ULTRA_PORT = 0; - public static final int REAR_ULTRA_PORT = 2; - public static final int SIDE_LIDAR_PORT = 1; + public static final int REAR_ULTRA_PORT = 1; + public static final int LEFT_LIDAR_PORT = 0; + public static final int RIGHT_LIDAR_PORT = 2; + + public final static class AutoModes { + public final static int NOTHING = 0; + public final static int CROSS_BASE_LINE = 6; + public final static int BALL_THEN_GEAR = 15; + public final static int GEAR_LEFT = 8; + public final static int GEAR_MIDDLE = 4; + public final static int GEAR_RIGHT = 2; + public final static int BALLS_ONLY = 1; + } } diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java deleted file mode 100644 index 7759dc7..0000000 --- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.usfirst.frc.team2521.robot.commands.automation; - -import org.usfirst.frc.team2521.robot.Robot; - -import edu.wpi.first.wpilibj.command.PIDCommand; - -/** - * This command drives to a specified angle using the navX. - */ -public class DriveToAngle extends PIDCommand { - private static final double P = 0.003; - private static final double I = 0; - private static final double D = 0; - - private static final double ERROR_THRESHOLD = 1; - private static final double MIN_OUTPUT = 0.1; - - private double targetAngle; - - /** - * @param targetAngle the angle we want to drive to - */ - public DriveToAngle(double targetAngle) { - super(P, I, D); - requires(Robot.drivetrain); - this.targetAngle = targetAngle; - getPIDController().setAbsoluteTolerance(ERROR_THRESHOLD); - } - - @Override - protected void initialize() { - Robot.sensors.resetNavxAngle(); - targetAngle += Robot.sensors.getNavxAngle(); - setSetpoint(targetAngle); - } - - @Override - protected boolean isFinished() { - return getPIDController().onTarget(); - } - - @Override - protected double returnPIDInput() { - return Robot.sensors.getNavxAngle(); - } - - @Override - protected void usePIDOutput(double output) { - if (Math.abs(output) < MIN_OUTPUT) { - output = Math.signum(output) * MIN_OUTPUT; - } - Robot.drivetrain.setLeft(output); - Robot.drivetrain.setRight(output); - } -} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java new file mode 100644 index 0000000..149c2ef --- /dev/null +++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java @@ -0,0 +1,67 @@ +package org.usfirst.frc.team2521.robot.commands.automation; + +import org.usfirst.frc.team2521.robot.Robot; + +import edu.wpi.first.wpilibj.command.PIDCommand; + +/** + * This abstract class provides a template for driving to the gear or the boiler. + */ +public abstract class DriveToBlob extends PIDCommand { + private static final double CAMERA_PROJ_PLANE_DISTANCE = 216.226; + + /** true if we're straight-on */ + protected boolean oriented = false; + /** true if we're on the left side of the target */ + private boolean onLeftSide; + private boolean hasFoundBlob = false; + + /** + * @param onLeftSide whether we're on the left side of the target + */ + protected DriveToBlob(double p, double i, double d, boolean onLeftSide) { + super(p, i, d); + this.onLeftSide = onLeftSide; + requires(Robot.drivetrain); + } + + protected abstract double getSlowSpeed(); + + @Override + protected abstract void initialize(); + + @Override + protected final void execute() { + // Angle between camera line of sight and target + double targetAngle = Math.atan(Robot.sensors.getCVOffsetX() / CAMERA_PROJ_PLANE_DISTANCE); + + // Convert angle to degrees + targetAngle *= 180 / Math.PI; + + // Consider ourselves oriented when our angle error is less than 20 + oriented = Math.abs(targetAngle) < 20; + + targetAngle += Robot.sensors.getNavxAngle(); + setSetpoint(targetAngle); + + if (Robot.sensors.getBlobFound()) { + hasFoundBlob = true; + } + } + + @Override + protected final double returnPIDInput() { + return Robot.sensors.getNavxAngle(); + } + + protected void setCurrentBlobFoundMotorSpeed() { + if (hasFoundBlob) { + Robot.drivetrain.setLeft(getSlowSpeed()); + Robot.drivetrain.setRight(getSlowSpeed()); + } else { + // Turn clockwise if we're too far left, counter-clockwise if we're too far right + Robot.drivetrain.setLeft(onLeftSide ? -getSlowSpeed() : getSlowSpeed()); + Robot.drivetrain.setRight(onLeftSide ? -getSlowSpeed() : getSlowSpeed()); + } + } +} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java new file mode 100644 index 0000000..c2f8efa --- /dev/null +++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java @@ -0,0 +1,54 @@ +package org.usfirst.frc.team2521.robot.commands.automation; + +import org.usfirst.frc.team2521.robot.Robot; +import org.usfirst.frc.team2521.robot.subsystems.Sensors; + +/** + * This command drives to the correct spot to shoot from automatically. + */ +public class DriveToBoiler extends DriveToBlob { + private static final double P = 0.008; + private static final double I = 0; + private static final double D = 0; + + private static final double P2 = 0.0125; + private static final double DISTANCE_SETPOINT = 35; + private static final double DISTANCE_ERROR_THRESHOLD = 3; + + public DriveToBoiler() { + super(P, I, D, false); + } + + @Override + protected double getSlowSpeed() { + return P2 * (DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches()); + } + + @Override + protected void initialize() { + Robot.sensors.setCVCamera(Sensors.Camera.REAR); + } + + @Override + protected boolean isFinished() { + return Math.abs(DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches()) < DISTANCE_ERROR_THRESHOLD; + } + + @Override + protected final void usePIDOutput(double output) { + if (Robot.sensors.getBlobFound()) { + // If we are already oriented, drive straight + if (oriented) { + Robot.drivetrain.setLeft(getSlowSpeed()); + Robot.drivetrain.setRight(getSlowSpeed()); + } else if (-output > 0) { + Robot.drivetrain.setLeft(output); + } else { + Robot.drivetrain.setRight(-output); + } + } else { + setCurrentBlobFoundMotorSpeed(); + } + } + +} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java deleted file mode 100644 index 0d73770..0000000 --- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc.team2521.robot.commands.automation; - -import org.usfirst.frc.team2521.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveToEncoder extends Command { - private final double ENC_PULSES_PER_REVOLUTION = 8192; - private final double WHEEL_DIAMETER = 6; - - private double distance; - - public DriveToEncoder(double distance) { - requires(Robot.drivetrain); - - distance = (distance / (WHEEL_DIAMETER * Math.PI)) * ENC_PULSES_PER_REVOLUTION; - distance += Robot.drivetrain.getRightPosition(); - - this.distance = distance; - } - - protected void execute() { - Robot.drivetrain.setPosition(distance); - } - - @Override - protected void end() { - Robot.drivetrain.setLeft(0); - Robot.drivetrain.setRight(0); - } - - @Override - protected boolean isFinished() { - return Math.abs(Robot.drivetrain.getRightEncoderError()) < 100; - } -} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java index b45ea0b..2543de5 100644 --- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java +++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java @@ -2,94 +2,54 @@ import org.usfirst.frc.team2521.robot.Robot; import org.usfirst.frc.team2521.robot.subsystems.Drivetrain; - -import edu.wpi.first.wpilibj.command.PIDCommand; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.usfirst.frc.team2521.robot.subsystems.Sensors; /** * This command drives to the gear drop-off automatically. */ -public class DriveToGear extends PIDCommand { +public class DriveToGear extends DriveToBlob { private static final double P = 0.008; private static final double I = 0; private static final double D = 0; - private static final double CAMERA_PROJ_PLANE_DISTANCE = 216.226; - - /** true if we're straight-on */ - private boolean oriented = false; - /** true if we're on the left side of the target */ - private boolean onLeftSide; - - private boolean hasFoundBlob = false; - - /** - * @param onLeftSide whether we're on the left side of the target - */ public DriveToGear(boolean onLeftSide) { - super(P, I, D); - this.onLeftSide = onLeftSide; - requires(Robot.drivetrain); + super(P, I, D, onLeftSide); } @Override - protected void execute() { - // Angle between camera line of sight and target - double targetAngle = Math.atan(Robot.sensors.getCVOffsetX() / CAMERA_PROJ_PLANE_DISTANCE); - - // Convert angle to degrees - targetAngle *= 180 / Math.PI; - - // Consider ourselves oriented when our angle error is less than 20 - oriented = Math.abs(targetAngle) < 20; - - targetAngle += Robot.sensors.getNavxAngle(); - setSetpoint(targetAngle); - - if (Robot.sensors.getBlobFound()) { - hasFoundBlob = true; - } + protected double getSlowSpeed() { + return Drivetrain.SLOW_SPEED; + } - if (Robot.DEBUG) { - SmartDashboard.putNumber("Drive to gear setpoint", targetAngle); - SmartDashboard.putBoolean("Oriented", oriented); - } + @Override + protected void initialize() { + Robot.sensors.setCVCamera(Sensors.Camera.FRONT); } @Override protected boolean isFinished() { - return false; + return !Robot.sensors.getBlobFound(); } @Override - protected double returnPIDInput() { - return Robot.sensors.getNavxAngle(); + protected void end() { + Robot.sensors.setCVCamera(Sensors.Camera.REAR); } @Override - protected void usePIDOutput(double output) { - if (Robot.DEBUG) { - SmartDashboard.putNumber("Drive to gear output", output); - } + protected final void usePIDOutput(double output) { if (Robot.sensors.getBlobFound()) { // If we are already oriented, drive straight if (oriented) { - Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED); - } else if (output < 0) { + Robot.drivetrain.setLeft(getSlowSpeed()); + Robot.drivetrain.setRight(getSlowSpeed()); + } else if (output > 0) { Robot.drivetrain.setLeft(output); } else { - Robot.drivetrain.setRight(output); + Robot.drivetrain.setRight(-output); } } else { - if (hasFoundBlob) { - Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED); - } else { - // Turn clockwise if we're too far left, counter-clockwise if we're too far right - Robot.drivetrain.setLeft(onLeftSide ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(onLeftSide ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED); - } + setCurrentBlobFoundMotorSpeed(); } } } diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java deleted file mode 100644 index c975399..0000000 --- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.usfirst.frc.team2521.robot.commands.automation; - -import org.usfirst.frc.team2521.robot.Robot; -import org.usfirst.frc.team2521.robot.subsystems.Drivetrain; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * This command drives to a specified ultrasonic distance automatically. - */ -public class DriveToUltra extends Command { - private static final double ERROR_THRESHOLD = 1; - /** {@code true} if we should use the front ultrasonic */ - protected boolean useRearUltra = false; - private double setpoint; - private double ultrasonicValue; - - /** - * @param setpoint the ultrasonic setpoint - * @param useRearUltra whether we should use the rear ultrasonic - */ - public DriveToUltra(double setpoint, boolean useRearUltra) { - this.setpoint = setpoint; - this.useRearUltra = useRearUltra; - } - - @Override - protected void execute() { - ultrasonicValue = useRearUltra ? Robot.sensors.getRearUltraInches() : Robot.sensors.getFrontUltraInches(); - - if (useRearUltra) { - Robot.drivetrain.setLeft(setpoint - ultrasonicValue > 0 ? Drivetrain.SLOW_SPEED : -Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(setpoint - ultrasonicValue > 0 ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED); - } else { - Robot.drivetrain.setLeft(setpoint - ultrasonicValue > 0 ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(setpoint - ultrasonicValue > 0 ? Drivetrain.SLOW_SPEED : -Drivetrain.SLOW_SPEED); - } - } - - @Override - protected boolean isFinished() { - if (Robot.DEBUG) { - SmartDashboard.putNumber("Drive to ultra setpoint", setpoint); - SmartDashboard.putNumber("Drive to ultra error", setpoint - ultrasonicValue); - } - return Math.abs(setpoint - ultrasonicValue) < ERROR_THRESHOLD; - } -} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java b/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java deleted file mode 100644 index a827111..0000000 --- a/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.usfirst.frc.team2521.robot.commands.automation; - -import org.usfirst.frc.team2521.robot.Robot; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * This command backs away from the intake, spins the robot 180 degrees, - * and drives back. - */ -public class Spintake extends CommandGroup { - public Spintake() { - addSequential(new RearDriveToUltra(10)); - addSequential(new DriveToAngle(180)); - addSequential(new RearDriveToUltra(0)); - } - - private static class RearDriveToUltra extends DriveToUltra { - public RearDriveToUltra(double setpoint) { - super(setpoint, false); - } - - @Override - protected void initialize() { - useRearUltra = Robot.sensors.getFrontUltraInches() > Robot.sensors.getRearUltraInches(); - } - } -} diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java b/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java new file mode 100644 index 0000000..3849447 --- /dev/null +++ b/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java @@ -0,0 +1,43 @@ +package org.usfirst.frc.team2521.robot.commands.automation; + +import org.usfirst.frc.team2521.robot.Robot; +import org.usfirst.frc.team2521.robot.subsystems.Sensors; + +import edu.wpi.first.wpilibj.command.PIDCommand; + +public class TurnToBoiler extends PIDCommand { + private static final double P = 0.0025; + private static final double I = 0.00015; + private static final double D = 0; + + public TurnToBoiler() { + super(P, I, D); + requires(Robot.drivetrain); + } + + @Override + protected void initialize() { + Robot.sensors.setCVCamera(Sensors.Camera.REAR); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected double returnPIDInput() { + return Robot.sensors.getCVOffsetX(); + } + + @Override + protected void usePIDOutput(double output) { + if (Robot.sensors.getBlobFound()) { + Robot.drivetrain.setLeft(-output); + Robot.drivetrain.setRight(output); + } else { + Robot.drivetrain.setLeft(0); + Robot.drivetrain.setRight(0); + } + } +} diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java index 99f9d23..a968cfa 100644 --- a/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java +++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java @@ -6,14 +6,13 @@ import edu.wpi.first.wpilibj.command.Command; public class RunDrivetrain extends Command { - public RunDrivetrain() { requires(Robot.drivetrain); } protected void execute() { Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED); - Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED); + Robot.drivetrain.setRight(Drivetrain.SLOW_SPEED); } @Override diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java index 7ab6c17..41fe348 100644 --- a/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java +++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team2521.robot.commands.base; import org.usfirst.frc.team2521.robot.Robot; +import org.usfirst.frc.team2521.robot.subsystems.Feeder; import edu.wpi.first.wpilibj.command.Command; @@ -8,15 +9,13 @@ * This command feeds the balls to the shooter. */ public class RunFeeder extends Command { - private static final double FEEDER_SPEED = 0.75; - public RunFeeder() { requires(Robot.feeder); } @Override protected void execute() { - Robot.feeder.setMotor(-FEEDER_SPEED); + Robot.feeder.setMotor(-Feeder.FEEDER_SPEED); } @Override diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java index 973a709..6928580 100644 --- a/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java +++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java @@ -13,7 +13,7 @@ public class RunShooter extends PIDCommand { private static final double I = 0; private static final double D = 0; - private static final int SETPOINT = -280; + private static final int SETPOINT = -290; public RunShooter() { super(P, I, D); @@ -28,10 +28,7 @@ public void initialize() { @Override public void execute() { - if (Robot.DEBUG) { - SmartDashboard.putNumber("Encoder Value", Robot.shooter.getEncVelocity()); - SmartDashboard.putNumber("Error", SETPOINT - Robot.shooter.getEncVelocity()); - } + SmartDashboard.putNumber("Encoder velocity", Robot.shooter.getEncVelocity()); } @Override @@ -52,6 +49,5 @@ protected double returnPIDInput() { @Override protected void usePIDOutput(double output) { Robot.shooter.setMotor(-output); - //Robot.shooter.setMotor(0.535); } } diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java b/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java new file mode 100644 index 0000000..b510d97 --- /dev/null +++ b/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java @@ -0,0 +1,22 @@ +package org.usfirst.frc.team2521.robot.commands.groups; + +import org.usfirst.frc.team2521.robot.commands.automation.DriveToBoiler; +import org.usfirst.frc.team2521.robot.commands.automation.TurnToBoiler; +import org.usfirst.frc.team2521.robot.commands.base.RunShooter; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.TimedCommand; + +/** + * This command turns until the boiler is centered for the camera, + * drives to a specified distance, and then runs the shooter. + */ +public class AlignShooter extends CommandGroup { + public AlignShooter() { + addParallel(new RunShooter()); + addSequential(new DriveToBoiler()); + addParallel(new TurnToBoiler()); + addSequential(new TimedCommand(1)); + addParallel(new RunShooterSubsystems()); + } +} diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java b/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java index e5b7738..875a259 100644 --- a/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java +++ b/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java @@ -1,13 +1,46 @@ package org.usfirst.frc.team2521.robot.commands.groups; +import org.usfirst.frc.team2521.robot.OI; +import org.usfirst.frc.team2521.robot.Robot; +import org.usfirst.frc.team2521.robot.RobotMap; import org.usfirst.frc.team2521.robot.commands.automation.DriveToGear; import org.usfirst.frc.team2521.robot.commands.base.RunDrivetrain; +import org.usfirst.frc.team2521.robot.subsystems.Sensors; import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.TimedCommand; public class Auto extends CommandGroup { public Auto() { - addSequential(new RunDrivetrain(), 2); - addSequential(new DriveToGear(false)); + switch (OI.getInstance().getAutoMode()) { + case RobotMap.AutoModes.NOTHING: + break; + case RobotMap.AutoModes.CROSS_BASE_LINE: + addSequential(new RunDrivetrain(), 2); + break; + case RobotMap.AutoModes.BALLS_ONLY: + addSequential(new RunDrivetrain(), 1); + addSequential(new AlignShooter()); + break; + case RobotMap.AutoModes.BALL_THEN_GEAR: + Robot.sensors.setCVCamera(Sensors.Camera.FRONT); + addSequential(new RunDrivetrain(), 1); + addSequential(new DriveToGear(false)); + addSequential(new RunDrivetrain(), 0.25); + addSequential(new TimedCommand(1)); + addSequential(new AlignShooter()); + break; + case RobotMap.AutoModes.GEAR_LEFT: + addSequential(new RunDrivetrain(), 1); + addSequential(new DriveToGear(false)); + break; + case RobotMap.AutoModes.GEAR_MIDDLE: + addSequential(new DriveToGear(false)); + break; + case RobotMap.AutoModes.GEAR_RIGHT: + addSequential(new RunDrivetrain(), 1); + addSequential(new DriveToGear(true)); + break; + } } } diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java deleted file mode 100644 index 12a4d37..0000000 --- a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.usfirst.frc.team2521.robot.commands.groups; - -import org.usfirst.frc.team2521.robot.commands.base.RunAgitator; -import org.usfirst.frc.team2521.robot.commands.base.RunShooter; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class RunShooterAndAgitator extends CommandGroup { - public RunShooterAndAgitator() { - addParallel(new RunAgitator(true)); - addParallel(new RunShooter()); - } -} diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java new file mode 100644 index 0000000..5d1dca5 --- /dev/null +++ b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java @@ -0,0 +1,33 @@ +package org.usfirst.frc.team2521.robot.commands.groups; + +import org.usfirst.frc.team2521.robot.Robot; +import org.usfirst.frc.team2521.robot.commands.base.RunAgitator; +import org.usfirst.frc.team2521.robot.commands.base.RunFeeder; +import org.usfirst.frc.team2521.robot.commands.base.RunShooter; +import org.usfirst.frc.team2521.robot.subsystems.Feeder; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * This command runs the agitator and the shooter immediately, then + * starts the feeder once the shooter is up to speed. + */ +public class RunShooterSubsystems extends CommandGroup { + private double speedCutoff = -200; + + public RunShooterSubsystems() { + addParallel(new RunAgitator(true)); + addParallel(new RunShooter()); + addSequential(new RunFeeder() { + private boolean upToSpeed = false; + + @Override + protected void execute() { + if (upToSpeed || Robot.shooter.getEncVelocity() < speedCutoff) { + upToSpeed = true; + Robot.feeder.setMotor(-Feeder.FEEDER_SPEED); + } + } + }); + } +} diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java index 3b571c4..c4eedb9 100644 --- a/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java @@ -18,7 +18,7 @@ */ public class Drivetrain extends Subsystem { /** Speed to set drivetrain to when we want to move at a slow, constant speed */ - public static final double SLOW_SPEED = 0.2; + public static final double SLOW_SPEED = 0.4; private final double P = 0.01; private final double I = 0; @@ -108,7 +108,7 @@ public void setLeft(double value) { */ public void setRight(double value) { frontRight.changeControlMode(TalonControlMode.PercentVbus); - frontRight.set(value); + frontRight.set(-value); rearRight.changeControlMode(TalonControlMode.Follower); rearRight.set(RobotMap.FRONT_RIGHT_MOTOR); diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java b/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java index 0448410..a246420 100644 --- a/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java +++ b/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java @@ -11,6 +11,8 @@ * indexer to the fly wheel. */ public class Feeder extends Subsystem { + public static final double FEEDER_SPEED = 0.75; + private CANTalon feeder; public Feeder() { diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java b/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java index 547c769..26908df 100644 --- a/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java +++ b/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java @@ -7,6 +7,7 @@ import org.usfirst.frc.team2521.robot.commands.base.DisplaySensors; import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.networktables.NetworkTable; @@ -17,9 +18,9 @@ * methods to show sensor data on the SmartDashboard. */ public class Sensors extends Subsystem { - private AnalogInput frontUltra; private AnalogInput rearUltra; - private AnalogInput sideLidar; + private AnalogInput leftLidar; + private AnalogInput rightLidar; private NetworkTable table; @@ -30,67 +31,64 @@ public class Sensors extends Subsystem { private double MED_LIDAR_B = -1.045; public Sensors() { - frontUltra = new AnalogInput(RobotMap.FRONT_ULTRA_PORT); rearUltra = new AnalogInput(RobotMap.REAR_ULTRA_PORT); - sideLidar = new AnalogInput(RobotMap.SIDE_LIDAR_PORT); + leftLidar = new AnalogInput(RobotMap.LEFT_LIDAR_PORT); + rightLidar = new AnalogInput(RobotMap.RIGHT_LIDAR_PORT); table = NetworkTable.getTable("Vision"); ahrs = new AHRS(SPI.Port.kMXP); ahrs.reset(); + + setCVThresholds(); } /** * Displays sensor data on the SmartDashboard. */ public void display() { + SmartDashboard.putNumber("Rear ultra distance", getRearUltraInches()); + SmartDashboard.putBoolean("Blob found", getBlobFound()); + if (Robot.DEBUG) { - SmartDashboard.putNumber("Angle", getNavxAngle()); - SmartDashboard.putBoolean("Blob found", getBlobFound()); + SmartDashboard.putNumber("CV offset", getCVOffsetX()); } } /** - * @return the raw voltage from the front (gear side) ultrasonic sensor - */ - private double getFrontUltraRaw() { - return frontUltra.getVoltage(); - } - - /** - * @return the raw voltage from the rear (shooter side) ultrasonic sensor + * @return the distance in inches from the rear (shooter side) ultrasonic sensor */ - private double getRearUltraRaw() { - return rearUltra.getVoltage(); + public double getRearUltraInches() { + return rearUltra.getVoltage() * 1000 / 9.8; } /** - * @return the distance in inches from the front (gear side) ultrasonic sensor + * @return the raw value from the side lidar */ - public double getFrontUltraInches() { - return getFrontUltraRaw() * 1000 / 9.8; + private double getLeftLidarRaw() { + return leftLidar.getValue(); } /** - * @return the distance in inches from the rear (shooter side) ultrasonic sensor + * @return the distance in inches from the side lidar */ - public double getRearUltraInches() { - return getRearUltraRaw() * 1000 / 9.8; + public double getLeftLidarInches() { + return MED_LIDAR_M / Math.pow(getLeftLidarRaw(), 2) + MED_LIDAR_B; } /** * @return the raw value from the side lidar */ - private double getSideLidarRaw() { - return sideLidar.getValue(); + private double getRightLidarRaw() { + return rightLidar.getValue(); } /** * @return the distance in inches from the side lidar */ - public double getSideLidarInches() { - return MED_LIDAR_M / Math.pow(getSideLidarRaw(), 2) + MED_LIDAR_B; + public double getRightLidarInches() { + return MED_LIDAR_M / Math.pow(getRightLidarRaw(), 2) + MED_LIDAR_B; } /** @@ -112,6 +110,48 @@ public boolean getBlobFound() { return table.getBoolean("blob_found", false); } + /** + * Sets which camera (front or back) vision code on the minnow + * board should use. + * + * @param cameraType desired camera to use + */ + public void setCVCamera(Camera cameraType) { + table.putBoolean("front_camera", cameraType == Camera.FRONT); + } + + /** + * Sets the minnow board's blob thresholds based on SmartDashboard + * preferences. + */ + private void setCVThresholds() { + double frontLowerRed = Preferences.getInstance().getDouble("front_lower_red", 0); + double frontLowerGreen = Preferences.getInstance().getDouble("front_lower_green", 55); + double frontLowerBlue = Preferences.getInstance().getDouble("front_lower_blue", 0); + double frontUpperRed = Preferences.getInstance().getDouble("front_upper_red", 50); + double frontUpperGreen = Preferences.getInstance().getDouble("front_upper_green", 175); + double frontUpperBlue = Preferences.getInstance().getDouble("front_upper_blue", 50); + double rearLowerRed = Preferences.getInstance().getDouble("rear_lower_red", 0); + double rearLowerGreen = Preferences.getInstance().getDouble("rear_lower_green", 55); + double rearLowerBlue = Preferences.getInstance().getDouble("rear_lower_blue", 0); + double rearUpperRed = Preferences.getInstance().getDouble("rear_upper_red", 50); + double rearUpperGreen = Preferences.getInstance().getDouble("rear_upper_green", 175); + double rearUpperBlue = Preferences.getInstance().getDouble("rear_upper_blue", 50); + + table.putNumber("front_lower_red", frontLowerRed); + table.putNumber("front_lower_green", frontLowerGreen); + table.putNumber("front_lower_blue", frontLowerBlue); + table.putNumber("front_upper_red", frontUpperRed); + table.putNumber("front_upper_green", frontUpperGreen); + table.putNumber("front_upper_blue", frontUpperBlue); + table.putNumber("rear_lower_red", rearLowerRed); + table.putNumber("rear_lower_green", rearLowerGreen); + table.putNumber("rear_lower_blue", rearLowerBlue); + table.putNumber("rear_upper_red", rearUpperRed); + table.putNumber("rear_upper_green", rearUpperGreen); + table.putNumber("rear_upper_blue", rearUpperBlue); + } + /** * @return the Navx's current angle measurement */ @@ -130,5 +170,9 @@ public void resetNavxAngle() { public void initDefaultCommand() { setDefaultCommand(new DisplaySensors()); } + + public enum Camera { + FRONT, REAR + } }