From 45335d0535aa5be8abd9d2a4ada9e3f0e6cbc2aa Mon Sep 17 00:00:00 2001 From: Zach Smith Date: Sun, 14 Apr 2019 20:04:16 -0700 Subject: [PATCH] Commit Before Worlds - Modified Limelight code to use a new function. - Implemented automatic climbing code into the Climb command. -- What happened to the commits before champs, like AZNorth and Ventura? __The world may never know.__ --- build.gradle | 2 +- src/main/java/frc/auto/commands/Climb.java | 76 ++++++ .../java/frc/enums/ElevatorPositions.java | 11 +- src/main/java/frc/enums/IDs.java | 6 +- src/main/java/frc/enums/PIDFeedback.java | 2 +- src/main/java/frc/opmode/Autonomous.java | 27 ++- src/main/java/frc/opmode/Disabled.java | 9 +- src/main/java/frc/opmode/TeleOp.java | 216 ++++++++++++------ src/main/java/frc/opmode/Test.java | 44 ++++ src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotMap.java | 20 +- src/main/java/frc/subsystem/CameraSystem.java | 2 +- src/main/java/frc/subsystem/Climber.java | 82 +++++++ src/main/java/frc/subsystem/DriveTrain.java | 27 +-- src/main/java/frc/subsystem/Elevator.java | 117 ++++++---- src/main/java/frc/subsystem/Limelight.java | 27 ++- src/main/java/frc/subsystem/Pigeon.java | 17 +- src/main/java/frc/utils/DataPublisher.java | 12 +- 18 files changed, 532 insertions(+), 171 deletions(-) create mode 100644 src/main/java/frc/auto/commands/Climb.java create mode 100644 src/main/java/frc/subsystem/Climber.java diff --git a/build.gradle b/build.gradle index 627e18f..23b002c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.4.1" } def ROBOT_MAIN_CLASS = "frc.robot.Main" diff --git a/src/main/java/frc/auto/commands/Climb.java b/src/main/java/frc/auto/commands/Climb.java new file mode 100644 index 0000000..3b0cdb5 --- /dev/null +++ b/src/main/java/frc/auto/commands/Climb.java @@ -0,0 +1,76 @@ +package frc.auto.commands; + +import java.util.HashMap; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.auto.commands.utils.Command; +import frc.auto.commands.utils.ICommand; +import frc.robot.RobotMap; + +/** + * Drives the left and right motors a specified distance for a specified time. + */ +@Command(name = "Climb") +public class Climb extends ICommand { + NetworkTable nt; // Used for debugging by printing messages. + boolean isFront; // Switch for whether the front elevator is active. + + /** + * Empty constructor for use in serialization. Do not add anything to this! + * ... Do not call past midnight. + */ + public Climb(HashMap map) { + super(map); + } + + /** + * Creates a new instance of Climb with the following parameters. + * When run() is called, the robot will wobble itself onto the habitat. + * Once the duration has exceeded the runtime, the command will return. + * @param angleSetpoint Target angle for climbing. + * @param tiltThreshold Toggle climbing direction when the error exceeds the threshold. + * @param duration Duration to climb for before timing out. Not yet implemented. + */ + public Climb(double angleSetpoint, double tiltThreshold, double duration) { + super(); + this.parameters.put("angleSetpoint", angleSetpoint); + this.parameters.put("tiltThreshold", tiltThreshold); + this.parameters.put("duration", duration); + + nt = NetworkTableInstance.getDefault().getTable("datapublisher"); + } + + /** + * {@inheritDoc} + */ + @Override + public void run() { + RobotMap.logger.printStatus("Attempting to climb. Wish me luck!"); + + double error = this.parameters.get("angleSetpoint") - RobotMap.pigeon.getPitch(); + nt.getEntry("Climb Error").setNumber(error); + + if(isFront == true && error >= this.parameters.get("tiltThreshold")) { + isFront = !isFront; + RobotMap.logger.printDebug("Set direction on the climber to FALSE."); + } + else if(error < this.parameters.get("tiltThreshold")) { + isFront = !isFront; + RobotMap.logger.printDebug("Set direction on the climber to TRUE."); + } + + if(isFront == true) { + RobotMap.climber.disableArm(); + RobotMap.elevator.setInnerSpeed(-0.85); + } + else { + RobotMap.climber.enableArm(); + RobotMap.elevator.setInnerSpeed(0); + } + + RobotMap.climber.climbRollers.set(ControlMode.PercentOutput, 0.75); + } +} \ No newline at end of file diff --git a/src/main/java/frc/enums/ElevatorPositions.java b/src/main/java/frc/enums/ElevatorPositions.java index 037af84..7fa10b2 100644 --- a/src/main/java/frc/enums/ElevatorPositions.java +++ b/src/main/java/frc/enums/ElevatorPositions.java @@ -1,12 +1,15 @@ package frc.enums; /** - * ElevatorPositions + * The main elevator has two values: inner elevator and outer elevator. + * The climbing elevator has one value: inner elevator. The second value is unused. */ public enum ElevatorPositions { - CARGO_LOW(15837, 3382), CARGO_MEDIUM(4500, -3554), CARGO_HIGH(26331, -19072), - HATCH_LOW(0, 0), HATCH_MEDIUM(0, 0), HATCH_HIGH(0, 0), - NEUTRAL(704, 3378); + CARGO_LOW(-9500, 0), CARGO_MEDIUM(-900, 9000), CARGO_HIGH(150, 23500), + HATCH_LOW(-22014, 0), HATCH_MEDIUM(-4500, 0), HATCH_HIGH(-316, 15302), + CARGO_SHIP(-2000, 0), + CLIMB_NEUTRAL(-10, 0), CLIMB_ENABLED(-895668, 0), CLIMB_MAIN(-24000, 0), + NEUTRAL(-22014, 0); private final int innerEncoderPos; private final int outerEncoderPos; diff --git a/src/main/java/frc/enums/IDs.java b/src/main/java/frc/enums/IDs.java index 176eea4..673abec 100644 --- a/src/main/java/frc/enums/IDs.java +++ b/src/main/java/frc/enums/IDs.java @@ -7,13 +7,15 @@ public enum IDs { // Talon SRX IDs LEFT(1), LEFT_FOLLOWER(3), RIGHT(2), RIGHT_FOLLOWER(4), ELEVATOR_INNER(10), ELEVATOR_OUTER(11), ROLLERS(12), + CLIMB_ROLLERS(30), CLIMB_FOLLOWER(16), CLIMB_ARM(33), // Pneumatic IDs DRIVE_SHIFTER(4), PCM(21), ELEVATOR_PISTONS(5), VACUUM_ENABLE(6), - VACUUM_DISABLE(7), + VACUUM_DISABLE(7), CLIMB_PISTON(3), // Sensor IDs - ELEVATOR_ULTRASONIC(3), ELEVATOR_DIGITAL(0), + ELEVATOR_ULTRASONIC(3), ELEVATOR_DIGITAL(0), ELEVATOR_LIGHT(2), + PIGEON(31), // Input Device IDs LEFT_JOYSTICK(0), RIGHT_JOYSTICK(1), SECONDARY_OPERATOR(2); diff --git a/src/main/java/frc/enums/PIDFeedback.java b/src/main/java/frc/enums/PIDFeedback.java index c3b8370..65c9d3e 100644 --- a/src/main/java/frc/enums/PIDFeedback.java +++ b/src/main/java/frc/enums/PIDFeedback.java @@ -4,7 +4,7 @@ * Provides PIDF values for different subsystems. */ public enum PIDFeedback { - INNER_ELEVATOR(0.2, 0, 0, 0), OUTER_ELEVATOR(0, 0, 0, 0); + INNER_ELEVATOR(0.1, 0, 0, 0), OUTER_ELEVATOR(0.35, 0, 0, 0); private final double kP; private final double kI; diff --git a/src/main/java/frc/opmode/Autonomous.java b/src/main/java/frc/opmode/Autonomous.java index 9313af9..28d7578 100755 --- a/src/main/java/frc/opmode/Autonomous.java +++ b/src/main/java/frc/opmode/Autonomous.java @@ -6,6 +6,8 @@ import com.google.common.base.Stopwatch; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import frc.robot.RobotMap; @@ -14,6 +16,8 @@ */ public class Autonomous extends OpMode { + NetworkTable instance; + public Autonomous() { super(); } @@ -24,6 +28,13 @@ public void init() { RobotMap.compressor.set(false); RobotMap.driveTrain.zeroSensors(); driveTrain.configPeakOutput(0.3); + RobotMap.elevator.elevatorForward(); + + instance = NetworkTableInstance.getDefault().getTable("parameters"); + instance.getEntry("innerSpeed").setDouble(0); + instance.getEntry("outerSpeed").setDouble(0); + instance.getEntry("innerSetpoint").setDouble(-22014); + instance.getEntry("outerSetpoint").setDouble(0); } @Override @@ -33,7 +44,21 @@ public void loop() { // RobotMap.driveTrain.set(-1, -1); // sleep(5000); - limelight.drive(); + //limelight.drive(); + + // double innerSpeed = instance.getEntry("innerSpeed").getDouble(0); + // double outerSpeed = instance.getEntry("outerSpeed").getDouble(0); + + // elevator.setInnerSpeed(innerSpeed); + // elevator.setOuterSpeed(outerSpeed); + + // double innerSetpoint = instance.getEntry("innerSetpoint").getDouble(0); + // double outerSetpoint = instance.getEntry("outerSetpoint").getDouble(0); + + // elevator.innerStage.set(ControlMode.Position, innerSetpoint); + // elevator.outerStage.set(ControlMode.Position, outerSetpoint); + + } @SuppressWarnings("unused") diff --git a/src/main/java/frc/opmode/Disabled.java b/src/main/java/frc/opmode/Disabled.java index d17642e..7264185 100755 --- a/src/main/java/frc/opmode/Disabled.java +++ b/src/main/java/frc/opmode/Disabled.java @@ -1,5 +1,7 @@ package frc.opmode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + import frc.robot.RobotMap; /** @@ -13,13 +15,14 @@ public Disabled() { @Override public void init() { + RobotMap.elevator.innerStage.setNeutralMode(NeutralMode.Coast); RobotMap.limelight.setLights(false); + RobotMap.limelight.setMode(true); } @Override public void loop() { - + RobotMap.elevator.zeroEncoder(); + RobotMap.climber.zeroEncoder(); } - - } \ No newline at end of file diff --git a/src/main/java/frc/opmode/TeleOp.java b/src/main/java/frc/opmode/TeleOp.java index a52d044..4b00be5 100755 --- a/src/main/java/frc/opmode/TeleOp.java +++ b/src/main/java/frc/opmode/TeleOp.java @@ -2,9 +2,13 @@ import static frc.robot.RobotMap.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.google.common.base.Stopwatch; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import frc.auto.commands.Climb; import frc.enums.ElevatorPositions; import frc.enums.Shifter; @@ -13,6 +17,9 @@ */ public class TeleOp extends OpMode { + DriverStation ds = DriverStation.getInstance(); + Climb climbCommand; + public TeleOp() { super(); } @@ -23,111 +30,182 @@ public void init() { compressor.set(true); driveTrain.configPeakOutput(1); driveTrain.setShifter(Shifter.LOW); + elevator.innerStage.setNeutralMode(NeutralMode.Brake); + elevator.outerStage.setNeutralMode(NeutralMode.Brake); + + climber.disableArm(); + climbCommand = new Climb(20, -10, 0); } double[] speeds = new double[2]; - double sum = 0; + double sum = 0; @Override public void loop() { - speeds = driveTrain.getSpeeds(); - sum = speeds[0] + speeds[1]; - - //Limelight control. - if(driveTrain.joystick1.getRawButton(4)) + // Limelight control. + if (driveTrain.leftJoystick.getRawButton(4)) { + // limelight.setMode(false); + limelight.setLights(true); limelightControl(); - else + } else { + limelight.setMode(true); + limelight.setLights(true); operatorControl(); - - //if(driveTrain.joystick1.getRawButtonPressed(3)) - //cameraSystem.toggle(); + } } public void limelightControl() { + if (elevator.getIRBeam() == true) // Cargo + isHatch = false; + else if (elevator.getIRBeam() == false) + isHatch = true; + + if (ds.isFMSAttached()) { + // Pipeline 2 is hatches, pipeline 3 is cargo + if (isHatch == true) + limelight.setPipeline(2); + else + limelight.setPipeline(3); + } else { + if (isHatch == true) + limelight.setPipeline(5); + else + limelight.setPipeline(4); + } + limelight.drive(); } Stopwatch sw = Stopwatch.createUnstarted(); boolean elevatorHeld = false; + boolean isHatch = true; + boolean climbHeld = false; + boolean temp = false; + final double ultrasonicTolerance = 0.32; + public void operatorControl() { - // Driving code. - //this is one joystick - //driveTrain.set(-driveTrain.getJoystickY() + driveTrain.getJoystickX(), -driveTrain.getJoystickY() - driveTrain.getJoystickX()); - //this is two joysticks - driveTrain.set(-driveTrain.getJoystickY(), - driveTrain.getJoystick2Y()); - if (driveTrain.isTriggerPressed()) + /* Driving code. */ + driveTrain.set(-driveTrain.leftJoystick.getY(), -driveTrain.rightJoystick.getY()); + + if (driveTrain.leftJoystick.getRawButtonPressed(3) || driveTrain.rightJoystick.getRawButtonPressed(3)) driveTrain.setShifter(!driveTrain.getShifterState().getValue()); - // Elevator code. - boolean elevatorPressed = driveTrain.joystick1.getRawButton(8); - if(elevatorHeld == false) - if(elevatorPressed) + /* Elevator code. */ + // Toggles the elevator from the down position to the up. + boolean elevatorPressed = driveTrain.leftJoystick.getRawButton(8); + if (elevatorHeld == false) + if (elevatorPressed) elevator.elevatorToggle(); elevatorHeld = elevatorPressed ? true : false; - ElevatorPositions desiredPos = driveTrain.getDesiredElevator(); - if(desiredPos != ElevatorPositions.NEUTRAL) - elevator.set(desiredPos); + if (elevator.getIRBeam() == true) // Cargo + isHatch = false; + else if (elevator.getIRBeam() == false) + isHatch = true; + + if (!driveTrain.secondaryOperator.getRawButton(9)) { + if (isHatch) { + // HATCHES + if (driveTrain.secondaryOperator.getRawButton(4)) // Low + elevator.set(ElevatorPositions.HATCH_LOW); + else if (driveTrain.secondaryOperator.getRawButton(5)) // Medium + elevator.set(ElevatorPositions.HATCH_MEDIUM); + else if (driveTrain.secondaryOperator.getRawButton(6)) // High + elevator.set(ElevatorPositions.HATCH_HIGH); + else + elevator.disablePower(); + } else if (!isHatch) { + // CARGO + if (driveTrain.secondaryOperator.getRawButton(3)) + elevator.set(ElevatorPositions.CARGO_SHIP); + else if (driveTrain.secondaryOperator.getRawButton(4)) + elevator.set(ElevatorPositions.CARGO_LOW); + else if (driveTrain.secondaryOperator.getRawButton(5)) + elevator.set(ElevatorPositions.CARGO_MEDIUM); + else if (driveTrain.secondaryOperator.getRawButton(6)) + elevator.set(ElevatorPositions.CARGO_HIGH); + else + elevator.disablePower(); + } + } else { + elevator.outerStage.set(ControlMode.Position, 0); + + if (false/** driveTrain.secondaryOperator.getRawButton(3) */) { + climbCommand.run(); + } else { + if (driveTrain.secondaryOperator.getRawButton(4)) + elevator.setInnerSpeed(-0.85); + else if (driveTrain.secondaryOperator.getRawButton(5)) + elevator.setInnerSpeed(0.55); + else + elevator.setInnerSpeed(0); + + // boolean climbPressed = driveTrain.secondaryOperator.getRawButton(7); + // if(climbHeld == false) + // if(climbPressed) + // climber.climbPistons.set(!climber.climbPistons.get()); + // climbHeld = climbPressed ? true : false; + + // boolean climbPressed = driveTrain.secondaryOperator.getRawButton(7); + // if(climbHeld == false) + // if(climbPressed) + // if(climber.isNeutral == true) { + // climber.enableArm(); + // //elevator.set(ElevatorPositions.CLIMB_MAIN); + // } + // else { + // climber.disableArm(); + // elevator.disablePower(); + // } + // climbHeld = climbPressed ? true : false; + + if (driveTrain.secondaryOperator.getRawButton(6)) + climber.enableArm(); + else if (driveTrain.secondaryOperator.getRawButton(7)) + climber.disableArm(); + else + climber.zeroPower(); + + climber.climbRollers.set(ControlMode.PercentOutput, -driveTrain.getJoystickY()); + } + } // Elevator manual override. Positive is up. - if(driveTrain.secondaryOperator.getRawButton(3)) - elevator.setInnerSpeed(0.55); - else if(driveTrain.secondaryOperator.getRawButton(7)) - elevator.setInnerSpeed(-0.55); - else - elevator.setInnerSpeed(0); - - // Positive is in. - if(driveTrain.joystick1.getRawButton(3) || driveTrain.joystick2.getRawButton(3)) + // if(driveTrain.secondaryOperator.getRawButton(3)) + // elevator.setInnerSpeed(0.55); + // else if(driveTrain.secondaryOperator.getRawButton(7)) + // elevator.setInnerSpeed(-0.55); + // else + // elevator.setInnerSpeed(0); + + // Sets the intake rollers. Positive is in. + if (driveTrain.leftJoystick.getTrigger()) elevator.intakeRollers(); - else if(driveTrain.joystick1.getRawButton(2) || driveTrain.joystick2.getRawButton(2)) + else if (driveTrain.rightJoystick.getTrigger()) elevator.outtakeRollers(); else elevator.setRollers(0); - // Vacuum code. - boolean vacuumEnable = driveTrain.joystick1.getRawButton(5); - boolean vacuumRelease = driveTrain.joystick2.getRawButton(5); - if(!(vacuumEnable && vacuumRelease)) { - if(vacuumEnable) + // Vacuum pump code. + boolean vacuumEnable = driveTrain.leftJoystick.getRawButton(5); + boolean vacuumRelease = driveTrain.rightJoystick.getRawButton(5); + if (!(vacuumEnable && vacuumRelease)) { + if (vacuumEnable) elevator.enableVacuum(true); else elevator.enableVacuum(false); - - if(vacuumRelease) + + if (vacuumRelease) elevator.releaseVacuum(true); else elevator.releaseVacuum(false); } - - /** Automatic shifting code. It doesn't work. :( */ - - // if(Math.abs(driveTrain.left.getSelectedSensorVelocity() + driveTrain.right.getSelectedSensorVelocity()) > 10000) { - // driveTrain.setShifter(true); - // } - // else { - // driveTrain.setShifter(false); - // } - - // switch(driveTrain.getShifterState()) { - // // 20,000 is the max velocity for one side. - // case HIGH: - // if(sum < 10000 && driveTrain.getShifterState() == Shifter.HIGH) - // driveTrain.setShifter(Shifter.LOW); - // break; - // // 10,000 is the max velocity for one side. - // case LOW: - // if(sum > 10000 && driveTrain.getShifterState() == Shifter.LOW) - // driveTrain.setShifter(Shifter.HIGH); - // break; - // } - - if(Math.abs(sum) > 24000) { - if(sum < 0 && cameraSystem.getStatus()) - cameraSystem.setRear(); - else if(sum > 0 && !cameraSystem.getStatus()) - cameraSystem.setFront(); - } + + // if(driveTrain.leftJoystick.getRawButton(5) || + // driveTrain.rightJoystick.getRawButton(5)) + // elevator.vacuumPump.set(ControlMode.PercentOutput, 1); + // else + // elevator.vacuumPump.set(ControlMode.PercentOutput, 0) } } \ No newline at end of file diff --git a/src/main/java/frc/opmode/Test.java b/src/main/java/frc/opmode/Test.java index c828265..d0a6a13 100755 --- a/src/main/java/frc/opmode/Test.java +++ b/src/main/java/frc/opmode/Test.java @@ -1,11 +1,17 @@ package frc.opmode; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import static frc.robot.RobotMap.*; + +import com.ctre.phoenix.motorcontrol.NeutralMode; + /** * Test */ public class Test extends OpMode { + Joystick gamepad; public Test() { super(); @@ -14,11 +20,49 @@ public Test() { @Override public void init() { Shuffleboard.selectTab("Test"); + gamepad = new Joystick(4); + elevator.innerStage.setNeutralMode(NeutralMode.Brake); + //elevator.elevatorBackward(); } + boolean isHatch = false; + @Override public void loop() { + // Hatch positions + // if (elevator.getIRBeam() == true) { // Cargo + // isHatch = false; + // } + // else if (elevator.getIRBeam() == false) { + // isHatch = true; + // } + + // if (isHatch == true) { + // // HATCHES + // if (gamepad.getRawButton(1)) // Low + // elevator.set(ElevatorPositions.HATCH_LOW); + // else if (gamepad.getRawButton(2) || gamepad.getRawButton(3)) // Medium + // elevator.set(ElevatorPositions.HATCH_MEDIUM); + // else if (gamepad.getRawButton(4)) // High + // elevator.set(ElevatorPositions.HATCH_HIGH); + // else + // elevator.disablePower(); + // } + // else if (isHatch == false) { + // // CARGO + // if (gamepad.getRawButton(1)) + // elevator.set(ElevatorPositions.CARGO_LOW); + // else if (gamepad.getRawButton(2) || gamepad.getRawButton(3)) + // elevator.set(ElevatorPositions.CARGO_MEDIUM); + // else if (gamepad.getRawButton(4)) + // elevator.set(ElevatorPositions.CARGO_HIGH); + // else + // elevator.disablePower(); + // } + + // elevator.setRollers(gamepad.getRawAxis(2) - gamepad.getRawAxis(3)); + //RobotMap.limelight.drive(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c8205be..73bdcde 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import frc.auto.missions.AutoMission; -import frc.opmode.Autonomous; import frc.opmode.Disabled; import frc.opmode.OpMode; import frc.opmode.TeleOp; @@ -37,7 +36,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - _opMode = new Autonomous(); + _opMode = new TeleOp(); logger.printStatus("Initializing autonomous."); _opMode.init(); logger.printStatus("Looping autonomous."); @@ -50,7 +49,8 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - _opMode = new TeleOp(); + if(!(_opMode instanceof TeleOp)) + _opMode = new TeleOp(); logger.printStatus("Initializing teleop."); _opMode.init(); logger.printStatus("Looping teleop."); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 67c75e7..0bdb0fd 100755 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -2,14 +2,17 @@ import frc.utils.DataPublisher; import frc.utils.Logger; - +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import frc.auto.AutoFactory; import frc.subsystem.CameraSystem; +import frc.subsystem.Climber; import frc.subsystem.Compressor; import frc.subsystem.DriveTrain; import frc.subsystem.Elevator; import frc.subsystem.Limelight; +import frc.subsystem.Pigeon; /** * Provides an interface for accessing the subsystems in a SAFE manner. @@ -19,11 +22,12 @@ public class RobotMap public static RobotConfig config; public static DriveTrain driveTrain; public static Limelight limelight; - //public static Pigeon pigeon; + public static Pigeon pigeon; public static Compressor compressor; public static CameraSystem cameraSystem; public static AutoFactory autoFactory; public static Elevator elevator; + public static Climber climber; public static Logger logger = Logger.getInstance(); @@ -36,6 +40,15 @@ public static void init() { // Publish data to NetworkTables every 0.1 seconds. Notifier notifier = new Notifier(new DataPublisher()); notifier.startPeriodic(0.1); + + CameraServer.getInstance().startAutomaticCapture(0); + CameraServer.getInstance().startAutomaticCapture(1); + + DriverStation ds = DriverStation.getInstance(); + + if(ds.isFMSAttached()) + logger.printWarning(String.format("You are playing %s match %s at %s.", ds.getMatchType(), ds.getMatchNumber(), ds.getLocation())); + logger.printWarning("Good luck and have fun! :)"); } private static void initRobotMap() { @@ -61,10 +74,11 @@ private static void initRobotMap() { RobotMap.driveTrain = DriveTrain.getInstance(); driveTrain.init(); limelight = Limelight.getInstance(); - //pigeon = Pigeon.getInstance(); + pigeon = Pigeon.getInstance(); compressor = Compressor.getInstance(); //cameraSystem = CameraSystem.getInstance(); elevator = Elevator.getInstance(); + climber = Climber.getInstance(); // autoFactory = AutoFactory.getInstance(); } diff --git a/src/main/java/frc/subsystem/CameraSystem.java b/src/main/java/frc/subsystem/CameraSystem.java index 7b69407..a55a605 100644 --- a/src/main/java/frc/subsystem/CameraSystem.java +++ b/src/main/java/frc/subsystem/CameraSystem.java @@ -17,7 +17,7 @@ public class CameraSystem { private CameraSystem() { frontCamera = CameraServer.getInstance().startAutomaticCapture(0); - rearCamera = CameraServer.getInstance().startAutomaticCapture(1); + //rearCamera = CameraServer.getInstance().startAutomaticCapture(1); server = CameraServer.getInstance().getServer(); server.setSource(frontCamera); diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java new file mode 100644 index 0000000..2147df5 --- /dev/null +++ b/src/main/java/frc/subsystem/Climber.java @@ -0,0 +1,82 @@ +package frc.subsystem; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc.enums.IDs; +import frc.robot.RobotMap; + +/** + * Climber + */ +public class Climber { + private static Climber instance = null; + + public TalonSRX climbRollers; + public TalonSRX climbRollersFollower; + public TalonSRX climbArm; + public boolean isNeutral = true; + + //public Solenoid climbPistons; + + private Climber() { + init(); + } + + public static Climber getInstance() { + if(instance == null) + instance = new Climber(); + return instance; + } + + public void init() { + climbRollers = new TalonSRX(IDs.CLIMB_ROLLERS.getValue()); + climbRollers.setInverted(InvertType.InvertMotorOutput); + + climbArm = new TalonSRX(IDs.CLIMB_ARM.getValue()); + climbArm.setSensorPhase(true); + climbArm.configClosedLoopPeakOutput(0, 1.0f); + climbArm.configClosedloopRamp(0.2); + + //climbPistons = new Solenoid(IDs.PCM.getValue(), IDs.CLIMB_PISTON.getValue()); + + climbArm.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + climbArm.config_kP(0, 0.35); + climbArm.config_kI(0, 0); + climbArm.config_kD(0, 0); + climbArm.config_kF(0, 0); + } + + public void enableArm() { + climbArm.set(ControlMode.PercentOutput, -1.0); + isNeutral = false; + //DriverStation.reportWarning("Arm enabled.", false); + } + + public void disableArm() { + climbArm.set(ControlMode.PercentOutput, 1.0); + isNeutral = true; + //DriverStation.reportWarning("Arm DISABLED.", false); + } + + public void zeroPower() { + climbArm.set(ControlMode.PercentOutput, 0.0); + } + + public void enableWheels() { + climbRollers.set(ControlMode.PercentOutput, 0.5); + } + + public void disableWheels() { + climbRollers.set(ControlMode.PercentOutput, 0); + } + + public void zeroEncoder() { + if(RobotMap.elevator.elevatorZero.get() == true) { + climbArm.setSelectedSensorPosition(0); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/subsystem/DriveTrain.java b/src/main/java/frc/subsystem/DriveTrain.java index 7386142..5360661 100755 --- a/src/main/java/frc/subsystem/DriveTrain.java +++ b/src/main/java/frc/subsystem/DriveTrain.java @@ -16,8 +16,7 @@ /** * DriveTrain */ -public class DriveTrain -{ +public class DriveTrain { private static DriveTrain instance = null; public TalonSRX left = null; @@ -27,8 +26,8 @@ public class DriveTrain public Solenoid driveShifters = null; - public Joystick joystick1 = null; - public Joystick joystick2 = null; + public Joystick leftJoystick = null; + public Joystick rightJoystick = null; public Joystick secondaryOperator = null; private DriveTrain() { @@ -51,8 +50,8 @@ private void assign() { driveShifters = new Solenoid(IDs.PCM.getValue(), IDs.DRIVE_SHIFTER.getValue()); - joystick1 = new Joystick(IDs.LEFT_JOYSTICK.getValue()); - joystick2 = new Joystick(IDs.RIGHT_JOYSTICK.getValue()); + leftJoystick = new Joystick(IDs.LEFT_JOYSTICK.getValue()); + rightJoystick = new Joystick(IDs.RIGHT_JOYSTICK.getValue()); secondaryOperator = new Joystick(IDs.SECONDARY_OPERATOR.getValue()); } @@ -86,7 +85,7 @@ private void assignKeys() { RobotMap.logger.printError(String.format("Key %s could not be found.", keys[4])); if(RobotMap.config.inputDeviceIDs.containsKey(keys[6])) - joystick1 = new Joystick(RobotMap.config.inputDeviceIDs.get(keys[6])); + leftJoystick = new Joystick(RobotMap.config.inputDeviceIDs.get(keys[6])); else RobotMap.logger.printError(String.format("Key %s could not be found.", keys[6])); } @@ -204,29 +203,25 @@ public double[] getSpeeds() { } public double getJoystickX() { - return joystick1.getX(); + return leftJoystick.getX(); } public double getJoystickY() { - return - joystick1.getY(); + return leftJoystick.getY(); } public double getJoystick2X() { - return - joystick2.getX(); + return rightJoystick.getX(); } public double getJoystick2Y() { - return - joystick2.getY(); + return rightJoystick.getY(); } public boolean isTriggerPressed() { - return - joystick1.getTriggerPressed(); + return leftJoystick.getTriggerPressed(); } public void configPeakOutput(double maxSpeed) { diff --git a/src/main/java/frc/subsystem/Elevator.java b/src/main/java/frc/subsystem/Elevator.java index dd51ddf..6da84fb 100644 --- a/src/main/java/frc/subsystem/Elevator.java +++ b/src/main/java/frc/subsystem/Elevator.java @@ -2,30 +2,35 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Solenoid; import frc.enums.ElevatorPositions; import frc.enums.IDs; import frc.enums.PIDFeedback; +import frc.robot.RobotMap; /** * Handles the elevator and its relevant systems. */ public class Elevator extends ISubsystem { - private TalonSRX innerStage; - private TalonSRX outerStage; + public TalonSRX innerStage; + public TalonSRX outerStage; private TalonSRX rollers; + //public TalonSRX vacuumPump; - private AnalogInput ultrasonic; - private DigitalInput elevatorZero; + public DigitalInput elevatorZero; private Solenoid elevator; private Solenoid vacuumEnable; private Solenoid vacuumRelease; + //public Solenoid climbing; + //private boolean hasZeroed = false; + + public DigitalInput IRBeam; private static Elevator instance = null; private Elevator() { @@ -44,16 +49,19 @@ public static Elevator getInstance() { */ @Override protected void assign() { - innerStage = new TalonSRX(IDs.ELEVATOR_INNER.getValue()); - outerStage = new TalonSRX(IDs.ELEVATOR_OUTER.getValue()); - rollers = new TalonSRX(IDs.ROLLERS.getValue()); + innerStage = new TalonSRX(IDs.ELEVATOR_INNER.getValue()); + outerStage = new TalonSRX(IDs.ELEVATOR_OUTER.getValue()); + rollers = new TalonSRX(IDs.ROLLERS.getValue()); + //vacuumPump = new TalonSRX(13); - ultrasonic = new AnalogInput(IDs.ELEVATOR_ULTRASONIC.getValue()); elevatorZero = new DigitalInput(IDs.ELEVATOR_DIGITAL.getValue()); elevator = new Solenoid(IDs.PCM.getValue(), IDs.ELEVATOR_PISTONS.getValue()); vacuumEnable = new Solenoid(IDs.PCM.getValue(), IDs.VACUUM_ENABLE.getValue()); vacuumRelease = new Solenoid(IDs.PCM.getValue(), IDs.VACUUM_DISABLE.getValue()); + //climbing = new Solenoid(IDs.PCM.getValue(), 3); + + IRBeam = new DigitalInput(IDs.ELEVATOR_LIGHT.getValue()); } /** @@ -63,7 +71,11 @@ private void init() { //innerStage.setInverted(InvertType.InvertMotorOutput); innerStage.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); outerStage.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - innerStage.setSensorPhase(true); + innerStage.setSensorPhase(false); + outerStage.setSensorPhase(true); + + outerStage.configClosedLoopPeakOutput(0, 1); + outerStage.configClosedloopRamp(0.2); configPIDF(innerStage, PIDFeedback.INNER_ELEVATOR); configPIDF(outerStage, PIDFeedback.OUTER_ELEVATOR); @@ -75,6 +87,7 @@ private void init() { * @param talon Talon instance to zero. */ @Deprecated + @SuppressWarnings("unused") private void zeroEncoder(TalonSRX talon) { int absolutePosition = talon.getSensorCollection().getPulseWidthPosition(); /* mask out overflows, keep bottom 12 bits */ @@ -88,12 +101,17 @@ private void zeroEncoder(TalonSRX talon) { * Once the limit switch is pressed, it will set the encoders of the elevator to zero. */ public void zeroEncoder() { - innerStage.set(ControlMode.PercentOutput, -0.15); - while(!elevatorZero.get()) ; // Wait until the elevator's sensor is pressed + //innerStage.set(ControlMode.PercentOutput, 0.15); + //while(!elevatorZero.get()) ; // Wait until the elevator's sensor is pressed // Set the encoders to zero. - innerStage.setSelectedSensorPosition(0); - outerStage.setSelectedSensorPosition(0); + if(elevatorZero.get() == true) { + innerStage.setSelectedSensorPosition(0); + outerStage.setSelectedSensorPosition(0); + RobotMap.elevator.innerStage.setNeutralMode(NeutralMode.Brake); + } + + //innerStage.set(ControlMode.PercentOutput, 0); } /** @@ -119,32 +137,38 @@ public void set(ElevatorPositions pos) { innerStage.set(ControlMode.Position, pos.getInner()); outerStage.set(ControlMode.Position, pos.getOuter()); - while(!onTarget()); - - switch(pos) { - case CARGO_LOW: - case CARGO_MEDIUM: - case CARGO_HIGH: - // Spit out the ball. - setRollers(-1); - super.sleep(1000); - break; - case HATCH_LOW: - case HATCH_MEDIUM: - case HATCH_HIGH: - // Disable the vacuum. - releaseVacuum(true); - super.sleep(100); - releaseVacuum(false); - case NEUTRAL: - // Do nothing. We are already in neutral. - break; - } - - innerStage.set(ControlMode.Position, ElevatorPositions.NEUTRAL.getInner()); - outerStage.set(ControlMode.Position, ElevatorPositions.NEUTRAL.getOuter()); + // while(!onTarget()); + + // switch(pos) { + // case CARGO_LOW: + // case CARGO_MEDIUM: + // case CARGO_HIGH: + // // Spit out the ball. + // setRollers(-1); + // super.sleep(1000); + // break; + // case HATCH_LOW: + // case HATCH_MEDIUM: + // case HATCH_HIGH: + // // Disable the vacuum. + // releaseVacuum(true); + // super.sleep(100); + // releaseVacuum(false); + // case NEUTRAL: + // // Do nothing. We are already in neutral. + // break; + // } + + // innerStage.set(ControlMode.Position, ElevatorPositions.NEUTRAL.getInner()); + // outerStage.set(ControlMode.Position, ElevatorPositions.NEUTRAL.getOuter()); + // while(!onTarget()); + // innerStage.set(ControlMode.PercentOutput, 0); + // outerStage.set(ControlMode.PercentOutput, 0); + } - while(!onTarget()); + public void disablePower() { + innerStage.set(ControlMode.PercentOutput, 0); + outerStage.set(ControlMode.PercentOutput, 0); } /** @@ -179,16 +203,6 @@ public boolean onTarget() { return (innerElevatorTarget && outerElevatorTarget); } - /** - * Get the raw value from the ultrasonic sensor. - * Will be used to determine whether a cargo ball or hatch panel is in place. - * If the sensor returns zero, then there is no cargo ball. - * @return A raw, unscaled output from the ultrasonic sensor. - */ - public double getRawUltrasonic() { - return ultrasonic.getVoltage(); - } - /** * Set the elevators to the forward position. */ @@ -262,4 +276,9 @@ public void releaseVacuum(boolean value) { vacuumRelease.set(value); } + public boolean getIRBeam() + { + return IRBeam.get(); + } + } \ No newline at end of file diff --git a/src/main/java/frc/subsystem/Limelight.java b/src/main/java/frc/subsystem/Limelight.java index 111e781..2a0ecc8 100644 --- a/src/main/java/frc/subsystem/Limelight.java +++ b/src/main/java/frc/subsystem/Limelight.java @@ -41,8 +41,8 @@ private Limelight() { limelightConfig.getEntry(sPeriod).setDouble(0.013); // x: stopping point, y: output speed - double[] point1 = new double[] { 1.4, 0 }; - double[] point2 = new double[] { 0.3, 1 }; + double[] point1 = new double[] { 14, 0 }; + double[] point2 = new double[] { 1.2, 1 }; // Calculate slope. m = (point2[1] - point1[1])/(point2[0] - point1[0]); @@ -54,7 +54,6 @@ private Limelight() { public static Limelight getInstance() { if(instance == null) instance = new Limelight(); - return instance; } @@ -80,8 +79,8 @@ public double[] getValue() { double leftOutput = result * aLeft; double rightOutput = result * aRight; - RobotMap.logger.printStatus(String.format("Limelight was calculated. Left: %s; Right: %s", - new DecimalFormat("#.####").format(leftOutput), new DecimalFormat("#.####").format(rightOutput))); + // RobotMap.logger.printStatus(String.format("Limelight was calculated. Left: %s; Right: %s", + // new DecimalFormat("#.####").format(leftOutput), new DecimalFormat("#.####").format(rightOutput))); return new double[] { leftOutput, rightOutput }; } @@ -108,7 +107,7 @@ public void drive() { public void setLights(boolean state) { double value; value = state ? 0.0 : 1.0; - limelightConfig.getEntry("ledMode").setDouble(value); + limelight.getEntry("ledMode").setDouble(value); // Do nothing now. It doesn't work. } /** @@ -117,7 +116,21 @@ public void setLights(boolean state) { */ public boolean getLight() { boolean value; - value = limelightConfig.getEntry("ledMode").getDouble(0) == 0 ? true : false; + value = limelight.getEntry("ledMode").getDouble(0) == 0 ? true : false; return value; } + + /** + * Sets whether the robot should be in drive mode or targetting mode. + */ + public void setMode(boolean isDrive) { + if(isDrive) + limelight.getEntry("pipeline").setDouble(1); + else + limelight.getEntry("pipeline").setDouble(0); + } + + public void setPipeline(int pipeline) { + limelight.getEntry("pipeline").setNumber(pipeline); + } } \ No newline at end of file diff --git a/src/main/java/frc/subsystem/Pigeon.java b/src/main/java/frc/subsystem/Pigeon.java index ed15ee0..2efb2d9 100644 --- a/src/main/java/frc/subsystem/Pigeon.java +++ b/src/main/java/frc/subsystem/Pigeon.java @@ -2,6 +2,7 @@ import com.ctre.phoenix.sensors.PigeonIMU; +import frc.enums.IDs; import frc.robot.RobotMap; /** @@ -11,8 +12,10 @@ public class Pigeon { private static Pigeon instance = null; PigeonIMU pigeonIMU = null; + double[] ypr; + private Pigeon() { - pigeonIMU = new PigeonIMU(RobotMap.driveTrain.leftFollower); + pigeonIMU = new PigeonIMU(IDs.PIGEON.getValue()); pigeonIMU.setYaw(0); } @@ -25,8 +28,18 @@ public static Pigeon getInstance() { } public double getHeading() { - double[] ypr = new double[3]; + ypr = new double[3]; pigeonIMU.getYawPitchRoll(ypr); return ypr[0]; } + + public double getPitch() { + pigeonIMU.getYawPitchRoll(ypr); + return ypr[1]; + } + + public double getRoll() { + pigeonIMU.getYawPitchRoll(ypr); + return ypr[2]; + } } \ No newline at end of file diff --git a/src/main/java/frc/utils/DataPublisher.java b/src/main/java/frc/utils/DataPublisher.java index 025c99a..dab25d1 100644 --- a/src/main/java/frc/utils/DataPublisher.java +++ b/src/main/java/frc/utils/DataPublisher.java @@ -10,21 +10,15 @@ public class DataPublisher implements java.lang.Runnable { NetworkTable instance = NetworkTableInstance.getDefault().getTable("datapublisher"); public void run() { - instance.getEntry("Left Speed").setDouble(RobotMap.driveTrain.left.getSelectedSensorVelocity(0)); - instance.getEntry("Right Speed").setDouble(RobotMap.driveTrain.right.getSelectedSensorVelocity(0)); - - instance.getEntry("Left Position").setDouble(RobotMap.driveTrain.left.getSelectedSensorPosition(0)); - instance.getEntry("Right Position").setDouble(RobotMap.driveTrain.right.getSelectedSensorPosition(0)); - - instance.getEntry("Left Error").setDouble(RobotMap.driveTrain.left.getClosedLoopError(0)); - instance.getEntry("Right Error").setDouble(RobotMap.driveTrain.right.getClosedLoopError(0)); //instance.getEntry("Heading").setDouble(RobotMap.pigeon.getHeading()); instance.getEntry("Shifters").setString(RobotMap.driveTrain.getShifterState().name()); - instance.getEntry("Elevator Ultrasonic").setDouble(RobotMap.elevator.getRawUltrasonic()); + instance.getEntry("Elevator IR Beam").setBoolean(RobotMap.elevator.getIRBeam()); instance.getEntry("Elevator Inner Position").setDouble(RobotMap.elevator.getInnerEncoder()); instance.getEntry("Elevator Outer Position").setDouble(RobotMap.elevator.getOuterEncoder()); + instance.getEntry("Climb Arm Position").setDouble(RobotMap.climber.climbArm.getSelectedSensorPosition()); + instance.getEntry("Elevator Button").setBoolean(RobotMap.elevator.elevatorZero.get()); } } \ No newline at end of file