diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index f032efa..f3b4c5b 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -15,6 +15,8 @@ import frc.robot.commands.EjectCargo; import frc.robot.commands.IntakeCargo; import frc.robot.commands.SlowDrive; +import frc.robot.commands.ToggleHatch; +import frc.robot.subsystems.HatchPanel; import frc.robot.commands.ToggleCamera; import frc.robot.subsystems.Cargo; @@ -23,34 +25,30 @@ * interface to the commands and command groups that allow control of the robot. */ public class OI { - Joystick leftJoy; - Joystick rightJoy; - Joystick manipulator; + Joystick leftJoy, rightJoy, manipulator; - JoystickButton leftSlowBtn; - JoystickButton rightSlowBtn; - - JoystickButton cargoIntakeBtn; - JoystickButton cargoEjectBtn; + JoystickButton leftSlowBtn, rightSlowBtn; + JoystickButton toggleHatchBtn; + JoystickButton cargoIntakeBtn, cargoEjectBtn; JoystickButton toggleCameraBtn; - Cargo cargo; - - OI(Cargo cargo, UsbCamera driveCamera, UsbCamera hatchCamera, VideoSink cameraServer) { - this.cargo = cargo; - - leftJoy = new Joystick(0); // TODO: set ports to correct values - rightJoy = new Joystick(1); // TODO: set ports to correct values + OI(Cargo cargo, HatchPanel hp, UsbCamera driveCamera, UsbCamera hatchCamera, VideoSink cameraServer) { + leftJoy = new Joystick(0); + rightJoy = new Joystick(1); + manipulator = new Joystick(2); leftSlowBtn = new JoystickButton(leftJoy, 1); leftSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.LEFT)); rightSlowBtn = new JoystickButton(rightJoy, 1); rightSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT)); - cargoIntakeBtn = new JoystickButton(manipulator, 0); + toggleHatchBtn = new JoystickButton(manipulator, 0); // TODO: set ports to correct values + toggleHatchBtn.whenPressed(new ToggleHatch(hp)); + + cargoIntakeBtn = new JoystickButton(manipulator, 1); // TODO: set ports to correct values cargoIntakeBtn.whenPressed(new IntakeCargo(cargo)); - cargoEjectBtn = new JoystickButton(manipulator, 1); + cargoEjectBtn = new JoystickButton(manipulator, 2); // TODO: set ports to correct values cargoEjectBtn.whenPressed(new EjectCargo(cargo)); toggleCameraBtn = new JoystickButton(leftJoy, 2); diff --git a/Robot2019/src/main/java/frc/robot/Robot.java b/Robot2019/src/main/java/frc/robot/Robot.java index 762018c..5250f18 100644 --- a/Robot2019/src/main/java/frc/robot/Robot.java +++ b/Robot2019/src/main/java/frc/robot/Robot.java @@ -16,9 +16,11 @@ import frc.robot.commands.TeleopDrive; import frc.robot.subsystems.Cargo; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.HatchPanel; public class Robot extends TimedRobot { private static Drivetrain dt; + private static HatchPanel hp; private static OI oi; private static Cargo cargo; @@ -30,8 +32,10 @@ public void robotInit() { dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster, RobotMap.rightSlave1, RobotMap.rightSlave2, oi.leftJoy, oi.rightJoy, RobotMap.leftEnc, RobotMap.rightEnc, RobotMap.gyro); + hp = new HatchPanel(RobotMap.hatchPistons); cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp); - oi = new OI(cargo, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); + + oi = new OI(cargo, hp, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); chooser.setDefaultOption("Default Auto", new TeleopDrive(dt)); SmartDashboard.putData("Auto Mode", chooser); diff --git a/Robot2019/src/main/java/frc/robot/RobotMap.java b/Robot2019/src/main/java/frc/robot/RobotMap.java index bd1e1cc..054ee02 100644 --- a/Robot2019/src/main/java/frc/robot/RobotMap.java +++ b/Robot2019/src/main/java/frc/robot/RobotMap.java @@ -17,6 +17,7 @@ import edu.wpi.cscore.VideoSource; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.SPI; @@ -31,6 +32,7 @@ public class RobotMap { static WPI_TalonSRX leftMaster, rightMaster; static WPI_VictorSPX leftSlave1, leftSlave2, rightSlave1, rightSlave2; + static DoubleSolenoid hatchPistons; static VictorSP cargoRoller; static Encoder leftEnc, rightEnc; static String driveMode; @@ -53,6 +55,9 @@ public class RobotMap { // Initialize motors on the cargo mech cargoRoller = new VictorSP(0); + // Initialize solenoid on hatch panel mech + hatchPistons = new DoubleSolenoid(0, 1); + leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1)); rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3)); diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleHatch.java b/Robot2019/src/main/java/frc/robot/commands/ToggleHatch.java new file mode 100644 index 0000000..5d626c9 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleHatch.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.subsystems.HatchPanel; + +public class ToggleHatch extends InstantCommand { + private HatchPanel hp; + + public ToggleHatch(HatchPanel hp) { + super(); + requires(hp); + + this.hp = hp; + } + + // Called once when the command executes + @Override + protected void initialize() { + hp.toggle(); + } + +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java new file mode 100644 index 0000000..6333111 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; + + +public class HatchPanel extends Subsystem { + private DoubleSolenoid pistons; + + /** + * Subsystem for controlling the hatch panel mechanism + * + * @param pistons the solenoid that controls all three pistons on the mechanism + */ + public HatchPanel(DoubleSolenoid pistons) { + this.pistons = pistons; + } + + /** + * toggles the hatch panel pistons + * + * @return if the pistons are extended after the call + */ + public boolean toggle() { + if (pistons.get() == DoubleSolenoid.Value.kForward) { + pistons.set(DoubleSolenoid.Value.kReverse); + return false; + } else { + pistons.set(DoubleSolenoid.Value.kForward); + return true; + } + } + + @Override + public void initDefaultCommand() {} +}