From b1fcdfc5349d711c239848ec0c6127ce49885885 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Tue, 25 Feb 2020 20:56:23 -0800 Subject: [PATCH 01/11] yay we shoot shoot --- src/main/java/frc/team2412/robot/Logging.java | 7 +- src/main/java/frc/team2412/robot/OI.java | 73 ++-- src/main/java/frc/team2412/robot/Robot.java | 5 +- .../frc/team2412/robot/RobotContainer.java | 26 +- .../java/frc/team2412/robot/RobotMap.java | 4 +- .../intake/IntakeBothInCommandGroup.java | 2 +- .../intake/IntakeBothOffCommandGroup.java | 2 +- .../intake/IntakeBothOutCommandGroup.java | 2 +- .../commands/intake/IntakeBothUpCommand.java | 4 +- .../intake/back/IntakeBackInCommand.java | 8 +- .../intake/back/IntakeBackOffCommand.java | 8 +- .../intake/back/IntakeBackOutCommand.java | 8 +- .../intake/front/IntakeFrontInCommand.java | 8 +- .../intake/front/IntakeFrontOffCommand.java | 8 +- .../intake/front/IntakeFrontOutCommand.java | 8 +- .../robot/subsystems/DriveBaseSubsystem.java | 8 - .../robot/subsystems/HoodSubsystem.java | 2 +- .../subsystems/IntakeOnOffSubsystem.java | 2 +- .../subsystems/IntakeUpDownSubsystem.java | 4 +- .../robot/subsystems/LiftSubsystem.java | 2 +- .../robot/subsystems/LimelightSubsystem.java | 4 +- vendordeps/Phoenix.json | 358 +++++++++--------- vendordeps/REVColorSensorV3.json | 72 ++-- 23 files changed, 338 insertions(+), 287 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Logging.java b/src/main/java/frc/team2412/robot/Logging.java index f7268879..db3efaf9 100644 --- a/src/main/java/frc/team2412/robot/Logging.java +++ b/src/main/java/frc/team2412/robot/Logging.java @@ -6,6 +6,7 @@ public class Logging implements Loggable { + @Log.Exclude Robot m_robot; // 4 rows, 10 col @@ -16,14 +17,14 @@ public class Logging implements Loggable { // Goal Able @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 0, rowIndex = 0, height = 2, tabName = "Driver View") - @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 2, rowIndex = 0, height = 2, tabName = "Driver View") + @Log.BooleanBox(name = "asdf", colorWhenFalse = white, colorWhenTrue = green, columnIndex = 2, rowIndex = 0, height = 2, tabName = "Driver View") public boolean outerGoalAble = false; @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 0, rowIndex = 0, height = 2, tabName = "Driver View") public boolean innerGoalAble = false; // Goal Aimed @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 0, rowIndex = 0, height = 2, tabName = "Driver View") - @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 2, rowIndex = 0, height = 2, tabName = "Driver View") + @Log.BooleanBox(name = "asdf2", colorWhenFalse = white, colorWhenTrue = green, columnIndex = 2, rowIndex = 0, height = 2, tabName = "Driver View") public boolean outerGoalAimed = false; @Log.BooleanBox(colorWhenFalse = white, colorWhenTrue = green, columnIndex = 0, rowIndex = 0, height = 2, tabName = "Driver View") public boolean innerGoalAimed = false; @@ -48,7 +49,7 @@ public class Logging implements Loggable { // Dial Boolean @Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 7, rowIndex = 1, tabName = "Driver View") - @Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 9, rowIndex = 1, tabName = "Driver View") + //@Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 9, rowIndex = 1, tabName = "Driver View") public boolean brownoutWarning = false; // Timer diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index 8f0434ec..4ea482e7 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team2412.robot.commands.drive.DriveCommand; +import frc.team2412.robot.commands.drive.DriveShiftToHighGearCommand; +import frc.team2412.robot.commands.drive.DriveShiftToLowGearCommand; import frc.team2412.robot.commands.indexer.IndexShootCommand; import frc.team2412.robot.commands.indexer.IndexSpitCommand; import frc.team2412.robot.commands.intake.IntakeBothInCommandGroup; @@ -17,6 +19,8 @@ import frc.team2412.robot.commands.intake.back.IntakeBackUpCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontDownCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontUpCommand; +import frc.team2412.robot.commands.lift.LiftDownCommand; +import frc.team2412.robot.commands.lift.LiftUpCommand; //This is the class in charge of all the buttons and joysticks that the drivers will use to control the robot public class OI { @@ -36,7 +40,8 @@ public enum DriverControls { ALIGN_STICKS(Joysticks.DRIVER_LEFT, 3), // // temp testing controls FRONT_INTAKE_DOWN(Joysticks.DRIVER_RIGHT, 6), BACK_INTAKE_DOWN(Joysticks.DRIVER_RIGHT, 5), - INTAKE_IN(Joysticks.DRIVER_RIGHT, 3), INTAKE_OUT(Joysticks.DRIVER_RIGHT, 4); + INTAKE_IN(Joysticks.DRIVER_RIGHT, 3), INTAKE_OUT(Joysticks.DRIVER_RIGHT, 4), LIFT_UP(Joysticks.DRIVER_LEFT, 6), + LIFT_DOWN(Joysticks.DRIVER_LEFT, 7); public Joysticks stick; public int buttonID; @@ -76,6 +81,8 @@ public Button createFrom(Joystick stick) { Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); + public Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); + // Buttons public Button indexerShootButton = DriverControls.SHOOT.createFrom(driverRightStick); public Button indexerSpitButton = DriverControls.SPIT.createFrom(driverLeftStick); @@ -85,20 +92,27 @@ public Button createFrom(Joystick stick) { public Button intakeInButton = DriverControls.INTAKE_IN.createFrom(driverRightStick); public Button intakeOutButton = DriverControls.INTAKE_OUT.createFrom(driverRightStick); + public Button liftUpButton = DriverControls.LIFT_UP.createFrom(driverLeftStick); + public Button liftDownButton = DriverControls.LIFT_DOWN.createFrom(driverLeftStick); + // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { if (RobotMap.INTAKE_CONNECTED) { // INTAKE front -// intakeFrontOnButton.whenPressed(new IntakeFrontBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, -// robotContainer.m_intakeMotorOnOffSubsystem)); -// intakeFrontOffButton.whenPressed(new IntakeFrontBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, -// robotContainer.m_intakeMotorOnOffSubsystem)); -// + // intakeFrontOnButton.whenPressed(new + // IntakeFrontBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, + // robotContainer.m_intakeMotorOnOffSubsystem)); + // intakeFrontOffButton.whenPressed(new + // IntakeFrontBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, + // robotContainer.m_intakeMotorOnOffSubsystem)); + // // INTAKE back -// intakeBackOnButton.whenPressed(new IntakeBackBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, -// robotContainer.m_intakeMotorOnOffSubsystem)); -// intakeBackOffButton.whenPressed(new IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, -// robotContainer.m_intakeMotorOnOffSubsystem)); + // intakeBackOnButton.whenPressed(new + // IntakeBackBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, + // robotContainer.m_intakeMotorOnOffSubsystem)); + // intakeBackOffButton.whenPressed(new + // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, + // robotContainer.m_intakeMotorOnOffSubsystem)); intakeFrontDownButton.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); intakeFrontDownButton.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); @@ -115,18 +129,25 @@ public OI(RobotContainer robotContainer) { if (RobotMap.CONTROL_PANEL_CONNECTED) { // CONTROL PANEL -// controlPanelSpinThreeTimesButton -// .whenPressed(new RotateControlPanelCommand(robotContainer.m_controlPanelColorSubsystem)); -// controlPanelSetToTargetButton -// .whenPressed(new SetToTargetColorCommand(robotContainer.m_controlPanelColorSubsystem)); + // controlPanelSpinThreeTimesButton + // .whenPressed(new + // RotateControlPanelCommand(robotContainer.m_controlPanelColorSubsystem)); + // controlPanelSetToTargetButton + // .whenPressed(new + // SetToTargetColorCommand(robotContainer.m_controlPanelColorSubsystem)); } if (RobotMap.CLIMB_CONNECTED) { -// climbDeployRailsButton.whenActive(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); -// climbExtendArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); -// climbRetractArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); -// climbRetractRailsButton.whenActive(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); -// climbStopArmButton.whenActive(new ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem)); + // climbDeployRailsButton.whenActive(new + // ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); + // climbExtendArmButton.whenActive(new + // ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); + // climbRetractArmButton.whenActive(new + // ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); + // climbRetractRailsButton.whenActive(new + // ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); + // climbStopArmButton.whenActive(new + // ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem)); } if (RobotMap.INDEX_CONNECTED) { @@ -144,20 +165,22 @@ public OI(RobotContainer robotContainer) { if (RobotMap.DRIVE_BASE_CONNECTED) { robotContainer.m_driveBaseSubsystem.setDefaultCommand(new DriveCommand(robotContainer.m_driveBaseSubsystem, driverRightStick, driverLeftStick, DriverControls.ALIGN_STICKS.createFrom(driverLeftStick))); + + shifter.whenPressed(new DriveShiftToHighGearCommand(robotContainer.m_driveBaseSubsystem)); + shifter.whenReleased(new DriveShiftToLowGearCommand(robotContainer.m_driveBaseSubsystem)); } // LIFT if (RobotMap.LIFT_CONNECTED) { -// liftUpButton.whenPressed( -// new LiftUpCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); -// liftDownButton.whenPressed( -// new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); + liftUpButton.whenPressed( + new LiftUpCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); + liftDownButton.whenPressed( + new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); } Trigger intakeUpWhenFiveBalls = new Trigger(RobotState::hasFiveBalls); if (RobotMap.INTAKE_CONNECTED) { - intakeUpWhenFiveBalls.whenActive(new IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem, - robotContainer.m_intakeMotorOnOffSubsystem)); + //intakeUpWhenFiveBalls.whenActive(new IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem)); } } diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index eba45076..22734ebe 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -26,7 +26,6 @@ public class Robot extends TimedRobot implements Loggable { public double timeRemaining; // Have instances of robot container and OI for easy access - @SuppressWarnings("unused") private RobotContainer m_robotContainer = RobotMap.m_robotContainer; @SuppressWarnings("unused") private OI m_OI = RobotMap.m_OI; @@ -39,8 +38,8 @@ public class Robot extends TimedRobot implements Loggable { */ @Override public void robotInit() { - Logger.configureLoggingAndConfig(this, false); - Shuffleboard.startRecording(); + Logger.configureLoggingAndConfig(m_robotContainer, false); + // Shuffleboard.startRecording(); DriverStation driverStation = DriverStation.getInstance(); diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java index 546e54d4..71c47bc4 100644 --- a/src/main/java/frc/team2412/robot/RobotContainer.java +++ b/src/main/java/frc/team2412/robot/RobotContainer.java @@ -20,43 +20,43 @@ // this is the class for containing all the subsystems and OI of the robot public class RobotContainer implements Loggable { - @Log(tabName = "Turret") + // @Log( name = "Limelight Subsystem", tabName = "Turret") public LimelightSubsystem m_LimelightSubsystem; - @Log(tabName = "Turret Subsystem", name = "Turret") + // @Log(name = "Turret Subsystem", tabName = "Turret") public TurretSubsystem m_turretSubsystem; - @Log(tabName = "Flywheel Subsystem", name = "Turret") + // @Log(name = "Flywheel Subsystem", tabName = "Turret") public FlywheelSubsystem m_flywheelSubsystem; - @Log(tabName = "Hood Subsystem", name = "Turret") + // @Log(name = "Hood Subsystem", tabName = "Turret") public HoodSubsystem m_hoodSubsystem; - @Log(tabName = "Lift Subsystem", name = "Lift") + // @Log(tabName = "Lift Subsystem", name = "Lift") public LiftSubsystem m_liftSubsystem; - @Log(tabName = "Drivebase Subsystem", name = "Drivebase Subsystem") + // @Log(tabName = "Drivebase Subsystem", name = "Drivebase Subsystem") public DriveBaseSubsystem m_driveBaseSubsystem; - @Log(tabName = "Intake Motors Subsystem", name = "Intake") + // @Log(tabName = "Intake Motors Subsystem", name = "Intake") public IntakeOnOffSubsystem m_intakeMotorOnOffSubsystem; - @Log(tabName = "Intake Lift Subsystem", name = "Intake") + // @Log(tabName = "Intake Lift Subsystem", name = "Intake") public IntakeUpDownSubsystem m_intakeUpDownSubsystem; - @Log(tabName = "Control Panel Subsystem", name = "Control Panel") + // @Log(tabName = "Control Panel Subsystem", name = "Control Panel") public ControlPanelColorSubsystem m_controlPanelColorSubsystem; - @Log(tabName = "Indexer Motor Subsystem", name = "Indexer") + // @Log(tabName = "Indexer Motor Subsystem", name = "Indexer") public IndexerMotorSubsystem m_indexerMotorSubsystem; - @Log(tabName = "Indexer Sensor Subsystem", name = "Indexer") + // @Log(tabName = "Indexer Sensor Subsystem", name = "Indexer") public IndexerSensorSubsystem m_indexerSensorSubsystem; - @Log(tabName = "Climb Lift Subsystem", name = "Climb") + // @Log(tabName = "Climb Lift Subsystem", name = "Climb") public ClimbLiftSubsystem m_climbLiftSubsystem; - @Log(tabName = "Climb Motor Subsystem", name = "Climb") + // @Log(tabName = "Climb Motor Subsystem", name = "Climb") public ClimbMotorSubsystem m_climbMotorSubsystem; // A chooser for autonomous commands diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index ff480383..a24ba8e4 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -29,11 +29,11 @@ public class RobotMap { public static boolean CLIMB_CONNECTED = false; public static boolean CONTROL_PANEL_CONNECTED = false; - public static boolean SHOOTER_CONNECTED = false; + public static boolean SHOOTER_CONNECTED = true; public static boolean INDEX_CONNECTED = true; public static boolean INTAKE_CONNECTED = true; public static boolean LIFT_CONNECTED = true; - public static boolean DRIVE_BASE_CONNECTED = true; + public static boolean DRIVE_BASE_CONNECTED = false; public static enum CANBus { INTAKE1(11), INDEX1(12), INTAKE2(21), INDEX2(22), INTAKE3(31), INDEX3(32), INDEX_MID(40), DRIVE_LEFT_FRONT(1), diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java index a07653f1..5e2d2403 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java @@ -7,7 +7,7 @@ public class IntakeBothInCommandGroup extends ParallelCommandGroup { public IntakeBothInCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontInCommand(intakeOnOffSubsystem), new IntakeBackInCommand(intakeOnOffSubsystem)); + addCommands(new IntakeFrontInCommand(intakeOnOffSubsystem, false), new IntakeBackInCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java index 04c3c0ad..e6827243 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java @@ -8,7 +8,7 @@ public class IntakeBothOffCommandGroup extends ParallelCommandGroup { public IntakeBothOffCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontOffCommand(intakeOnOffSubsystem), new IntakeBackOffCommand(intakeOnOffSubsystem)); + addCommands(new IntakeFrontOffCommand(intakeOnOffSubsystem, false), new IntakeBackOffCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java index 4a83a27d..3915e95a 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java @@ -7,7 +7,7 @@ public class IntakeBothOutCommandGroup extends ParallelCommandGroup { public IntakeBothOutCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontOutCommand(intakeOnOffSubsystem), new IntakeBackOutCommand(intakeOnOffSubsystem)); + addCommands(new IntakeFrontOutCommand(intakeOnOffSubsystem, false), new IntakeBackOutCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java index 74399e8c..c9afd25f 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java @@ -8,8 +8,8 @@ public class IntakeBothUpCommand extends ParallelCommandGroup { - public IntakeBothUpCommand(IntakeUpDownSubsystem intakeUpDownSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { - addRequirements(intakeUpDownSubsystem, intakeOnOffSubsystem); + public IntakeBothUpCommand(IntakeUpDownSubsystem intakeUpDownSubsystem) { + addRequirements(intakeUpDownSubsystem); addCommands(new IntakeFrontUpCommand(intakeUpDownSubsystem), new IntakeBackUpCommand(intakeUpDownSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackInCommand.java index 7d2e1139..d7f76954 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackInCommand.java @@ -8,7 +8,13 @@ public class IntakeBackInCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeOnOffSubsystem; public IntakeBackInCommand(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addRequirements(intakeOnOffSubsystem); + this(intakeOnOffSubsystem, true); + } + + public IntakeBackInCommand(IntakeOnOffSubsystem intakeOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeOnOffSubsystem); + } this.m_intakeOnOffSubsystem = intakeOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOffCommand.java b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOffCommand.java index c6408ad3..9321388d 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOffCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOffCommand.java @@ -8,7 +8,13 @@ public class IntakeBackOffCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeOnOffSubsystem; public IntakeBackOffCommand(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addRequirements(intakeOnOffSubsystem); + this(intakeOnOffSubsystem, true); + } + + public IntakeBackOffCommand(IntakeOnOffSubsystem intakeOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeOnOffSubsystem); + } this.m_intakeOnOffSubsystem = intakeOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOutCommand.java b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOutCommand.java index f21eb8ca..0de40810 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOutCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/back/IntakeBackOutCommand.java @@ -8,7 +8,13 @@ public class IntakeBackOutCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeOnOffSubsystem; public IntakeBackOutCommand(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addRequirements(intakeOnOffSubsystem); + this(intakeOnOffSubsystem, true); + } + + public IntakeBackOutCommand(IntakeOnOffSubsystem intakeOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeOnOffSubsystem); + } this.m_intakeOnOffSubsystem = intakeOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontInCommand.java index 50b198b8..01d705d9 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontInCommand.java @@ -8,7 +8,13 @@ public class IntakeFrontInCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeMotorOnOffSubsystem; public IntakeFrontInCommand(IntakeOnOffSubsystem intakeMotorOnOffSubsystem) { - addRequirements(intakeMotorOnOffSubsystem); + this(intakeMotorOnOffSubsystem, true); + } + + public IntakeFrontInCommand(IntakeOnOffSubsystem intakeMotorOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeMotorOnOffSubsystem); + } this.m_intakeMotorOnOffSubsystem = intakeMotorOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOffCommand.java b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOffCommand.java index 92dae4f9..32fd0753 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOffCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOffCommand.java @@ -8,7 +8,13 @@ public class IntakeFrontOffCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeOnOffSubsystem; public IntakeFrontOffCommand(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addRequirements(intakeOnOffSubsystem); + this(intakeOnOffSubsystem, true); + } + + public IntakeFrontOffCommand(IntakeOnOffSubsystem intakeOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeOnOffSubsystem); + } this.m_intakeOnOffSubsystem = intakeOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOutCommand.java b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOutCommand.java index b66101cc..0a884f6c 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOutCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/front/IntakeFrontOutCommand.java @@ -8,7 +8,13 @@ public class IntakeFrontOutCommand extends CommandBase { private IntakeOnOffSubsystem m_intakeMotorOnOffSubsystem; public IntakeFrontOutCommand(IntakeOnOffSubsystem intakeMotorOnOffSubsystem) { - addRequirements(intakeMotorOnOffSubsystem); + this(intakeMotorOnOffSubsystem, true); + } + + public IntakeFrontOutCommand(IntakeOnOffSubsystem intakeMotorOnOffSubsystem, boolean require) { + if (require) { + addRequirements(intakeMotorOnOffSubsystem); + } this.m_intakeMotorOnOffSubsystem = intakeMotorOnOffSubsystem; } diff --git a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java index 6087f97b..847271fd 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java @@ -161,14 +161,6 @@ public double getCurrentDraw() { return m_driveBaseCurrentDraw; } - // OBLOG - // --------------------------------------------------------------------------------- - - @Override - public String configureLogName() { - return "Drivebase Subsystem"; - } - // Periodic // ------------------------------------------------------------------------------ diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index aadb57f8..eaf0c4e7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -48,7 +48,7 @@ public void servoWithdraw() { } - @Config.NumberSlider(max = 0.65, min = 0, name = "Set Servo Angle", tabName = "Hood Subsystem", width = 2, height = 1, columnIndex = 2, rowIndex = 0) + @Config.NumberSlider(max = 0.65, min = 0, name = "Set Servo Angle", tabName = "Hood", width = 2, height = 1, columnIndex = 2, rowIndex = 0) public void setServo(double angle) { System.out.println(angle); m_hoodServo2.set(angle); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java index fa3399c8..299139e4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java @@ -70,7 +70,7 @@ public void frontIntakeIn() { } public void frontIntakeOut() { - m_intakeFrontMotor.set(IntakeConstants.MAX_INTAKE_SPEED); + m_intakeFrontMotor.set(-IntakeConstants.MAX_INTAKE_SPEED); m_lastMotor = IntakeLastMotor.FRONT; RobotState.m_intakeDirection = IntakeDirection.FRONT; } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeUpDownSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeUpDownSubsystem.java index 182ad406..85412687 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeUpDownSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeUpDownSubsystem.java @@ -48,8 +48,8 @@ public boolean isBackIntakeUp() { return m_backIntakeUpDown.get(); } - private void setIntake(IntakeState newState, Solenoid Solenoid) { - m_frontIntakeUpDown.set(newState.value); + private void setIntake(IntakeState newState, Solenoid solenoid) { + solenoid.set(newState.value); m_currentState = newState; } diff --git a/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java index a492aa3a..e8a5fec8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java @@ -21,7 +21,7 @@ public class LiftSubsystem extends SubsystemBase implements Loggable { // local liftSubsystem public LiftSubsystem(DoubleSolenoid liftUpDown) { this.m_liftUpDown = liftUpDown; - this.setName("Lift Subsystem"); + // this.setName("Lift Subsystem"); } // gets the current state diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index a48d8366..607f6f5f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -22,9 +22,9 @@ public class LimelightSubsystem extends SubsystemBase implements Loggable { // Store local values of distance and yaw so they aren't calculated multiple // times a loop - @Log.ToString(tabName = "Turret") + // @Log.ToString(tabName = "Turret") public Distance m_distanceToTarget; - @Log.ToString(tabName = "turret") + // @Log.ToString(tabName = "turret") public Rotations m_yawFromTarget; @Log.CameraStream diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index eb88abb0..aa5554e1 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,180 +1,180 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.17.6", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "http://devsite.ctr-electronics.com/maven/release/" - ], - "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.17.6" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.17.6" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.17.6", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "diagnostics", - "version": "5.17.6", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "canutils", - "version": "5.17.6", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "platform-stub", - "version": "5.17.6", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.17.6", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.17.6", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.17.6", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.17.6", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "diagnostics", - "version": "5.17.6", - "libName": "CTRE_PhoenixDiagnostics", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "canutils", - "version": "5.17.6", - "libName": "CTRE_PhoenixCanutils", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "platform-stub", - "version": "5.17.6", - "libName": "CTRE_PhoenixPlatform", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.17.6", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } - ] +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.18.2", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.18.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.18.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.18.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.18.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.18.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.18.2", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.18.2", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.18.2", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.18.2", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/REVColorSensorV3.json b/vendordeps/REVColorSensorV3.json index b047a40a..9601ef88 100644 --- a/vendordeps/REVColorSensorV3.json +++ b/vendordeps/REVColorSensorV3.json @@ -1,37 +1,37 @@ -{ - "fileName": "REVColorSensorV3.json", - "name": "REVColorSensorV3", - "version": "1.0.0", - "uuid": "cda7b4b1-d003-44ac-a1ef-485c1347059a", - "mavenUrls": [ - "http://www.revrobotics.com/content/sw/color-sensor-v3/sdk/maven/" - ], - "jsonUrl": "http://www.revrobotics.com/content/sw/color-sensor-v3/sdk/REVColorSensorV3.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "ColorSensorV3-java", - "version": "1.0.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "ColorSensorV3-cpp", - "version": "1.0.0", - "libName": "ColorSensorV3", - "headerClassifier": "headers", - "sourcesClassifier": "source", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "linuxathena", - "linuxraspbian" - ] - } - ] +{ + "fileName": "REVColorSensorV3.json", + "name": "REVColorSensorV3", + "version": "1.2.0", + "uuid": "cda7b4b1-d003-44ac-a1ef-485c1347059a", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/color-sensor-v3/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/color-sensor-v3/sdk/REVColorSensorV3.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "ColorSensorV3-java", + "version": "1.2.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "ColorSensorV3-cpp", + "version": "1.2.0", + "libName": "ColorSensorV3", + "headerClassifier": "headers", + "sourcesClassifier": "source", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] } \ No newline at end of file From 7a80113dbdc54bbe88b2163bcbfe4d48db76bf89 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Wed, 26 Feb 2020 17:28:18 -0800 Subject: [PATCH 02/11] driving works! --- src/main/java/frc/team2412/robot/OI.java | 81 ++++++++++++------- .../frc/team2412/robot/RobotContainer.java | 35 ++++---- .../java/frc/team2412/robot/RobotMap.java | 8 +- .../drive/DriveShiftToHighGearCommand.java | 3 +- .../drive/DriveShiftToLowGearCommand.java | 4 +- .../commands/hood/HoodAdjustCommand.java | 24 ++++++ .../commands/indexer/IndexShootCommand.java | 32 ++++---- .../commands/indexer/IndexSpitCommand.java | 2 +- .../robot/subsystems/HoodSubsystem.java | 6 +- .../subsystems/IntakeOnOffSubsystem.java | 3 + .../robot/subsystems/LiftSubsystem.java | 1 + .../robot/subsystems/TurretSubsystem.java | 4 +- 12 files changed, 132 insertions(+), 71 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index 4ea482e7..f648f02a 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -1,6 +1,7 @@ package frc.team2412.robot; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.Button; @@ -9,6 +10,7 @@ import frc.team2412.robot.commands.drive.DriveCommand; import frc.team2412.robot.commands.drive.DriveShiftToHighGearCommand; import frc.team2412.robot.commands.drive.DriveShiftToLowGearCommand; +import frc.team2412.robot.commands.hood.HoodAdjustCommand; import frc.team2412.robot.commands.indexer.IndexShootCommand; import frc.team2412.robot.commands.indexer.IndexSpitCommand; import frc.team2412.robot.commands.intake.IntakeBothInCommandGroup; @@ -16,8 +18,12 @@ import frc.team2412.robot.commands.intake.IntakeBothOutCommandGroup; import frc.team2412.robot.commands.intake.IntakeBothUpCommand; import frc.team2412.robot.commands.intake.back.IntakeBackDownCommand; +import frc.team2412.robot.commands.intake.back.IntakeBackInCommand; +import frc.team2412.robot.commands.intake.back.IntakeBackOffCommand; import frc.team2412.robot.commands.intake.back.IntakeBackUpCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontDownCommand; +import frc.team2412.robot.commands.intake.front.IntakeFrontInCommand; +import frc.team2412.robot.commands.intake.front.IntakeFrontOffCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontUpCommand; import frc.team2412.robot.commands.lift.LiftDownCommand; import frc.team2412.robot.commands.lift.LiftUpCommand; @@ -37,11 +43,7 @@ private Joysticks(int id) { public enum DriverControls { SHOOT(Joysticks.DRIVER_RIGHT, 1), SHIFT(Joysticks.DRIVER_RIGHT, 2), SPIT(Joysticks.DRIVER_LEFT, 1), - ALIGN_STICKS(Joysticks.DRIVER_LEFT, 3), // - // temp testing controls - FRONT_INTAKE_DOWN(Joysticks.DRIVER_RIGHT, 6), BACK_INTAKE_DOWN(Joysticks.DRIVER_RIGHT, 5), - INTAKE_IN(Joysticks.DRIVER_RIGHT, 3), INTAKE_OUT(Joysticks.DRIVER_RIGHT, 4), LIFT_UP(Joysticks.DRIVER_LEFT, 6), - LIFT_DOWN(Joysticks.DRIVER_LEFT, 7); + ALIGN_STICKS(Joysticks.DRIVER_LEFT, 3); public Joysticks stick; public int buttonID; @@ -60,7 +62,9 @@ public Button createFrom(Joystick stick) { } public enum CodriverControls { - SOMETHING(0); + SOMETHING(0), LIFT_UP(XboxController.Button.kY.value), LIFT_DOWN(XboxController.Button.kA.value), + FRONT_INTAKE_DOWN(XboxController.Button.kBumperRight.value), + BACK_INTAKE_DOWN(XboxController.Button.kBumperLeft.value), HOOD_UP(0), HOOD_DOWN(180); public int buttonID; @@ -74,12 +78,26 @@ public Button createFrom(Joystick stick) { } return new JoystickButton(stick, this.buttonID); } + + public Button createFrom(XboxController controller) { + if (controller.getPort() != Joysticks.CODRIVER.id) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getRawButton(this.buttonID)); + } + + public Button createFromPOV(XboxController controller) { + if (controller.getPort() != Joysticks.CODRIVER.id) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getPOV() == this.buttonID); + } } Joystick driverRightStick = new Joystick(Joysticks.DRIVER_RIGHT.id); Joystick driverLeftStick = new Joystick(Joysticks.DRIVER_LEFT.id); - Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); - Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); + XboxController codriverStick = new XboxController(Joysticks.CODRIVER.id); + // Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); public Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); @@ -87,16 +105,18 @@ public Button createFrom(Joystick stick) { public Button indexerShootButton = DriverControls.SHOOT.createFrom(driverRightStick); public Button indexerSpitButton = DriverControls.SPIT.createFrom(driverLeftStick); - public Button intakeFrontDownButton = DriverControls.FRONT_INTAKE_DOWN.createFrom(driverRightStick); - public Button intakeBackDownButton = DriverControls.BACK_INTAKE_DOWN.createFrom(driverRightStick); - public Button intakeInButton = DriverControls.INTAKE_IN.createFrom(driverRightStick); - public Button intakeOutButton = DriverControls.INTAKE_OUT.createFrom(driverRightStick); + public Button intakeFrontDownButton = CodriverControls.FRONT_INTAKE_DOWN.createFrom(codriverStick); + public Button intakeBackDownButton = CodriverControls.BACK_INTAKE_DOWN.createFrom(codriverStick); + + public Button liftUpButton = CodriverControls.LIFT_UP.createFrom(codriverStick); + public Button liftDownButton = CodriverControls.LIFT_DOWN.createFrom(codriverStick); - public Button liftUpButton = DriverControls.LIFT_UP.createFrom(driverLeftStick); - public Button liftDownButton = DriverControls.LIFT_DOWN.createFrom(driverLeftStick); + public Button hoodUpButton = CodriverControls.HOOD_UP.createFromPOV(codriverStick); + public Button hoodDownButton = CodriverControls.HOOD_DOWN.createFromPOV(codriverStick); // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { + if (RobotMap.INTAKE_CONNECTED) { // INTAKE front // intakeFrontOnButton.whenPressed(new @@ -111,20 +131,26 @@ public OI(RobotContainer robotContainer) { // IntakeBackBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, // robotContainer.m_intakeMotorOnOffSubsystem)); // intakeBackOffButton.whenPressed(new - // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, + // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem,\][] // robotContainer.m_intakeMotorOnOffSubsystem)); - intakeFrontDownButton.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); - intakeFrontDownButton.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); + intakeFrontDownButton.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem) + .andThen(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem)) + .andThen(new InstantCommand(() -> System.out.println("Front Down")))); + intakeFrontDownButton.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem) + .andThen(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem)) + .andThen(new InstantCommand(() -> System.out.println("Front Up")))); - intakeBackDownButton.whenPressed(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); - intakeBackDownButton.whenReleased(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); + intakeBackDownButton.whenPressed(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem) + .andThen(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem))); + intakeBackDownButton.whenReleased(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem) + .andThen(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem))); - intakeInButton.whenPressed(new IntakeBothInCommandGroup(robotContainer.m_intakeMotorOnOffSubsystem)); - intakeOutButton.whenPressed(new IntakeBothOutCommandGroup(robotContainer.m_intakeMotorOnOffSubsystem)); + } - intakeInButton.or(intakeOutButton) - .whenInactive(new IntakeBothOffCommandGroup(robotContainer.m_intakeMotorOnOffSubsystem)); + if (RobotMap.SHOOTER_CONNECTED) { + hoodDownButton.whileHeld(new HoodAdjustCommand(robotContainer.m_hoodSubsystem, -0.05)); + hoodUpButton.whileHeld(new HoodAdjustCommand(robotContainer.m_hoodSubsystem, 0.05)); } if (RobotMap.CONTROL_PANEL_CONNECTED) { @@ -155,11 +181,7 @@ public OI(RobotContainer robotContainer) { robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem)); indexerShootButton.whenPressed(new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, - robotContainer.m_indexerMotorSubsystem) - .andThen(new WaitCommand(2).alongWith(new InstantCommand(() -> { - robotContainer.m_indexerMotorSubsystem.setMidMotor(1); - }))) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setMidMotor(0)))); + robotContainer.m_indexerMotorSubsystem)); } if (RobotMap.DRIVE_BASE_CONNECTED) { @@ -180,7 +202,8 @@ public OI(RobotContainer robotContainer) { Trigger intakeUpWhenFiveBalls = new Trigger(RobotState::hasFiveBalls); if (RobotMap.INTAKE_CONNECTED) { - //intakeUpWhenFiveBalls.whenActive(new IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem)); + // intakeUpWhenFiveBalls.whenActive(new + // IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem)); } } diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java index 71c47bc4..892824b9 100644 --- a/src/main/java/frc/team2412/robot/RobotContainer.java +++ b/src/main/java/frc/team2412/robot/RobotContainer.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.commands.indexer.IndexSpitCommand; import frc.team2412.robot.subsystems.ClimbLiftSubsystem; import frc.team2412.robot.subsystems.ClimbMotorSubsystem; import frc.team2412.robot.subsystems.ControlPanelColorSubsystem; @@ -20,45 +21,48 @@ // this is the class for containing all the subsystems and OI of the robot public class RobotContainer implements Loggable { - // @Log( name = "Limelight Subsystem", tabName = "Turret") + @Log( name = "Limelight Subsystem", tabName = "Turret") public LimelightSubsystem m_LimelightSubsystem; - // @Log(name = "Turret Subsystem", tabName = "Turret") + @Log(name = "Turret Subsystem", tabName = "Turret") public TurretSubsystem m_turretSubsystem; - // @Log(name = "Flywheel Subsystem", tabName = "Turret") + @Log(name = "Flywheel Subsystem", tabName = "Turret") public FlywheelSubsystem m_flywheelSubsystem; - // @Log(name = "Hood Subsystem", tabName = "Turret") + @Log(name = "Hood Subsystem", tabName = "Turret") public HoodSubsystem m_hoodSubsystem; - // @Log(tabName = "Lift Subsystem", name = "Lift") + @Log(name = "Lift Subsystem", tabName = "Lift") public LiftSubsystem m_liftSubsystem; - // @Log(tabName = "Drivebase Subsystem", name = "Drivebase Subsystem") + @Log(name = "Drivebase Subsystem", tabName = "Drivebase Subsystem") public DriveBaseSubsystem m_driveBaseSubsystem; - // @Log(tabName = "Intake Motors Subsystem", name = "Intake") + @Log(name = "Intake Motors Subsystem", tabName = "Intake") public IntakeOnOffSubsystem m_intakeMotorOnOffSubsystem; - // @Log(tabName = "Intake Lift Subsystem", name = "Intake") + @Log(name = "Intake Subsystem", tabName = "Intake") public IntakeUpDownSubsystem m_intakeUpDownSubsystem; - // @Log(tabName = "Control Panel Subsystem", name = "Control Panel") + @Log(name = "Control Panel Subsystem", tabName = "Control Panel") public ControlPanelColorSubsystem m_controlPanelColorSubsystem; - // @Log(tabName = "Indexer Motor Subsystem", name = "Indexer") + @Log(name = "Indexer Motor Subsystem", tabName = "Indexer") public IndexerMotorSubsystem m_indexerMotorSubsystem; - // @Log(tabName = "Indexer Sensor Subsystem", name = "Indexer") + @Log(name = "Indexer Sensor Subsystem", tabName = "Indexer") public IndexerSensorSubsystem m_indexerSensorSubsystem; - // @Log(tabName = "Climb Lift Subsystem", name = "Climb") + @Log(name = "Climb Lift Subsystem", tabName = "Climb") public ClimbLiftSubsystem m_climbLiftSubsystem; - // @Log(tabName = "Climb Motor Subsystem", name = "Climb") + @Log(name = "Climb Motor Subsystem", tabName = "Climb") public ClimbMotorSubsystem m_climbMotorSubsystem; + @Log(tabName = "Indexer") + Command spitCommand; + // A chooser for autonomous commands SendableChooser m_chooser = new SendableChooser<>(); @@ -75,6 +79,9 @@ public RobotContainer() { m_indexerMotorSubsystem = new IndexerMotorSubsystem(RobotMap.indexFrontMotor, RobotMap.indexMidMotor, RobotMap.indexBackMotor, m_indexerSensorSubsystem); + + + } if (RobotMap.LIFT_CONNECTED) { @@ -106,6 +113,6 @@ public RobotContainer() { m_hoodSubsystem = new HoodSubsystem(RobotMap.hoodServo1, RobotMap.hoodServo2); } - + spitCommand = new IndexSpitCommand(m_indexerSensorSubsystem, m_indexerMotorSubsystem, m_intakeMotorOnOffSubsystem); } } diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index a24ba8e4..5ce90bfc 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -33,11 +33,11 @@ public class RobotMap { public static boolean INDEX_CONNECTED = true; public static boolean INTAKE_CONNECTED = true; public static boolean LIFT_CONNECTED = true; - public static boolean DRIVE_BASE_CONNECTED = false; + public static boolean DRIVE_BASE_CONNECTED = true; public static enum CANBus { - INTAKE1(11), INDEX1(12), INTAKE2(21), INDEX2(22), INTAKE3(31), INDEX3(32), INDEX_MID(40), DRIVE_LEFT_FRONT(1), - DRIVE_LEFT_BACK(2), DRIVE_RIGHT_FRONT(3), DRIVE_RIGHT_BACK(4), CLIMB1(5), CLIMB2(6), TURRET(7), + INTAKE1(11), INDEX1(12), INTAKE2(21), INDEX2(22), INTAKE3(31), INDEX3(32), INDEX_MID(40), DRIVE_LEFT_FRONT(3), + DRIVE_LEFT_BACK(4), DRIVE_RIGHT_FRONT(1), DRIVE_RIGHT_BACK(2), CLIMB1(5), CLIMB2(6), TURRET(7), FLYWHEEL_LEFT(8), FLYWHEEL_RIGHT(9), CONTROL_PANEL(10); public final int id; @@ -48,7 +48,7 @@ private CANBus(int canBusId) { } public static enum PneumaticPort { - DRIVE(0), CLIMB_LEFT(1), CLIMB_RIGHT(2), INTAKE_FRONT_UP(6), INTAKE_BACK_UP(7), LIFT_UP(5), LIFT_DOWN(4); + DRIVE(2), CLIMB_LEFT(4), CLIMB_RIGHT(5), INTAKE_FRONT_UP(7), INTAKE_BACK_UP(6), LIFT_UP(0), LIFT_DOWN(1); public final int id; diff --git a/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToHighGearCommand.java b/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToHighGearCommand.java index 063ebb6c..c08e97b0 100644 --- a/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToHighGearCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToHighGearCommand.java @@ -8,7 +8,6 @@ public class DriveShiftToHighGearCommand extends CommandBase { private DriveBaseSubsystem m_driveBaseSubsystem; public DriveShiftToHighGearCommand(DriveBaseSubsystem driveBaseSubsystem) { - addRequirements(driveBaseSubsystem); this.m_driveBaseSubsystem = driveBaseSubsystem; } @@ -19,6 +18,6 @@ public void execute() { @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToLowGearCommand.java b/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToLowGearCommand.java index b80b70ce..6e789e42 100644 --- a/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToLowGearCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drive/DriveShiftToLowGearCommand.java @@ -8,7 +8,6 @@ public class DriveShiftToLowGearCommand extends CommandBase { private DriveBaseSubsystem m_driveBaseSubsystem; public DriveShiftToLowGearCommand(DriveBaseSubsystem driveBaseSubsystem) { - addRequirements(driveBaseSubsystem); this.m_driveBaseSubsystem = driveBaseSubsystem; } @@ -16,11 +15,10 @@ public DriveShiftToLowGearCommand(DriveBaseSubsystem driveBaseSubsystem) { @Override public void execute() { m_driveBaseSubsystem.shiftToLowGear(); - ; } @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java b/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java new file mode 100644 index 00000000..a9aa7f4d --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java @@ -0,0 +1,24 @@ +package frc.team2412.robot.commands.hood; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.subsystems.HoodSubsystem; + +public class HoodAdjustCommand extends CommandBase{ + private double m_increment; + private HoodSubsystem m_HoodSubsystem; + + public HoodAdjustCommand(HoodSubsystem hoodSubsystem, double increment){ + this.m_HoodSubsystem = hoodSubsystem; + this.m_increment = increment; + } + + public void execute(){ + this.m_HoodSubsystem.add(this.m_increment); + } + + public boolean isFinished(){ + return true; + } + + +} diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java index ac85cc76..5fe9215c 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java @@ -20,20 +20,20 @@ public IndexShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSub @Override public void execute() { m_indexerMotorSubsystem.setMidMotor(1); - if (!m_indexerSensorSubsystem.getIndexBackInnerSensorValue() - && !m_indexerSensorSubsystem.getIndexFrontInnerSensorValue()) { - if (RobotState.m_unbalancedSide == RobotState.UnbalancedSide.FRONT) { - m_indexerMotorSubsystem.setFrontMotor(-1); - if (m_indexerSensorSubsystem.allFrontSensorsOff()) { - m_indexerMotorSubsystem.setBackMotor(-1); - } - } else { - m_indexerMotorSubsystem.setBackMotor(-1); - if (m_indexerSensorSubsystem.allBackSensorsOff()) { - m_indexerMotorSubsystem.setFrontMotor(-1); - } - } - } + // if (!m_indexerSensorSubsystem.getIndexBackInnerSensorValue() + // && !m_indexerSensorSubsystem.getIndexFrontInnerSensorValue()) { + // if (RobotState.m_unbalancedSide == RobotState.UnbalancedSide.FRONT) { + // m_indexerMotorSubsystem.setFrontMotor(1); + // if (m_indexerSensorSubsystem.allFrontSensorsOff()) { + // m_indexerMotorSubsystem.setBackMotor(1); + // } + // } else { + // m_indexerMotorSubsystem.setBackMotor(1); + // if (m_indexerSensorSubsystem.allBackSensorsOff()) { + // m_indexerMotorSubsystem.setFrontMotor(1); + // } + // } + // } } @Override @@ -45,13 +45,15 @@ public void end(boolean cancel) { @Override public boolean isFinished() { if (m_indexerSensorSubsystem.allInnerSensorsOff()) { - return true; + // return true; } else { RobotState.m_ballCount = m_indexerSensorSubsystem.totalSensorsOn(); return false; } + return false; + } } diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java index 34f42ee2..d319e390 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java @@ -27,7 +27,7 @@ public void execute() { m_indexerMotorSubsystem.setFrontMotor(1); m_indexerMotorSubsystem.setBackMotor(1); m_indexerMotorSubsystem.setMidMotor(-0.1); - m_intakeOnOffSubsystem.setIntake(1); + m_intakeOnOffSubsystem.setIntake(-1); } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index eaf0c4e7..14fe5991 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -28,7 +28,7 @@ public HoodSubsystem(Servo hoodServo1, Servo hoodServo2) { } public double getServo() { - return m_hoodServo1.get(); + return m_hoodServo2.get(); } @Log(name = "Extend Servos Fully", tabName = "Hood", width = 2, height = 1, columnIndex = 4, rowIndex = 2) @@ -59,4 +59,8 @@ public void setServo(double angle) { public void periodic() { } + public void add(double m_increment) { + this.setServo(getServo() + m_increment); + } + } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java index 299139e4..8d7af5c4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeOnOffSubsystem.java @@ -27,6 +27,9 @@ public IntakeOnOffSubsystem(CANSparkMax frontMotor, CANSparkMax backMotor) { this.m_intakeFrontMotor = frontMotor; this.m_intakeBackMotor = backMotor; this.m_intakeMotorGroup = new SpeedControllerGroup(m_intakeFrontMotor, m_intakeBackMotor); + + m_intakeBackMotor.setInverted(true); + m_intakeFrontMotor.setInverted(true); } public void backIntakeOff() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java index e8a5fec8..76a72657 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LiftSubsystem.java @@ -43,6 +43,7 @@ public void liftUp() { // the current state to that state private void setLift(LiftState value) { m_liftUpDown.set(value.value); + System.out.println(value); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java index fe0ae3d4..d697db00 100644 --- a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java @@ -65,7 +65,7 @@ public void initTurretEncoder() { @Override public void periodic() { - m_turretCurrentPosition = m_turretMotor.getSelectedSensorPosition(0); + // m_turretCurrentPosition = m_turretMotor.getSelectedSensorPosition(0); if (m_turretCurrentPosition - m_turretPastPosition > ENCODER_MAX_ERROR_JUMP) { m_turretOffsetPosition += TICKS_PER_REVOLUTION; @@ -77,7 +77,7 @@ public void periodic() { m_turretPastPosition = m_turretCurrentPosition; m_currentAngle = new Rotations((getMeasurement() == 0) ? 0 : (getMeasurement() / TICKS_PER_DEGREE), RotationUnits.DEGREE); - System.out.println(getMeasurement()); + // System.out.println(getMeasurement()); } @Config From 8377060fae40b2b03a32499e47ce5bf1250dcfe4 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Wed, 26 Feb 2020 21:01:43 -0800 Subject: [PATCH 03/11] yay boi shoot --- src/main/java/frc/team2412/robot/OI.java | 26 ++++++++++++++----- .../robot/subsystems/HoodSubsystem.java | 2 +- .../subsystems/IndexerMotorSubsystem.java | 8 +++--- .../constants/IndexerConstants.java | 2 +- 4 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index f648f02a..473ebd81 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -13,10 +12,6 @@ import frc.team2412.robot.commands.hood.HoodAdjustCommand; import frc.team2412.robot.commands.indexer.IndexShootCommand; import frc.team2412.robot.commands.indexer.IndexSpitCommand; -import frc.team2412.robot.commands.intake.IntakeBothInCommandGroup; -import frc.team2412.robot.commands.intake.IntakeBothOffCommandGroup; -import frc.team2412.robot.commands.intake.IntakeBothOutCommandGroup; -import frc.team2412.robot.commands.intake.IntakeBothUpCommand; import frc.team2412.robot.commands.intake.back.IntakeBackDownCommand; import frc.team2412.robot.commands.intake.back.IntakeBackInCommand; import frc.team2412.robot.commands.intake.back.IntakeBackOffCommand; @@ -64,7 +59,8 @@ public Button createFrom(Joystick stick) { public enum CodriverControls { SOMETHING(0), LIFT_UP(XboxController.Button.kY.value), LIFT_DOWN(XboxController.Button.kA.value), FRONT_INTAKE_DOWN(XboxController.Button.kBumperRight.value), - BACK_INTAKE_DOWN(XboxController.Button.kBumperLeft.value), HOOD_UP(0), HOOD_DOWN(180); + BACK_INTAKE_DOWN(XboxController.Button.kBumperLeft.value), HOOD_UP(0), HOOD_DOWN(180), + INDEX_FOWARD(XboxController.Button.kB.value), INDEX_BACKWARD(XboxController.Button.kX.value); public int buttonID; @@ -114,6 +110,9 @@ public Button createFromPOV(XboxController controller) { public Button hoodUpButton = CodriverControls.HOOD_UP.createFromPOV(codriverStick); public Button hoodDownButton = CodriverControls.HOOD_DOWN.createFromPOV(codriverStick); + public Button indexFowardButton = CodriverControls.INDEX_FOWARD.createFrom(codriverStick); + public Button indexBackwardButton = CodriverControls.INDEX_BACKWARD.createFrom(codriverStick); + // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { @@ -182,6 +181,21 @@ public OI(RobotContainer robotContainer) { indexerShootButton.whenPressed(new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, robotContainer.m_indexerMotorSubsystem)); + + indexFowardButton.whenPressed(new InstantCommand(() -> { + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0.5); + robotContainer.m_indexerMotorSubsystem.setBackMotor(-0.5); + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + })); + indexBackwardButton.whenPressed(new InstantCommand(() -> { + robotContainer.m_indexerMotorSubsystem.setFrontMotor(-0.5); + robotContainer.m_indexerMotorSubsystem.setBackMotor(0.5); + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + })); + + indexBackwardButton.or(indexFowardButton) + .whenInactive(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors())); + } if (RobotMap.DRIVE_BASE_CONNECTED) { diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index 14fe5991..2b6b80eb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -60,7 +60,7 @@ public void periodic() { } public void add(double m_increment) { - this.setServo(getServo() + m_increment); + this.setServo(Math.min(getServo() + m_increment, HoodConstants.MAX_EXTENSION)); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java index 187890a7..687f5f8e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java @@ -53,11 +53,11 @@ public IndexerMotorSubsystem(CANSparkMax frontMotor, CANSparkMax midMotor, CANSp m_sideMotors = new SpeedControllerGroup(m_indexFrontMotor, m_indexBackMotor); m_allMotors = new SpeedControllerGroup(m_indexFrontMotor, m_indexMidMotor, m_indexBackMotor); - Trigger frontProcess = new Trigger(indexerSensorSubsystem::getIntakeFrontSensorValue); - frontProcess.whenActive(new IndexIntakeFrontCommandGroup(indexerSensorSubsystem, this), true); + // Trigger frontProcess = new Trigger(indexerSensorSubsystem::getIntakeFrontSensorValue); + // frontProcess.whenActive(new IndexIntakeFrontCommandGroup(indexerSensorSubsystem, this), true); - Trigger backProcess = new Trigger(indexerSensorSubsystem::getIntakeBackSensorValue); - backProcess.whenActive(new IndexIntakeBackCommandGroup(indexerSensorSubsystem, this), true); + // Trigger backProcess = new Trigger(indexerSensorSubsystem::getIntakeBackSensorValue); + // backProcess.whenActive(new IndexIntakeBackCommandGroup(indexerSensorSubsystem, this), true); midTicks = m_midEncoder.getPosition(); } diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java index c5868efa..c33303bf 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java @@ -37,7 +37,7 @@ public void flip() { public static final double SPINDLE_DIAMETER = 1.2; public static final double INCH_STOP_DISTANCE = 2; - public static final double MAX_SPEED = 0.5; + public static final double MAX_SPEED = 0.3; public static final double TOP_TICKS = 20; public static final double BOTTOM_TICKS = 0; From 7777f3c6f7e045531132396658d127f5b72320a5 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Thu, 27 Feb 2020 15:58:26 -0800 Subject: [PATCH 04/11] working codriver board and manual index. Driver vision --- src/main/java/frc/team2412/robot/Logging.java | 18 ++-- src/main/java/frc/team2412/robot/OI.java | 87 +++++++++---------- src/main/java/frc/team2412/robot/Robot.java | 3 +- .../frc/team2412/robot/RobotContainer.java | 5 +- .../commands/indexer/IndexSpitCommand.java | 4 +- .../subsystems/IndexerMotorSubsystem.java | 16 ++-- .../robot/subsystems/LimelightSubsystem.java | 6 +- .../robot/subsystems/TurretSubsystem.java | 1 + .../constants/IndexerConstants.java | 1 + 9 files changed, 71 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Logging.java b/src/main/java/frc/team2412/robot/Logging.java index db3efaf9..ce9e123b 100644 --- a/src/main/java/frc/team2412/robot/Logging.java +++ b/src/main/java/frc/team2412/robot/Logging.java @@ -1,13 +1,12 @@ package frc.team2412.robot; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; -public class Logging implements Loggable { - - @Log.Exclude - Robot m_robot; +public class Logging implements Loggable, Sendable { // 4 rows, 10 col @@ -67,8 +66,7 @@ public class Logging implements Loggable { @Log(columnIndex = 9, rowIndex = 3, tabName = "Driver View") public double totalCurrentDraw = 0; - public Logging(Robot robot) { - this.m_robot = robot; + public Logging() { } public void periodic() { @@ -98,10 +96,14 @@ public void periodic() { // 6.8 V is the warning level for brownout brownoutWarning = (RobotController.getInputVoltage() < 7 || RobotController.isBrownedOut()); - double time = m_robot.timeRemaining; - timer = time / 60 + " : " + time % 60; + // double time = m_robot.timeRemaining; + // timer = time / 60 + " : " + time % 60; driveBaseCurrentDraw = RobotMap.m_robotContainer.m_driveBaseSubsystem.getCurrentDraw(); } + public void initSendable(SendableBuilder builder){ + + } + } diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index 473ebd81..f7f1874d 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -19,7 +19,9 @@ import frc.team2412.robot.commands.intake.front.IntakeFrontDownCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontInCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontOffCommand; +import frc.team2412.robot.commands.intake.front.IntakeFrontOutCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontUpCommand; +import frc.team2412.robot.commands.intake.back.IntakeBackOutCommand; import frc.team2412.robot.commands.lift.LiftDownCommand; import frc.team2412.robot.commands.lift.LiftUpCommand; @@ -57,11 +59,8 @@ public Button createFrom(Joystick stick) { } public enum CodriverControls { - SOMETHING(0), LIFT_UP(XboxController.Button.kY.value), LIFT_DOWN(XboxController.Button.kA.value), - FRONT_INTAKE_DOWN(XboxController.Button.kBumperRight.value), - BACK_INTAKE_DOWN(XboxController.Button.kBumperLeft.value), HOOD_UP(0), HOOD_DOWN(180), - INDEX_FOWARD(XboxController.Button.kB.value), INDEX_BACKWARD(XboxController.Button.kX.value); - + LIFT(7), FRONT_INTAKE_DOWN(2), BACK_INTAKE_DOWN(1), INTAKE_BACK_IN(4), INTAKE_BACK_OUT(3), INTAKE_FRONT_IN(6), + INTAKE_FRONT_OUT(5); public int buttonID; private CodriverControls(int buttonID) { @@ -92,7 +91,7 @@ public Button createFromPOV(XboxController controller) { Joystick driverRightStick = new Joystick(Joysticks.DRIVER_RIGHT.id); Joystick driverLeftStick = new Joystick(Joysticks.DRIVER_LEFT.id); - XboxController codriverStick = new XboxController(Joysticks.CODRIVER.id); + Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); // Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); public Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); @@ -101,17 +100,15 @@ public Button createFromPOV(XboxController controller) { public Button indexerShootButton = DriverControls.SHOOT.createFrom(driverRightStick); public Button indexerSpitButton = DriverControls.SPIT.createFrom(driverLeftStick); - public Button intakeFrontDownButton = CodriverControls.FRONT_INTAKE_DOWN.createFrom(codriverStick); - public Button intakeBackDownButton = CodriverControls.BACK_INTAKE_DOWN.createFrom(codriverStick); - - public Button liftUpButton = CodriverControls.LIFT_UP.createFrom(codriverStick); - public Button liftDownButton = CodriverControls.LIFT_DOWN.createFrom(codriverStick); + public Button liftButton = CodriverControls.LIFT.createFrom(codriverStick); - public Button hoodUpButton = CodriverControls.HOOD_UP.createFromPOV(codriverStick); - public Button hoodDownButton = CodriverControls.HOOD_DOWN.createFromPOV(codriverStick); + public Button frontIntakeUpDown = CodriverControls.FRONT_INTAKE_DOWN.createFrom(codriverStick); + public Button backIntakeUpDown = CodriverControls.BACK_INTAKE_DOWN.createFrom(codriverStick); - public Button indexFowardButton = CodriverControls.INDEX_FOWARD.createFrom(codriverStick); - public Button indexBackwardButton = CodriverControls.INDEX_BACKWARD.createFrom(codriverStick); + public Button intakeFrontIn = CodriverControls.INTAKE_FRONT_IN.createFrom(codriverStick); + public Button intakeFrontOut = CodriverControls.INTAKE_FRONT_OUT.createFrom(codriverStick); + public Button intakeBackIn = CodriverControls.INTAKE_BACK_IN.createFrom(codriverStick); + public Button intakeBackOut = CodriverControls.INTAKE_BACK_OUT.createFrom(codriverStick); // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { @@ -130,26 +127,38 @@ public OI(RobotContainer robotContainer) { // IntakeBackBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, // robotContainer.m_intakeMotorOnOffSubsystem)); // intakeBackOffButton.whenPressed(new - // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem,\][] + // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, // robotContainer.m_intakeMotorOnOffSubsystem)); - intakeFrontDownButton.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem) - .andThen(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem)) - .andThen(new InstantCommand(() -> System.out.println("Front Down")))); - intakeFrontDownButton.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem) - .andThen(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem)) - .andThen(new InstantCommand(() -> System.out.println("Front Up")))); + frontIntakeUpDown.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); + frontIntakeUpDown.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); + + backIntakeUpDown.whenPressed(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); + backIntakeUpDown.whenReleased(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); + + intakeFrontIn.whenPressed(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1)))); + intakeFrontIn.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); - intakeBackDownButton.whenPressed(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem) - .andThen(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem))); - intakeBackDownButton.whenReleased(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem) - .andThen(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem))); + intakeFrontOut.whenPressed(new IntakeFrontOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(1)))); + intakeFrontOut.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); + + intakeBackIn.whenPressed(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(-1)))); + intakeBackIn.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); + + intakeBackOut.whenPressed(new IntakeBackOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(1)))); + intakeBackOut.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); } if (RobotMap.SHOOTER_CONNECTED) { - hoodDownButton.whileHeld(new HoodAdjustCommand(robotContainer.m_hoodSubsystem, -0.05)); - hoodUpButton.whileHeld(new HoodAdjustCommand(robotContainer.m_hoodSubsystem, 0.05)); } if (RobotMap.CONTROL_PANEL_CONNECTED) { @@ -176,26 +185,12 @@ public OI(RobotContainer robotContainer) { } if (RobotMap.INDEX_CONNECTED) { - indexerSpitButton.whenPressed(new IndexSpitCommand(robotContainer.m_indexerSensorSubsystem, + indexerSpitButton.whileHeld(new IndexSpitCommand(robotContainer.m_indexerSensorSubsystem, robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem)); - indexerShootButton.whenPressed(new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, + indexerShootButton.whileHeld(new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, robotContainer.m_indexerMotorSubsystem)); - indexFowardButton.whenPressed(new InstantCommand(() -> { - robotContainer.m_indexerMotorSubsystem.setFrontMotor(0.5); - robotContainer.m_indexerMotorSubsystem.setBackMotor(-0.5); - robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); - })); - indexBackwardButton.whenPressed(new InstantCommand(() -> { - robotContainer.m_indexerMotorSubsystem.setFrontMotor(-0.5); - robotContainer.m_indexerMotorSubsystem.setBackMotor(0.5); - robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); - })); - - indexBackwardButton.or(indexFowardButton) - .whenInactive(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors())); - } if (RobotMap.DRIVE_BASE_CONNECTED) { @@ -208,9 +203,9 @@ public OI(RobotContainer robotContainer) { // LIFT if (RobotMap.LIFT_CONNECTED) { - liftUpButton.whenPressed( + liftButton.whenPressed( new LiftUpCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); - liftDownButton.whenPressed( + liftButton.whenReleased( new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 22734ebe..26b270b5 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -29,8 +29,6 @@ public class Robot extends TimedRobot implements Loggable { private RobotContainer m_robotContainer = RobotMap.m_robotContainer; @SuppressWarnings("unused") private OI m_OI = RobotMap.m_OI; - @SuppressWarnings("unused") - private Logging logging = new Logging(this); /** * This function is run when the robot is first started up and should be used @@ -64,6 +62,7 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); Logger.updateEntries(); + m_robotContainer.logger.periodic(); } /** diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java index 892824b9..cad67589 100644 --- a/src/main/java/frc/team2412/robot/RobotContainer.java +++ b/src/main/java/frc/team2412/robot/RobotContainer.java @@ -63,6 +63,9 @@ public class RobotContainer implements Loggable { @Log(tabName = "Indexer") Command spitCommand; + @Log(tabName = "Driver View") + public Logging logger = new Logging(); + // A chooser for autonomous commands SendableChooser m_chooser = new SendableChooser<>(); @@ -79,8 +82,6 @@ public RobotContainer() { m_indexerMotorSubsystem = new IndexerMotorSubsystem(RobotMap.indexFrontMotor, RobotMap.indexMidMotor, RobotMap.indexBackMotor, m_indexerSensorSubsystem); - - } diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java index d319e390..ae5fea7a 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java @@ -34,12 +34,12 @@ public void execute() { public void end(boolean cancel) { m_indexerMotorSubsystem.stopAllMotors(); m_intakeOnOffSubsystem.intakeOff(); - RobotState.m_ballCount = 0; + // RobotState.m_ballCount = 0; } @Override public boolean isFinished() { - return m_indexerSensorSubsystem.allBackSensorsOff() && m_indexerSensorSubsystem.allFrontSensorsOff(); + return false; } } diff --git a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java index 687f5f8e..82f0a030 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java @@ -74,7 +74,7 @@ public void setFrontMotor(double val) { } public void setMidMotor(double val) { - val = MathUtils.constrain(val, -IndexerConstants.MAX_SPEED, IndexerConstants.MAX_SPEED); + val = MathUtils.constrain(val, -IndexerConstants.MAX_LIFT_SPEED, IndexerConstants.MAX_LIFT_SPEED); m_indexMidMotor.set(val); } @@ -120,12 +120,14 @@ public void stopBackPID(double val) { } public void setMidPID(boolean upOrDown) { - if (upOrDown) { - m_midPIDController.setReference(midTicks + IndexerConstants.TOP_TICKS, ControlType.kPosition); - } else { - m_midPIDController.setReference(midTicks + IndexerConstants.BOTTOM_TICKS, ControlType.kPosition); - - } + return; + // if (upOrDown) { + + // m_midPIDController.setReference(midTicks + IndexerConstants.TOP_TICKS, ControlType.kPosition); + // } else { + // m_midPIDController.setReference(midTicks + IndexerConstants.BOTTOM_TICKS, ControlType.kPosition); + + // } } public void resetEncoderZero() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 607f6f5f..2443549a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -24,10 +24,10 @@ public class LimelightSubsystem extends SubsystemBase implements Loggable { // times a loop // @Log.ToString(tabName = "Turret") public Distance m_distanceToTarget; - // @Log.ToString(tabName = "turret") + // @Log.ToString(tabName = "Turret") public Rotations m_yawFromTarget; - @Log.CameraStream + @Log.CameraStream(tabName = "Turret") public HttpCamera limeCam; // Store the limelight @@ -45,7 +45,7 @@ public LimelightSubsystem(Limelight limelight) { this.m_yawFromTarget = new Rotations(0); this.setDefaultCommand(new LimelightReadCommand(this)); - + getCameraStream(); } public void getCameraStream() { diff --git a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java index d697db00..faa700f3 100644 --- a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java @@ -25,6 +25,7 @@ public class TurretSubsystem extends PIDSubsystem implements Loggable { @Log(tabName = "Turret") private WPI_TalonSRX m_turretMotor; + @Log.Exclude private LimelightSubsystem m_limelightSubsystem; private int m_turretOffsetPosition = 0; private int m_turretPastPosition; diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java index c33303bf..acd13e26 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java @@ -38,6 +38,7 @@ public void flip() { public static final double INCH_STOP_DISTANCE = 2; public static final double MAX_SPEED = 0.3; + public static final double MAX_LIFT_SPEED = 0; public static final double TOP_TICKS = 20; public static final double BOTTOM_TICKS = 0; From 12299455362803caa5e0ce842d8e239196d03727 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Sat, 29 Feb 2020 15:11:49 -0800 Subject: [PATCH 05/11] first half of the first day --- src/main/java/frc/team2412/robot/OI.java | 47 +++++++++++++++++-- src/main/java/frc/team2412/robot/Robot.java | 31 +++++++++++- .../frc/team2412/robot/RobotContainer.java | 5 -- .../java/frc/team2412/robot/RobotMap.java | 6 +-- .../commands/climb/ClimbJoystickCommand.java | 6 ++- .../commands/intake/IntakeBothUpCommand.java | 1 - .../robot/subsystems/ClimbMotorSubsystem.java | 12 ++++- .../robot/subsystems/DriveBaseSubsystem.java | 16 +++++-- .../robot/subsystems/HoodSubsystem.java | 5 +- .../subsystems/IndexerMotorSubsystem.java | 3 -- .../robot/subsystems/LimelightSubsystem.java | 1 + .../robot/subsystems/TurretSubsystem.java | 10 ++-- .../subsystems/constants/ClimbConstants.java | 4 +- .../subsystems/constants/HoodConstants.java | 2 +- .../constants/IndexerConstants.java | 2 +- 15 files changed, 117 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index f7f1874d..e178fd09 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -5,11 +5,12 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team2412.robot.commands.climb.ClimbDeployRailsCommand; +import frc.team2412.robot.commands.climb.ClimbRetractRailsCommand; +import frc.team2412.robot.commands.climb.ClimbJoystickCommand; import frc.team2412.robot.commands.drive.DriveCommand; import frc.team2412.robot.commands.drive.DriveShiftToHighGearCommand; import frc.team2412.robot.commands.drive.DriveShiftToLowGearCommand; -import frc.team2412.robot.commands.hood.HoodAdjustCommand; import frc.team2412.robot.commands.indexer.IndexShootCommand; import frc.team2412.robot.commands.indexer.IndexSpitCommand; import frc.team2412.robot.commands.intake.back.IntakeBackDownCommand; @@ -61,6 +62,7 @@ public Button createFrom(Joystick stick) { public enum CodriverControls { LIFT(7), FRONT_INTAKE_DOWN(2), BACK_INTAKE_DOWN(1), INTAKE_BACK_IN(4), INTAKE_BACK_OUT(3), INTAKE_FRONT_IN(6), INTAKE_FRONT_OUT(5); + public int buttonID; private CodriverControls(int buttonID) { @@ -89,10 +91,41 @@ public Button createFromPOV(XboxController controller) { } } + public enum CodriverManualControls { + CLIMB_MODE(5); + + public int buttonID; + + private CodriverManualControls(int buttonID) { + this.buttonID = buttonID; + } + + public Button createFrom(Joystick stick) { + if (stick.getPort() != Joysticks.CODRIVER_MANUAL.id) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new JoystickButton(stick, this.buttonID); + } + + public Button createFrom(XboxController controller) { + if (controller.getPort() != Joysticks.CODRIVER_MANUAL.id) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getRawButton(this.buttonID)); + } + + public Button createFromPOV(XboxController controller) { + if (controller.getPort() != Joysticks.CODRIVER_MANUAL.id) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getPOV() == this.buttonID); + } + } + Joystick driverRightStick = new Joystick(Joysticks.DRIVER_RIGHT.id); Joystick driverLeftStick = new Joystick(Joysticks.DRIVER_LEFT.id); Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); - // Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); + Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); public Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); @@ -110,6 +143,8 @@ public Button createFromPOV(XboxController controller) { public Button intakeBackIn = CodriverControls.INTAKE_BACK_IN.createFrom(codriverStick); public Button intakeBackOut = CodriverControls.INTAKE_BACK_OUT.createFrom(codriverStick); + public Button climbModeButton = CodriverManualControls.CLIMB_MODE.createFrom(codriverManualStick); + // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { @@ -182,6 +217,10 @@ public OI(RobotContainer robotContainer) { // ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); // climbStopArmButton.whenActive(new // ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem)); + climbModeButton + .whileHeld(new ClimbJoystickCommand(codriverManualStick, robotContainer.m_climbMotorSubsystem)); + climbModeButton.whenPressed(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); + climbModeButton.whenReleased(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); } if (RobotMap.INDEX_CONNECTED) { @@ -209,7 +248,7 @@ public OI(RobotContainer robotContainer) { new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); } - Trigger intakeUpWhenFiveBalls = new Trigger(RobotState::hasFiveBalls); + // Trigger intakeUpWhenFiveBalls = new Trigger(RobotState::hasFiveBalls); if (RobotMap.INTAKE_CONNECTED) { // intakeUpWhenFiveBalls.whenActive(new // IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem)); diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 26b270b5..15f0e496 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -7,10 +7,20 @@ package frc.team2412.robot; +import java.util.Set; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.team2412.robot.commands.flywheel.FlywheelShootCommand; +import frc.team2412.robot.commands.hood.HoodAdjustCommand; +import frc.team2412.robot.commands.hood.HoodWithdrawCommand; +import frc.team2412.robot.commands.indexer.IndexShootCommand; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.Logger; @@ -30,6 +40,8 @@ public class Robot extends TimedRobot implements Loggable { @SuppressWarnings("unused") private OI m_OI = RobotMap.m_OI; + Command autoCommand; + /** * This function is run when the robot is first started up and should be used * for any initialization code. @@ -82,6 +94,18 @@ public void autonomousInit() { * * */ + autoCommand = new HoodWithdrawCommand(m_robotContainer.m_hoodSubsystem) + .andThen(new HoodAdjustCommand(m_robotContainer.m_hoodSubsystem, .310)) + .andThen(new InstantCommand(() -> m_robotContainer.m_flywheelSubsystem.setSpeed(-0.9))) + .andThen(new WaitCommand(2)) + .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setMidMotor(1))) + .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setBackMotor(-1))) + .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1))) + .andThen(new WaitCommand(10)) + .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(12, 12)) + .andThen(new InstantCommand(() -> System.out.println("driving"))).andThen(new WaitCommand(0.5)) + .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(0, 0)))); + CommandScheduler.getInstance().schedule(autoCommand); } /** @@ -98,6 +122,8 @@ public void autonomousPeriodic() { @Override public void teleopInit() { timeRemaining = 135.0; + CommandScheduler.getInstance().cancel(autoCommand); + m_robotContainer.m_LimelightSubsystem.stopLimelight(); } /** @@ -106,6 +132,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { timeRemaining -= 0.02; + + double val = m_OI.codriverStick.getY() * 0.5 + 0.5; + m_robotContainer.m_hoodSubsystem.setServo(val); } @Override diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java index cad67589..48c56f11 100644 --- a/src/main/java/frc/team2412/robot/RobotContainer.java +++ b/src/main/java/frc/team2412/robot/RobotContainer.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import frc.team2412.robot.commands.indexer.IndexSpitCommand; import frc.team2412.robot.subsystems.ClimbLiftSubsystem; import frc.team2412.robot.subsystems.ClimbMotorSubsystem; import frc.team2412.robot.subsystems.ControlPanelColorSubsystem; @@ -60,9 +59,6 @@ public class RobotContainer implements Loggable { @Log(name = "Climb Motor Subsystem", tabName = "Climb") public ClimbMotorSubsystem m_climbMotorSubsystem; - @Log(tabName = "Indexer") - Command spitCommand; - @Log(tabName = "Driver View") public Logging logger = new Logging(); @@ -114,6 +110,5 @@ public RobotContainer() { m_hoodSubsystem = new HoodSubsystem(RobotMap.hoodServo1, RobotMap.hoodServo2); } - spitCommand = new IndexSpitCommand(m_indexerSensorSubsystem, m_indexerMotorSubsystem, m_intakeMotorOnOffSubsystem); } } diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index 5ce90bfc..e363370a 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -27,7 +27,7 @@ //remember to declare robot container at the bottom of this class public class RobotMap { - public static boolean CLIMB_CONNECTED = false; + public static boolean CLIMB_CONNECTED = true; public static boolean CONTROL_PANEL_CONNECTED = false; public static boolean SHOOTER_CONNECTED = true; public static boolean INDEX_CONNECTED = true; @@ -69,7 +69,7 @@ private DIOPort(int dioPortId) { } public static enum PWMPort { - HOOD_SERVO_1(0), HOOD_SERVO_2(1); + HOOD_SERVO_1(1), HOOD_SERVO_2(0); public final int id; @@ -233,7 +233,7 @@ private CANSparkMax tryGetMotor(int motorId) { ? NetworkTableInstance.create().getTable("limelight") : null; public static Limelight limelight = SHOOTER_CONNECTED - ? new Limelight(limelightNetworkTable, LEDMode.OFF, CamMode.VISION_PROCESSER, Pipeline.ZERO, + ? new Limelight(limelightNetworkTable, LEDMode.ON, CamMode.VISION_PROCESSER, Pipeline.FOUR, StreamMode.STANDARD, SnapshotMode.OFF) : null; diff --git a/src/main/java/frc/team2412/robot/commands/climb/ClimbJoystickCommand.java b/src/main/java/frc/team2412/robot/commands/climb/ClimbJoystickCommand.java index 66761299..86b9ecf3 100644 --- a/src/main/java/frc/team2412/robot/commands/climb/ClimbJoystickCommand.java +++ b/src/main/java/frc/team2412/robot/commands/climb/ClimbJoystickCommand.java @@ -18,11 +18,15 @@ public ClimbJoystickCommand(Joystick m_joystick, ClimbMotorSubsystem m_ClimbMoto public void execute() { double joystickValue = m_joystick.getY(); + if (Math.abs(joystickValue) < 0.1) { + joystickValue = 0; + } + m_ClimbMotorSubsystem.setMotors(joystickValue); } @Override public boolean isFinished() { - return true; + return false; } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java index c9afd25f..758b9296 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothUpCommand.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.team2412.robot.commands.intake.back.IntakeBackUpCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontUpCommand; -import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; import frc.team2412.robot.subsystems.IntakeUpDownSubsystem; public class IntakeBothUpCommand extends ParallelCommandGroup { diff --git a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java index d4199614..db6077f8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java @@ -11,6 +11,7 @@ import frc.team2412.robot.subsystems.constants.ClimbConstants; import frc.team2412.robot.subsystems.constants.ClimbConstants.ClimbHeight; import io.github.oblarg.oblog.Loggable; +import io.github.oblarg.oblog.annotations.Config; import io.github.oblarg.oblog.annotations.Log; public class ClimbMotorSubsystem extends SubsystemBase implements Loggable { @@ -24,6 +25,7 @@ public class ClimbMotorSubsystem extends SubsystemBase implements Loggable { private CANSparkMax m_rightClimbMotor; private CANEncoder m_encoder; + private CANEncoder m_leftEncoder; private CANPIDController m_pidController; @@ -33,10 +35,11 @@ public class ClimbMotorSubsystem extends SubsystemBase implements Loggable { public ClimbMotorSubsystem(CANSparkMax leftClimbMotor, CANSparkMax rightClimbMotor) { m_leftClimbMotor = leftClimbMotor; m_rightClimbMotor = rightClimbMotor; - m_leftClimbMotor.follow(rightClimbMotor); m_pidController = m_rightClimbMotor.getPIDController(); + m_encoder = m_rightClimbMotor.getEncoder(); + m_leftEncoder = m_leftClimbMotor.getEncoder(); m_pidController.setP(0.005); } @@ -70,8 +73,13 @@ public void periodic() { m_currentClimbHeight = new Distance(heightFromOffset).add(ClimbConstants.CLIMB_OFFSET_HEIGHT); } + @Config.NumberSlider public void setMotors(double value) { - m_rightClimbMotor.set(value); + m_rightClimbMotor.set(-value); + m_leftClimbMotor.set(value); + + System.out.println(m_encoder.getPosition()); + System.out.println(m_leftEncoder.getPosition()); RobotState.m_climbState = RobotState.ClimbState.CLIMBING; } diff --git a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java index 847271fd..bc5790ac 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java @@ -44,6 +44,7 @@ import edu.wpi.first.wpilibj2.command.button.Button; import frc.team2412.robot.RobotState; import io.github.oblarg.oblog.Loggable; +import io.github.oblarg.oblog.annotations.Config; import io.github.oblarg.oblog.annotations.Log; public class DriveBaseSubsystem extends SubsystemBase implements Loggable { @@ -68,8 +69,11 @@ public class DriveBaseSubsystem extends SubsystemBase implements Loggable { @SuppressWarnings("unused") private double m_rightMotorRevolutions, m_leftMotorRevolutions; - @Log.BooleanBox(colorWhenFalse = "#0000ff", colorWhenTrue = "#ffff00", tabName = "Drivebase Subsystem") - public boolean doItWork = false; + @Config.ToggleButton(name = "Toggle joystick drive", tabName = "Drivebase Subsystem") + private boolean oneJoystickDrive = false; + + @Log.BooleanBox(name = "In 1 joystick", tabName = "Drivebase Subsystem", colorWhenFalse = "red", colorWhenTrue = "green") + private boolean inOneJoystickDrive = oneJoystickDrive; // DifferentialDrive m_drive; @@ -104,6 +108,11 @@ public DriveBaseSubsystem(Solenoid gearShifter, Gyro gyro, WPI_TalonFX leftMotor } public void drive(Joystick rightJoystick, Joystick leftJoystick, Button button) { + if(oneJoystickDrive){ + m_rightMotor1.set(rightJoystick.getY()-Math.pow(rightJoystick.getTwist(),3)); + m_leftMotor1.set(rightJoystick.getY()+Math.pow(rightJoystick.getTwist(), 3)); + } else { + if (button.get()) { m_rightMotor1.set(rightJoystick.getY()); m_leftMotor1.set(rightJoystick.getY()); @@ -112,6 +121,7 @@ public void drive(Joystick rightJoystick, Joystick leftJoystick, Button button) m_leftMotor1.set(leftJoystick.getY()); } } + } public void oneJoystickDrive(Joystick joystick) { // m_drive.arcadeDrive(-1 * joystick.getY(), joystick.getTwist(), true); @@ -200,7 +210,7 @@ public DifferentialDriveWheelSpeeds getWheelSpeeds() { public void tankDriveVolts(double leftVolts, double rightVolts) { m_leftMotors.setVoltage(leftVolts); - m_rightMotors.setVoltage(-rightVolts); + m_rightMotors.setVoltage(rightVolts); // m_drive.feed(); } diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index 2b6b80eb..2692f316 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -14,10 +14,10 @@ public class HoodSubsystem extends SubsystemBase implements Loggable { - @Log(name = "Right Servo", tabName = "Hood", width = 8, height = 1, columnIndex = 0, rowIndex = 0) + @Log(name = "Right Servo", tabName = "Hood", width = 2, height = 1, columnIndex = 0, rowIndex = 0) private Servo m_hoodServo1; - @Log(name = "Left Servo", tabName = "Hood", width = 8, height = 1, columnIndex = 0, rowIndex = 1) + @Log(name = "Left Servo", tabName = "Hood", width = 2, height = 1, columnIndex = 0, rowIndex = 1) private Servo m_hoodServo2; public HoodSubsystem(Servo hoodServo1, Servo hoodServo2) { @@ -50,7 +50,6 @@ public void servoWithdraw() { @Config.NumberSlider(max = 0.65, min = 0, name = "Set Servo Angle", tabName = "Hood", width = 2, height = 1, columnIndex = 2, rowIndex = 0) public void setServo(double angle) { - System.out.println(angle); m_hoodServo2.set(angle); m_hoodServo1.set(1 - angle); } diff --git a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java index 82f0a030..b64c4cd2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java @@ -8,9 +8,6 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team2412.robot.commands.indexer.IndexIntakeBackCommandGroup; -import frc.team2412.robot.commands.indexer.IndexIntakeFrontCommandGroup; import frc.team2412.robot.subsystems.constants.IndexerConstants; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 2443549a..769fb8f7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -28,6 +28,7 @@ public class LimelightSubsystem extends SubsystemBase implements Loggable { public Rotations m_yawFromTarget; @Log.CameraStream(tabName = "Turret") + @Log.CameraStream(tabName = "Driver View") public HttpCamera limeCam; // Store the limelight diff --git a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java index faa700f3..25fc90bf 100644 --- a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java @@ -13,7 +13,6 @@ import com.robototes.units.UnitTypes.RotationUnits; import edu.wpi.first.wpilibj2.command.PIDSubsystem; -import frc.team2412.robot.commands.turret.TurretFollowLimelightCommand; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Config; import io.github.oblarg.oblog.annotations.Log; @@ -27,8 +26,10 @@ public class TurretSubsystem extends PIDSubsystem implements Loggable { @Log.Exclude private LimelightSubsystem m_limelightSubsystem; + private int m_turretOffsetPosition = 0; private int m_turretPastPosition; + @Log(tabName = "Turret") private int m_turretCurrentPosition; public TurretSubsystem(WPI_TalonSRX turretMotor, LimelightSubsystem limelightSubsystem) { @@ -38,7 +39,7 @@ public TurretSubsystem(WPI_TalonSRX turretMotor, LimelightSubsystem limelightSub initTurretEncoder(); - setDefaultCommand(new TurretFollowLimelightCommand(this, m_limelightSubsystem)); + // setDefaultCommand(new TurretFollowLimelightCommand(this, m_limelightSubsystem)); } public Rotations getCurrentAngle() { @@ -46,6 +47,7 @@ public Rotations getCurrentAngle() { } @Override + @Log(tabName = "Turret") public double getMeasurement() { return m_turretCurrentPosition - m_turretOffsetPosition; } @@ -66,7 +68,7 @@ public void initTurretEncoder() { @Override public void periodic() { - // m_turretCurrentPosition = m_turretMotor.getSelectedSensorPosition(0); + m_turretCurrentPosition = m_turretMotor.getSelectedSensorPosition(0); if (m_turretCurrentPosition - m_turretPastPosition > ENCODER_MAX_ERROR_JUMP) { m_turretOffsetPosition += TICKS_PER_REVOLUTION; @@ -81,7 +83,7 @@ public void periodic() { // System.out.println(getMeasurement()); } - @Config + @Config.NumberSlider(tabName = "Turret", min = -1, max = 1) public void set(double output) { output = MathUtils.constrain(output, -1, 1); diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/ClimbConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/ClimbConstants.java index 0c0e448f..adfa2c32 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/ClimbConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/ClimbConstants.java @@ -42,6 +42,6 @@ private ClimbState(boolean value) { public static final int MAX_SPEED = 1; public static final InterUnitRatio MOTOR_REVOLUTIONS_TO_INCHES = new InterUnitRatio( - RotationUnits.ROTATION, 1, DistanceUnits.INCH); - + RotationUnits.ROTATION, 14 * 1.273 * Math.PI, DistanceUnits.INCH); + public static final double MAX_EXTENSION = -70; } \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/HoodConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/HoodConstants.java index 3c7b2bda..155e5b5d 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/HoodConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/HoodConstants.java @@ -2,7 +2,7 @@ public class HoodConstants { - public static final double MAX_EXTENSION = 0.65; + public static final double MAX_EXTENSION = 0.6; public static final double MAX_WITHDRAWL = 0; public static final double MAX_WITHDRAWL_ANGLE = 90; diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java index acd13e26..c0408ae8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java @@ -38,7 +38,7 @@ public void flip() { public static final double INCH_STOP_DISTANCE = 2; public static final double MAX_SPEED = 0.3; - public static final double MAX_LIFT_SPEED = 0; + public static final double MAX_LIFT_SPEED = 0.9; public static final double TOP_TICKS = 20; public static final double BOTTOM_TICKS = 0; From 1e079c7f67abeccfe5cfde0cfb2cb064e30bfbf1 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Sat, 29 Feb 2020 15:25:45 -0800 Subject: [PATCH 06/11] Add changes for hood buttons, and some other things --- src/main/java/frc/team2412/robot/OI.java | 8 ++++++++ src/main/java/frc/team2412/robot/Robot.java | 2 +- src/main/java/frc/team2412/robot/RobotMap.java | 2 +- .../java/frc/team2412/robot/subsystems/HoodSubsystem.java | 1 + 4 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index e178fd09..dd166125 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -192,8 +192,16 @@ public OI(RobotContainer robotContainer) { .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); } + final double WALL = 0.1; + final double LINE = 0.31; + final double AUTO = 0.35; + Button[] hoodPresetButtons = { new JoystickButton(codriverManualStick, 2), + new JoystickButton(codriverManualStick, 1), new JoystickButton(codriverManualStick, 0) }; if (RobotMap.SHOOTER_CONNECTED) { + hoodPresetButtons[0].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(WALL))); + hoodPresetButtons[1].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(LINE))); + hoodPresetButtons[2].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(AUTO))); } if (RobotMap.CONTROL_PANEL_CONNECTED) { diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 15f0e496..8be1d309 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -101,7 +101,7 @@ public void autonomousInit() { .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setMidMotor(1))) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setBackMotor(-1))) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1))) - .andThen(new WaitCommand(10)) + .andThen(new WaitCommand(8)) .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(12, 12)) .andThen(new InstantCommand(() -> System.out.println("driving"))).andThen(new WaitCommand(0.5)) .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(0, 0)))); diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index e363370a..081dbe12 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -27,7 +27,7 @@ //remember to declare robot container at the bottom of this class public class RobotMap { - public static boolean CLIMB_CONNECTED = true; + public static boolean CLIMB_CONNECTED = false; public static boolean CONTROL_PANEL_CONNECTED = false; public static boolean SHOOTER_CONNECTED = true; public static boolean INDEX_CONNECTED = true; diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index 2692f316..3332e450 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -50,6 +50,7 @@ public void servoWithdraw() { @Config.NumberSlider(max = 0.65, min = 0, name = "Set Servo Angle", tabName = "Hood", width = 2, height = 1, columnIndex = 2, rowIndex = 0) public void setServo(double angle) { + angle = Math.min(0, HoodConstants.MAX_EXTENSION); m_hoodServo2.set(angle); m_hoodServo1.set(1 - angle); } From 5830bd890306a80797211e19ada5cb690754a43e Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Sat, 29 Feb 2020 15:27:17 -0800 Subject: [PATCH 07/11] Remove non-functional hood buttons --- src/main/java/frc/team2412/robot/OI.java | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index dd166125..7a48b899 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -192,17 +192,6 @@ public OI(RobotContainer robotContainer) { .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); } - final double WALL = 0.1; - final double LINE = 0.31; - final double AUTO = 0.35; - Button[] hoodPresetButtons = { new JoystickButton(codriverManualStick, 2), - new JoystickButton(codriverManualStick, 1), new JoystickButton(codriverManualStick, 0) }; - - if (RobotMap.SHOOTER_CONNECTED) { - hoodPresetButtons[0].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(WALL))); - hoodPresetButtons[1].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(LINE))); - hoodPresetButtons[2].whenPressed(new InstantCommand(() -> robotContainer.m_hoodSubsystem.setServo(AUTO))); - } if (RobotMap.CONTROL_PANEL_CONNECTED) { // CONTROL PANEL From 6ffd915712527be39158ce41375e408338161aa4 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Sat, 29 Feb 2020 19:03:55 -0800 Subject: [PATCH 08/11] code for GP --DO NOT CHANGE WITHOUT TESTING-- --- src/main/java/frc/team2412/robot/OI.java | 31 ++++++++++++++++--- src/main/java/frc/team2412/robot/Robot.java | 6 ++-- .../robot/subsystems/HoodSubsystem.java | 2 +- 3 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index 7a48b899..aa0d01f3 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -165,11 +165,11 @@ public OI(RobotContainer robotContainer) { // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, // robotContainer.m_intakeMotorOnOffSubsystem)); - frontIntakeUpDown.whenPressed(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); - frontIntakeUpDown.whenReleased(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); + frontIntakeUpDown.whenReleased(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); + frontIntakeUpDown.whenPressed(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); - backIntakeUpDown.whenPressed(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); - backIntakeUpDown.whenReleased(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); + backIntakeUpDown.whenReleased(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); + backIntakeUpDown.whenPressed(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); intakeFrontIn.whenPressed(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1)))); @@ -190,7 +190,28 @@ public OI(RobotContainer robotContainer) { .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(1)))); intakeBackOut.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); - + /*intakeFrontIn.whenPressed(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> { + if(robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1); + else + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); + if(!robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + robotContainer.m_indexerMotorSubsystem.setBackMotor(1); + else + robotContainer.m_indexerMotorSubsystem.setBackMotor(0); + }))); + intakeBackIn.whenPressed(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> { + if(robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + robotContainer.m_indexerMotorSubsystem.setBackMotor(-1); + else + robotContainer.m_indexerMotorSubsystem.setBackMotor(0); + if(!robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setFrontMotor(1); + else + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); + })));*/ } if (RobotMap.CONTROL_PANEL_CONNECTED) { diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 8be1d309..4aed9a4f 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -95,7 +95,7 @@ public void autonomousInit() { */ autoCommand = new HoodWithdrawCommand(m_robotContainer.m_hoodSubsystem) - .andThen(new HoodAdjustCommand(m_robotContainer.m_hoodSubsystem, .310)) + .andThen(new HoodAdjustCommand(m_robotContainer.m_hoodSubsystem, .330)) .andThen(new InstantCommand(() -> m_robotContainer.m_flywheelSubsystem.setSpeed(-0.9))) .andThen(new WaitCommand(2)) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setMidMotor(1))) @@ -103,7 +103,7 @@ public void autonomousInit() { .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1))) .andThen(new WaitCommand(8)) .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(12, 12)) - .andThen(new InstantCommand(() -> System.out.println("driving"))).andThen(new WaitCommand(0.5)) + .andThen(new InstantCommand(() -> System.out.println("driving"))).andThen(new WaitCommand(1.5)) .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(0, 0)))); CommandScheduler.getInstance().schedule(autoCommand); } @@ -123,6 +123,8 @@ public void autonomousPeriodic() { public void teleopInit() { timeRemaining = 135.0; CommandScheduler.getInstance().cancel(autoCommand); + CommandScheduler.getInstance() + .schedule(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.stopAllMotors())); m_robotContainer.m_LimelightSubsystem.stopLimelight(); } diff --git a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java index 3332e450..f058b5e4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/HoodSubsystem.java @@ -50,7 +50,7 @@ public void servoWithdraw() { @Config.NumberSlider(max = 0.65, min = 0, name = "Set Servo Angle", tabName = "Hood", width = 2, height = 1, columnIndex = 2, rowIndex = 0) public void setServo(double angle) { - angle = Math.min(0, HoodConstants.MAX_EXTENSION); + angle = Math.min(angle, HoodConstants.MAX_EXTENSION); m_hoodServo2.set(angle); m_hoodServo1.set(1 - angle); } From 78770435338d4f788ddd6607ac8ca83b0bd017af Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Sun, 1 Mar 2020 14:20:02 -0800 Subject: [PATCH 09/11] final gp code --- src/main/java/frc/team2412/robot/Robot.java | 8 ++++---- .../java/frc/team2412/robot/RobotMap.java | 3 ++- .../robot/subsystems/ClimbMotorSubsystem.java | 20 ++++++++++++++----- .../constants/IndexerConstants.java | 2 +- 4 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 4aed9a4f..030c2937 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -95,16 +95,16 @@ public void autonomousInit() { */ autoCommand = new HoodWithdrawCommand(m_robotContainer.m_hoodSubsystem) - .andThen(new HoodAdjustCommand(m_robotContainer.m_hoodSubsystem, .330)) + .andThen(new HoodAdjustCommand(m_robotContainer.m_hoodSubsystem, .300)) .andThen(new InstantCommand(() -> m_robotContainer.m_flywheelSubsystem.setSpeed(-0.9))) .andThen(new WaitCommand(2)) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setMidMotor(1))) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setBackMotor(-1))) .andThen(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1))) .andThen(new WaitCommand(8)) - .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(12, 12)) - .andThen(new InstantCommand(() -> System.out.println("driving"))).andThen(new WaitCommand(1.5)) - .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(0, 0)))); + .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(-12, -12))) + .andThen(new WaitCommand(1)) + .andThen(new InstantCommand(() -> m_robotContainer.m_driveBaseSubsystem.tankDriveVolts(0, 0))); CommandScheduler.getInstance().schedule(autoCommand); } diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index 081dbe12..c798ac4b 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -27,7 +27,7 @@ //remember to declare robot container at the bottom of this class public class RobotMap { - public static boolean CLIMB_CONNECTED = false; + public static boolean CLIMB_CONNECTED = true; public static boolean CONTROL_PANEL_CONNECTED = false; public static boolean SHOOTER_CONNECTED = true; public static boolean INDEX_CONNECTED = true; @@ -134,6 +134,7 @@ public int getIndexCANID() { public static CANSparkMax rightClimbMotor = CLIMB_CONNECTED ? new CANSparkMax(CANBus.CLIMB2.id, MotorType.kBrushless) : null; + // INDEX SUBSYSTEM // --------------------------------------------------------------------------- diff --git a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java index db6077f8..2cae5c43 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java @@ -75,11 +75,21 @@ public void periodic() { @Config.NumberSlider public void setMotors(double value) { - m_rightClimbMotor.set(-value); - m_leftClimbMotor.set(value); - - System.out.println(m_encoder.getPosition()); - System.out.println(m_leftEncoder.getPosition()); + // left : 0-75 + // right: -75 - 0 + if (value < 0 && m_encoder.getPosition() < 0 || value > 0 && m_encoder.getPosition() > -76) { + m_rightClimbMotor.set(-value); + } else { + m_rightClimbMotor.set(0); + } + + if (value < 0 && m_leftEncoder.getPosition() > 0 || value > 0 && m_leftEncoder.getPosition() < 76) { + m_leftClimbMotor.set(value); + }else{ + m_leftClimbMotor.set(0); + } + System.out.println("right: " + m_encoder.getPosition()); + System.out.println("left: " + m_leftEncoder.getPosition()); RobotState.m_climbState = RobotState.ClimbState.CLIMBING; } diff --git a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java index c0408ae8..27731397 100644 --- a/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java +++ b/src/main/java/frc/team2412/robot/subsystems/constants/IndexerConstants.java @@ -38,7 +38,7 @@ public void flip() { public static final double INCH_STOP_DISTANCE = 2; public static final double MAX_SPEED = 0.3; - public static final double MAX_LIFT_SPEED = 0.9; + public static final double MAX_LIFT_SPEED = 0.75; public static final double TOP_TICKS = 20; public static final double BOTTOM_TICKS = 0; From 9154d3a1b8f9a89a9693c371e116237a852a9628 Mon Sep 17 00:00:00 2001 From: Robototes programmers Date: Mon, 2 Mar 2020 16:38:13 -0800 Subject: [PATCH 10/11] Committing indexer changes made after Glacier Peak. --- src/main/java/frc/team2412/robot/Logging.java | 2 +- src/main/java/frc/team2412/robot/OI.java | 59 +++++++++++-------- src/main/java/frc/team2412/robot/Robot.java | 6 +- .../java/frc/team2412/robot/RobotMap.java | 8 +-- .../java/frc/team2412/robot/RobotState.java | 2 +- .../auto/AAHEliMemeAutoCommandGroup.java | 2 +- .../auto/EliMemeAutoCommandGroup.java | 2 +- .../auto/SixBallAutoCommandGroup.java | 4 +- .../auto/ThreeBallAutoCommandGroup.java | 2 +- .../indexer/IndexBackShootCommand.java | 35 +++++++++++ .../indexer/IndexFrontShootCommand.java | 35 +++++++++++ .../commands/indexer/IndexShootCommand.java | 52 ++++------------ .../subsystems/IndexerMotorSubsystem.java | 31 +++++++--- 13 files changed, 156 insertions(+), 84 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java create mode 100644 src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java diff --git a/src/main/java/frc/team2412/robot/Logging.java b/src/main/java/frc/team2412/robot/Logging.java index ce9e123b..a4db27e4 100644 --- a/src/main/java/frc/team2412/robot/Logging.java +++ b/src/main/java/frc/team2412/robot/Logging.java @@ -99,7 +99,7 @@ public void periodic() { // double time = m_robot.timeRemaining; // timer = time / 60 + " : " + time % 60; - driveBaseCurrentDraw = RobotMap.m_robotContainer.m_driveBaseSubsystem.getCurrentDraw(); + //driveBaseCurrentDraw = RobotMap.m_robotContainer.m_driveBaseSubsystem.getCurrentDraw(); } public void initSendable(SendableBuilder builder){ diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index aa0d01f3..01c51b59 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -171,47 +173,53 @@ public OI(RobotContainer robotContainer) { backIntakeUpDown.whenReleased(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); backIntakeUpDown.whenPressed(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); - intakeFrontIn.whenPressed(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1)))); + // intakeFrontIn.whenPressed(new + // IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + // .andThen(new InstantCommand(() -> + // robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1)))); intakeFrontIn.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); intakeFrontOut.whenPressed(new IntakeFrontOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(1)))); intakeFrontOut.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); - intakeBackIn.whenPressed(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(-1)))); + // intakeBackIn.whenPressed(new + // IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + // .andThen(new InstantCommand(() -> + // robotContainer.m_indexerMotorSubsystem.setBackMotor(-1)))); intakeBackIn.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); intakeBackOut.whenPressed(new IntakeBackOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(1)))); intakeBackOut.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); - /*intakeFrontIn.whenPressed(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + intakeFrontIn.whileHeld(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) .andThen(new InstantCommand(() -> { - if(robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + if (robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1); else robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); - if(!robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + if (!robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) robotContainer.m_indexerMotorSubsystem.setBackMotor(1); else robotContainer.m_indexerMotorSubsystem.setBackMotor(0); - }))); - intakeBackIn.whenPressed(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> { - if(robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) - robotContainer.m_indexerMotorSubsystem.setBackMotor(-1); - else - robotContainer.m_indexerMotorSubsystem.setBackMotor(0); - if(!robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) - robotContainer.m_indexerMotorSubsystem.setFrontMotor(1); - else - robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); - })));*/ + }))); + intakeBackIn.whileHeld(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> { + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + if (robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + robotContainer.m_indexerMotorSubsystem.setBackMotor(-1); + else + robotContainer.m_indexerMotorSubsystem.setBackMotor(0); + if (!robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setFrontMotor(1); + else + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); + }))); } if (RobotMap.CONTROL_PANEL_CONNECTED) { @@ -245,8 +253,13 @@ public OI(RobotContainer robotContainer) { indexerSpitButton.whileHeld(new IndexSpitCommand(robotContainer.m_indexerSensorSubsystem, robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem)); - indexerShootButton.whileHeld(new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, - robotContainer.m_indexerMotorSubsystem)); + Command indexShootCommand = new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, + robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem); + + indexerShootButton.whenPressed(indexShootCommand); + + indexerShootButton + .whenReleased(new InstantCommand(() -> CommandScheduler.getInstance().cancel(indexShootCommand))); } diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 030c2937..1c92a592 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -122,10 +122,11 @@ public void autonomousPeriodic() { @Override public void teleopInit() { timeRemaining = 135.0; - CommandScheduler.getInstance().cancel(autoCommand); + //CommandScheduler.getInstance().cancel(autoCommand); CommandScheduler.getInstance() .schedule(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.stopAllMotors())); - m_robotContainer.m_LimelightSubsystem.stopLimelight(); + m_robotContainer.m_flywheelSubsystem.setSpeed(-0.25); + //m_robotContainer.m_LimelightSubsystem.stopLimelight(); } /** @@ -137,6 +138,7 @@ public void teleopPeriodic() { double val = m_OI.codriverStick.getY() * 0.5 + 0.5; m_robotContainer.m_hoodSubsystem.setServo(val); + } @Override diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index c798ac4b..f8e9a0bc 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -27,13 +27,13 @@ //remember to declare robot container at the bottom of this class public class RobotMap { - public static boolean CLIMB_CONNECTED = true; + public static boolean CLIMB_CONNECTED = false; public static boolean CONTROL_PANEL_CONNECTED = false; public static boolean SHOOTER_CONNECTED = true; public static boolean INDEX_CONNECTED = true; public static boolean INTAKE_CONNECTED = true; public static boolean LIFT_CONNECTED = true; - public static boolean DRIVE_BASE_CONNECTED = true; + public static boolean DRIVE_BASE_CONNECTED = false; public static enum CANBus { INTAKE1(11), INDEX1(12), INTAKE2(21), INDEX2(22), INTAKE3(31), INDEX3(32), INDEX_MID(40), DRIVE_LEFT_FRONT(3), @@ -58,8 +58,8 @@ private PneumaticPort(int pneumaticPortId) { } public static enum DIOPort { - BACK_SENSOR(5), BACK_MID_SENSOR(6), BACK_INNER_SENSOR(7), FRONT_INNER_SENSOR(3), FRONT_MID_SENSOR(2), - FRONT_SENSOR(1), INTAKE_FRONT_SENSOR(0), INTAKE_BACK_SENSOR(4); + FRONT_SENSOR(5), FRONT_MID_SENSOR(6), FRONT_INNER_SENSOR(7), BACK_INNER_SENSOR(3), BACK_MID_SENSOR(2), + BACK_SENSOR(1), INTAKE_BACK_SENSOR(0), INTAKE_FRONT_SENSOR(4); public final int id; diff --git a/src/main/java/frc/team2412/robot/RobotState.java b/src/main/java/frc/team2412/robot/RobotState.java index 6263656e..1bfd69f4 100644 --- a/src/main/java/frc/team2412/robot/RobotState.java +++ b/src/main/java/frc/team2412/robot/RobotState.java @@ -12,7 +12,7 @@ public class RobotState implements Loggable { private RobotContainer m_robotContainer; @Log.ToString(tabName = "Robot State") - public static UnbalancedSide m_unbalancedSide; + public static UnbalancedSide m_unbalancedSide = UnbalancedSide.FRONT; @Log.Dial(min = 0, max = 5, showValue = true, name = "Power Cell Count", tabName = "Robot State") public static int m_ballCount = 0; diff --git a/src/main/java/frc/team2412/robot/commands/auto/AAHEliMemeAutoCommandGroup.java b/src/main/java/frc/team2412/robot/commands/auto/AAHEliMemeAutoCommandGroup.java index d31412bf..7e9f0f53 100644 --- a/src/main/java/frc/team2412/robot/commands/auto/AAHEliMemeAutoCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/auto/AAHEliMemeAutoCommandGroup.java @@ -23,7 +23,7 @@ public AAHEliMemeAutoCommandGroup(DriveBaseSubsystem driveBaseSubsystem, IntakeO addCommands( new StartUpCommand(liftSubsystem, flywheelSubsystem, hoodSubsystem, indexerMotorSubsystem), - new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem), + new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem, intakeOnOffSubsystem), new MoveToPowerCellsCommand(driveBaseSubsystem) ); diff --git a/src/main/java/frc/team2412/robot/commands/auto/EliMemeAutoCommandGroup.java b/src/main/java/frc/team2412/robot/commands/auto/EliMemeAutoCommandGroup.java index 26d18fd5..1e9a82e9 100644 --- a/src/main/java/frc/team2412/robot/commands/auto/EliMemeAutoCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/auto/EliMemeAutoCommandGroup.java @@ -23,7 +23,7 @@ public EliMemeAutoCommandGroup(DriveBaseSubsystem driveBaseSubsystem, IntakeOnOf addCommands( new StartUpCommand(liftSubsystem, flywheelSubsystem, hoodSubsystem, indexerMotorSubsystem), - new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem), + new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem, intakeOnOffSubsystem), new MoveToPowerCellsCommand(driveBaseSubsystem) ); diff --git a/src/main/java/frc/team2412/robot/commands/auto/SixBallAutoCommandGroup.java b/src/main/java/frc/team2412/robot/commands/auto/SixBallAutoCommandGroup.java index d9aadfea..120160d4 100644 --- a/src/main/java/frc/team2412/robot/commands/auto/SixBallAutoCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/auto/SixBallAutoCommandGroup.java @@ -24,10 +24,10 @@ public SixBallAutoCommandGroup(DriveBaseSubsystem driveBaseSubsystem, IntakeOnOf addCommands( new LiftUpCommand(liftSubsystem, indexerMotorSubsystem), - new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem), + new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem, intakeOnOffSubsystem), new MoveToIntakePowerCellsCommandGroup(driveBaseSubsystem, intakeOnOffSubsystem, intakeUpDownSubsystem), new MoveFromPowerCellsCommand(driveBaseSubsystem), - new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem)); + new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem, intakeOnOffSubsystem)); } } diff --git a/src/main/java/frc/team2412/robot/commands/auto/ThreeBallAutoCommandGroup.java b/src/main/java/frc/team2412/robot/commands/auto/ThreeBallAutoCommandGroup.java index ab6056e0..a0bb1bd0 100644 --- a/src/main/java/frc/team2412/robot/commands/auto/ThreeBallAutoCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/auto/ThreeBallAutoCommandGroup.java @@ -23,7 +23,7 @@ public ThreeBallAutoCommandGroup(DriveBaseSubsystem driveBaseSubsystem, IntakeOn addCommands( new LiftUpCommand(liftSubsystem, indexerMotorSubsystem), - new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem), + new IndexShootCommand(indexerSensorSubsystem, indexerMotorSubsystem, intakeOnOffSubsystem), new MoveToIntakePowerCellsCommandGroup(driveBaseSubsystem, intakeOnOffSubsystem, intakeUpDownSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java new file mode 100644 index 00000000..1af0a0fa --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java @@ -0,0 +1,35 @@ +package frc.team2412.robot.commands.indexer; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.RobotState; +import frc.team2412.robot.subsystems.IndexerMotorSubsystem; +import frc.team2412.robot.subsystems.IndexerSensorSubsystem; +import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; + +//This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor +public class IndexBackShootCommand extends CommandBase { + + private IndexerSensorSubsystem m_indexerSensorSubsystem; + private IndexerMotorSubsystem m_indexerMotorSubsystem; + private IntakeOnOffSubsystem m_IntakeOnOffSubsystem; + + public IndexBackShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { + m_indexerSensorSubsystem = sensorSubsystem; + m_indexerMotorSubsystem = motorSubsystem; + m_IntakeOnOffSubsystem = intakeOnOffSubsystem; + addRequirements(sensorSubsystem, motorSubsystem); + } + + @Override + public void execute() { + m_indexerMotorSubsystem.setMidMotor(1); + m_indexerMotorSubsystem.setBackMotor(-1); + m_IntakeOnOffSubsystem.backIntakeIn(); + } + + @Override + public boolean isFinished() { + return true; + } + +} diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java new file mode 100644 index 00000000..91f78f00 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java @@ -0,0 +1,35 @@ +package frc.team2412.robot.commands.indexer; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.RobotState; +import frc.team2412.robot.subsystems.IndexerMotorSubsystem; +import frc.team2412.robot.subsystems.IndexerSensorSubsystem; +import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; + +//This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor +public class IndexFrontShootCommand extends CommandBase { + + private IndexerSensorSubsystem m_indexerSensorSubsystem; + private IndexerMotorSubsystem m_indexerMotorSubsystem; + private IntakeOnOffSubsystem m_IntakeOnOffSubsystem; + + public IndexFrontShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { + m_indexerSensorSubsystem = sensorSubsystem; + m_indexerMotorSubsystem = motorSubsystem; + m_IntakeOnOffSubsystem = intakeOnOffSubsystem; + addRequirements(sensorSubsystem, motorSubsystem); + } + + @Override + public void execute() { + m_indexerMotorSubsystem.setMidMotor(1); + m_indexerMotorSubsystem.setFrontMotor(-1); + m_IntakeOnOffSubsystem.frontIntakeIn(); + } + + @Override + public boolean isFinished() { + return true; + } + +} diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java index 5fe9215c..dde7f95c 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java @@ -1,59 +1,31 @@ package frc.team2412.robot.commands.indexer; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team2412.robot.RobotState; import frc.team2412.robot.subsystems.IndexerMotorSubsystem; import frc.team2412.robot.subsystems.IndexerSensorSubsystem; +import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; //This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor -public class IndexShootCommand extends CommandBase { +public class IndexShootCommand extends SequentialCommandGroup { - private IndexerSensorSubsystem m_indexerSensorSubsystem; private IndexerMotorSubsystem m_indexerMotorSubsystem; + private IntakeOnOffSubsystem m_intakeOnOffSubsystem; - public IndexShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem) { - m_indexerSensorSubsystem = sensorSubsystem; + public IndexShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeSubsystem) { m_indexerMotorSubsystem = motorSubsystem; - addRequirements(sensorSubsystem, motorSubsystem); - } - - @Override - public void execute() { - m_indexerMotorSubsystem.setMidMotor(1); - // if (!m_indexerSensorSubsystem.getIndexBackInnerSensorValue() - // && !m_indexerSensorSubsystem.getIndexFrontInnerSensorValue()) { - // if (RobotState.m_unbalancedSide == RobotState.UnbalancedSide.FRONT) { - // m_indexerMotorSubsystem.setFrontMotor(1); - // if (m_indexerSensorSubsystem.allFrontSensorsOff()) { - // m_indexerMotorSubsystem.setBackMotor(1); - // } - // } else { - // m_indexerMotorSubsystem.setBackMotor(1); - // if (m_indexerSensorSubsystem.allBackSensorsOff()) { - // m_indexerMotorSubsystem.setFrontMotor(1); - // } - // } - // } + m_intakeOnOffSubsystem = intakeSubsystem; + m_indexerMotorSubsystem.setLifting(true); + addCommands(new IndexFrontShootCommand(sensorSubsystem, motorSubsystem, intakeSubsystem), new WaitCommand(3), + new IndexBackShootCommand(sensorSubsystem, motorSubsystem, intakeSubsystem), new WaitCommand(2)); } @Override public void end(boolean cancel) { + m_indexerMotorSubsystem.setLifting(false); m_indexerMotorSubsystem.stopAllMotors(); - RobotState.m_ballCount = 0; - } - - @Override - public boolean isFinished() { - if (m_indexerSensorSubsystem.allInnerSensorsOff()) { - // return true; - - } else { - RobotState.m_ballCount = m_indexerSensorSubsystem.totalSensorsOn(); - return false; - } - - return false; - + m_intakeOnOffSubsystem.setIntake(0); } - } diff --git a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java index b64c4cd2..e72ea407 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java @@ -29,6 +29,8 @@ public class IndexerMotorSubsystem extends SubsystemBase implements Loggable { private SpeedControllerGroup m_allMotors; private SpeedControllerGroup m_sideMotors; + private boolean lifting; + public IndexerMotorSubsystem(CANSparkMax frontMotor, CANSparkMax midMotor, CANSparkMax backMotor, IndexerSensorSubsystem indexerSensorSubsystem) { m_indexFrontMotor = frontMotor; @@ -50,11 +52,15 @@ public IndexerMotorSubsystem(CANSparkMax frontMotor, CANSparkMax midMotor, CANSp m_sideMotors = new SpeedControllerGroup(m_indexFrontMotor, m_indexBackMotor); m_allMotors = new SpeedControllerGroup(m_indexFrontMotor, m_indexMidMotor, m_indexBackMotor); - // Trigger frontProcess = new Trigger(indexerSensorSubsystem::getIntakeFrontSensorValue); - // frontProcess.whenActive(new IndexIntakeFrontCommandGroup(indexerSensorSubsystem, this), true); + // Trigger frontProcess = new + // Trigger(indexerSensorSubsystem::getIntakeFrontSensorValue); + // frontProcess.whenActive(new + // IndexIntakeFrontCommandGroup(indexerSensorSubsystem, this), true); - // Trigger backProcess = new Trigger(indexerSensorSubsystem::getIntakeBackSensorValue); - // backProcess.whenActive(new IndexIntakeBackCommandGroup(indexerSensorSubsystem, this), true); + // Trigger backProcess = new + // Trigger(indexerSensorSubsystem::getIntakeBackSensorValue); + // backProcess.whenActive(new + // IndexIntakeBackCommandGroup(indexerSensorSubsystem, this), true); midTicks = m_midEncoder.getPosition(); } @@ -72,7 +78,8 @@ public void setFrontMotor(double val) { public void setMidMotor(double val) { val = MathUtils.constrain(val, -IndexerConstants.MAX_LIFT_SPEED, IndexerConstants.MAX_LIFT_SPEED); - m_indexMidMotor.set(val); + m_indexMidMotor.set(lifting ? IndexerConstants.MAX_LIFT_SPEED : val); + System.out.println(m_indexMidMotor.get()); } public void setBackMotor(double val) { @@ -119,10 +126,12 @@ public void stopBackPID(double val) { public void setMidPID(boolean upOrDown) { return; // if (upOrDown) { - - // m_midPIDController.setReference(midTicks + IndexerConstants.TOP_TICKS, ControlType.kPosition); + + // m_midPIDController.setReference(midTicks + IndexerConstants.TOP_TICKS, + // ControlType.kPosition); // } else { - // m_midPIDController.setReference(midTicks + IndexerConstants.BOTTOM_TICKS, ControlType.kPosition); + // m_midPIDController.setReference(midTicks + IndexerConstants.BOTTOM_TICKS, + // ControlType.kPosition); // } } @@ -139,7 +148,13 @@ public double getCurrentDraw() { @Override public void periodic() { + System.out.println(m_indexMidMotor.get()); + } + + + public void setLifting(boolean b) { + this.lifting = b; } } From 34c7e06e1275084cbbad03aa7529b3b5f940bada Mon Sep 17 00:00:00 2001 From: s-oronae Date: Tue, 3 Mar 2020 16:15:33 -0800 Subject: [PATCH 11/11] More refactoring --- src/main/java/frc/team2412/robot/Logging.java | 9 +- src/main/java/frc/team2412/robot/OI.java | 347 +++++++++--------- src/main/java/frc/team2412/robot/Robot.java | 19 +- .../frc/team2412/robot/RobotContainer.java | 4 +- .../java/frc/team2412/robot/RobotMap.java | 1 - .../commands/hood/HoodAdjustCommand.java | 29 +- .../commands/hood/HoodJoystickCommand.java | 27 ++ .../indexer/IndexBackShootCommand.java | 12 +- .../indexer/IndexFrontShootCommand.java | 13 +- .../commands/indexer/IndexShootCommand.java | 20 +- .../commands/indexer/IndexSpitCommand.java | 11 +- .../intake/IntakeBothInCommandGroup.java | 3 +- .../intake/IntakeBothOffCommandGroup.java | 3 +- .../intake/IntakeBothOutCommandGroup.java | 3 +- .../robot/subsystems/ClimbMotorSubsystem.java | 2 +- .../robot/subsystems/DriveBaseSubsystem.java | 22 +- .../subsystems/IndexerMotorSubsystem.java | 3 +- .../robot/subsystems/TurretSubsystem.java | 3 +- 18 files changed, 269 insertions(+), 262 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/hood/HoodJoystickCommand.java diff --git a/src/main/java/frc/team2412/robot/Logging.java b/src/main/java/frc/team2412/robot/Logging.java index a4db27e4..a151d94c 100644 --- a/src/main/java/frc/team2412/robot/Logging.java +++ b/src/main/java/frc/team2412/robot/Logging.java @@ -48,7 +48,8 @@ public class Logging implements Loggable, Sendable { // Dial Boolean @Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 7, rowIndex = 1, tabName = "Driver View") - //@Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 9, rowIndex = 1, tabName = "Driver View") + // @Log.BooleanBox(colorWhenFalse = green, colorWhenTrue = red, columnIndex = 9, + // rowIndex = 1, tabName = "Driver View") public boolean brownoutWarning = false; // Timer @@ -99,10 +100,12 @@ public void periodic() { // double time = m_robot.timeRemaining; // timer = time / 60 + " : " + time % 60; - //driveBaseCurrentDraw = RobotMap.m_robotContainer.m_driveBaseSubsystem.getCurrentDraw(); + // driveBaseCurrentDraw = + // RobotMap.m_robotContainer.m_driveBaseSubsystem.getCurrentDraw(); } - public void initSendable(SendableBuilder builder){ + @Override + public void initSendable(SendableBuilder builder) { } diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java index 01c51b59..09870bad 100644 --- a/src/main/java/frc/team2412/robot/OI.java +++ b/src/main/java/frc/team2412/robot/OI.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team2412.robot.commands.climb.ClimbDeployRailsCommand; -import frc.team2412.robot.commands.climb.ClimbRetractRailsCommand; import frc.team2412.robot.commands.climb.ClimbJoystickCommand; +import frc.team2412.robot.commands.climb.ClimbRetractRailsCommand; import frc.team2412.robot.commands.drive.DriveCommand; import frc.team2412.robot.commands.drive.DriveShiftToHighGearCommand; import frc.team2412.robot.commands.drive.DriveShiftToLowGearCommand; @@ -18,19 +18,47 @@ import frc.team2412.robot.commands.intake.back.IntakeBackDownCommand; import frc.team2412.robot.commands.intake.back.IntakeBackInCommand; import frc.team2412.robot.commands.intake.back.IntakeBackOffCommand; +import frc.team2412.robot.commands.intake.back.IntakeBackOutCommand; import frc.team2412.robot.commands.intake.back.IntakeBackUpCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontDownCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontInCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontOffCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontOutCommand; import frc.team2412.robot.commands.intake.front.IntakeFrontUpCommand; -import frc.team2412.robot.commands.intake.back.IntakeBackOutCommand; import frc.team2412.robot.commands.lift.LiftDownCommand; import frc.team2412.robot.commands.lift.LiftUpCommand; //This is the class in charge of all the buttons and joysticks that the drivers will use to control the robot public class OI { + public static interface ButtonEnumInterface { + + public int getButtonID(); + + public int getJoystickID(); + + public default Button createFrom(Joystick stick) { + if (stick.getPort() != getJoystickID()) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new JoystickButton(stick, getButtonID()); + } + + public default Button createFrom(XboxController controller) { + if (controller.getPort() != getJoystickID()) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getRawButton(getButtonID())); + } + + public default Button createFromPOV(XboxController controller) { + if (controller.getPort() != getJoystickID()) { + System.err.println("Warning! Binding button to the wrong stick!"); + } + return new Button(() -> controller.getPOV() == getButtonID()); + } + } + public enum Joysticks { DRIVER_RIGHT(0), DRIVER_LEFT(1), CODRIVER(2), CODRIVER_MANUAL(3); @@ -41,7 +69,7 @@ private Joysticks(int id) { } } - public enum DriverControls { + public enum DriverControls implements ButtonEnumInterface { SHOOT(Joysticks.DRIVER_RIGHT, 1), SHIFT(Joysticks.DRIVER_RIGHT, 2), SPIT(Joysticks.DRIVER_LEFT, 1), ALIGN_STICKS(Joysticks.DRIVER_LEFT, 3); @@ -53,15 +81,19 @@ private DriverControls(Joysticks stick, int buttonID) { this.buttonID = buttonID; } - public Button createFrom(Joystick stick) { - if (stick.getPort() != this.stick.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new JoystickButton(stick, this.buttonID); + @Override + public int getButtonID() { + return this.buttonID; } + + @Override + public int getJoystickID() { + return stick.id; + } + } - public enum CodriverControls { + public enum CodriverControls implements ButtonEnumInterface { LIFT(7), FRONT_INTAKE_DOWN(2), BACK_INTAKE_DOWN(1), INTAKE_BACK_IN(4), INTAKE_BACK_OUT(3), INTAKE_FRONT_IN(6), INTAKE_FRONT_OUT(5); @@ -71,29 +103,19 @@ private CodriverControls(int buttonID) { this.buttonID = buttonID; } - public Button createFrom(Joystick stick) { - if (stick.getPort() != Joysticks.CODRIVER.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new JoystickButton(stick, this.buttonID); + @Override + public int getButtonID() { + return this.buttonID; } - public Button createFrom(XboxController controller) { - if (controller.getPort() != Joysticks.CODRIVER.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new Button(() -> controller.getRawButton(this.buttonID)); + @Override + public int getJoystickID() { + return Joysticks.CODRIVER.id; } - public Button createFromPOV(XboxController controller) { - if (controller.getPort() != Joysticks.CODRIVER.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new Button(() -> controller.getPOV() == this.buttonID); - } } - public enum CodriverManualControls { + public enum CodriverManualControls implements ButtonEnumInterface { CLIMB_MODE(5); public int buttonID; @@ -102,188 +124,157 @@ private CodriverManualControls(int buttonID) { this.buttonID = buttonID; } - public Button createFrom(Joystick stick) { - if (stick.getPort() != Joysticks.CODRIVER_MANUAL.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new JoystickButton(stick, this.buttonID); + @Override + public int getButtonID() { + return this.buttonID; } - public Button createFrom(XboxController controller) { - if (controller.getPort() != Joysticks.CODRIVER_MANUAL.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new Button(() -> controller.getRawButton(this.buttonID)); + @Override + public int getJoystickID() { + return Joysticks.CODRIVER_MANUAL.id; } - public Button createFromPOV(XboxController controller) { - if (controller.getPort() != Joysticks.CODRIVER_MANUAL.id) { - System.err.println("Warning! Binding button to the wrong stick!"); - } - return new Button(() -> controller.getPOV() == this.buttonID); - } } - Joystick driverRightStick = new Joystick(Joysticks.DRIVER_RIGHT.id); - Joystick driverLeftStick = new Joystick(Joysticks.DRIVER_LEFT.id); - Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); - Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); - - public Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); - - // Buttons - public Button indexerShootButton = DriverControls.SHOOT.createFrom(driverRightStick); - public Button indexerSpitButton = DriverControls.SPIT.createFrom(driverLeftStick); + // Joysticks + public final Joystick driverRightStick = new Joystick(Joysticks.DRIVER_RIGHT.id); + public final Joystick driverLeftStick = new Joystick(Joysticks.DRIVER_LEFT.id); + public final Joystick codriverStick = new Joystick(Joysticks.CODRIVER.id); + public final Joystick codriverManualStick = new Joystick(Joysticks.CODRIVER_MANUAL.id); - public Button liftButton = CodriverControls.LIFT.createFrom(codriverStick); + // Driver Controls + public final Button shifter = DriverControls.SHIFT.createFrom(driverRightStick); + public final Button indexerShootButton = DriverControls.SHOOT.createFrom(driverRightStick); + public final Button indexerSpitButton = DriverControls.SPIT.createFrom(driverLeftStick); - public Button frontIntakeUpDown = CodriverControls.FRONT_INTAKE_DOWN.createFrom(codriverStick); - public Button backIntakeUpDown = CodriverControls.BACK_INTAKE_DOWN.createFrom(codriverStick); + // Lift Controls + public final Button liftButton = CodriverControls.LIFT.createFrom(codriverStick); - public Button intakeFrontIn = CodriverControls.INTAKE_FRONT_IN.createFrom(codriverStick); - public Button intakeFrontOut = CodriverControls.INTAKE_FRONT_OUT.createFrom(codriverStick); - public Button intakeBackIn = CodriverControls.INTAKE_BACK_IN.createFrom(codriverStick); - public Button intakeBackOut = CodriverControls.INTAKE_BACK_OUT.createFrom(codriverStick); + // Intake Controls + public final Button frontIntakeUpDown = CodriverControls.FRONT_INTAKE_DOWN.createFrom(codriverStick); + public final Button backIntakeUpDown = CodriverControls.BACK_INTAKE_DOWN.createFrom(codriverStick); + public final Button intakeFrontIn = CodriverControls.INTAKE_FRONT_IN.createFrom(codriverStick); + public final Button intakeFrontOut = CodriverControls.INTAKE_FRONT_OUT.createFrom(codriverStick); + public final Button intakeBackIn = CodriverControls.INTAKE_BACK_IN.createFrom(codriverStick); + public final Button intakeBackOut = CodriverControls.INTAKE_BACK_OUT.createFrom(codriverStick); - public Button climbModeButton = CodriverManualControls.CLIMB_MODE.createFrom(codriverManualStick); + // Climb Controls + public final Button climbModeButton = CodriverManualControls.CLIMB_MODE.createFrom(codriverManualStick); // Constructor to set all of the commands and buttons public OI(RobotContainer robotContainer) { + bindClimbControls(robotContainer); + bindDriverControls(robotContainer); + bindIntakeControls(robotContainer); + bindLiftControls(robotContainer); + bindIndexControls(robotContainer); + } - if (RobotMap.INTAKE_CONNECTED) { - // INTAKE front - // intakeFrontOnButton.whenPressed(new - // IntakeFrontBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, - // robotContainer.m_intakeMotorOnOffSubsystem)); - // intakeFrontOffButton.whenPressed(new - // IntakeFrontBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, - // robotContainer.m_intakeMotorOnOffSubsystem)); - // - // INTAKE back - // intakeBackOnButton.whenPressed(new - // IntakeBackBothOnCommandGroup(robotContainer.m_intakeUpDownSubsystem, - // robotContainer.m_intakeMotorOnOffSubsystem)); - // intakeBackOffButton.whenPressed(new - // IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem, - // robotContainer.m_intakeMotorOnOffSubsystem)); - - frontIntakeUpDown.whenReleased(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); - frontIntakeUpDown.whenPressed(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); - - backIntakeUpDown.whenReleased(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); - backIntakeUpDown.whenPressed(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); - - // intakeFrontIn.whenPressed(new - // IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - // .andThen(new InstantCommand(() -> - // robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1)))); - intakeFrontIn.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); - - intakeFrontOut.whenPressed(new IntakeFrontOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(1)))); - intakeFrontOut.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); - - // intakeBackIn.whenPressed(new - // IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - // .andThen(new InstantCommand(() -> - // robotContainer.m_indexerMotorSubsystem.setBackMotor(-1)))); - intakeBackIn.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); - - intakeBackOut.whenPressed(new IntakeBackOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(1)))); - intakeBackOut.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); - intakeFrontIn.whileHeld(new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> { - robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); - if (robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) - robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1); - else - robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); - if (!robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) - robotContainer.m_indexerMotorSubsystem.setBackMotor(1); - else - robotContainer.m_indexerMotorSubsystem.setBackMotor(0); - }))); - intakeBackIn.whileHeld(new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem) - .andThen(new InstantCommand(() -> { - robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); - if (robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) - robotContainer.m_indexerMotorSubsystem.setBackMotor(-1); - else - robotContainer.m_indexerMotorSubsystem.setBackMotor(0); - if (!robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) - robotContainer.m_indexerMotorSubsystem.setFrontMotor(1); - else - robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); - }))); + public void bindIndexControls(RobotContainer robotContainer) { + if (RobotMap.INDEX_CONNECTED) { + return; } - if (RobotMap.CONTROL_PANEL_CONNECTED) { - // CONTROL PANEL - // controlPanelSpinThreeTimesButton - // .whenPressed(new - // RotateControlPanelCommand(robotContainer.m_controlPanelColorSubsystem)); - // controlPanelSetToTargetButton - // .whenPressed(new - // SetToTargetColorCommand(robotContainer.m_controlPanelColorSubsystem)); - } + indexerSpitButton.whileHeld(new IndexSpitCommand(robotContainer.m_indexerMotorSubsystem, + robotContainer.m_intakeMotorOnOffSubsystem)); - if (RobotMap.CLIMB_CONNECTED) { - // climbDeployRailsButton.whenActive(new - // ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); - // climbExtendArmButton.whenActive(new - // ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); - // climbRetractArmButton.whenActive(new - // ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem)); - // climbRetractRailsButton.whenActive(new - // ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); - // climbStopArmButton.whenActive(new - // ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem)); - climbModeButton - .whileHeld(new ClimbJoystickCommand(codriverManualStick, robotContainer.m_climbMotorSubsystem)); - climbModeButton.whenPressed(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); - climbModeButton.whenReleased(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); - } + Command indexShootCommand = new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, + robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem); - if (RobotMap.INDEX_CONNECTED) { - indexerSpitButton.whileHeld(new IndexSpitCommand(robotContainer.m_indexerSensorSubsystem, - robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem)); + indexerShootButton.whenPressed(indexShootCommand); - Command indexShootCommand = new IndexShootCommand(robotContainer.m_indexerSensorSubsystem, - robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem); + indexerShootButton + .whenReleased(new InstantCommand(() -> CommandScheduler.getInstance().cancel(indexShootCommand))); + } - indexerShootButton.whenPressed(indexShootCommand); + public void bindIntakeControls(RobotContainer robotContainer) { + if (!RobotMap.INTAKE_CONNECTED) { + return; + } - indexerShootButton - .whenReleased(new InstantCommand(() -> CommandScheduler.getInstance().cancel(indexShootCommand))); + frontIntakeUpDown.whenReleased(new IntakeFrontDownCommand(robotContainer.m_intakeUpDownSubsystem)); + frontIntakeUpDown.whenPressed(new IntakeFrontUpCommand(robotContainer.m_intakeUpDownSubsystem)); + backIntakeUpDown.whenReleased(new IntakeBackDownCommand(robotContainer.m_intakeUpDownSubsystem)); + backIntakeUpDown.whenPressed(new IntakeBackUpCommand(robotContainer.m_intakeUpDownSubsystem)); + + if (!RobotMap.INDEX_CONNECTED) { // Protects from null pointers lower, should not effect because intake requires + // index mech. + return; } - if (RobotMap.DRIVE_BASE_CONNECTED) { - robotContainer.m_driveBaseSubsystem.setDefaultCommand(new DriveCommand(robotContainer.m_driveBaseSubsystem, - driverRightStick, driverLeftStick, DriverControls.ALIGN_STICKS.createFrom(driverLeftStick))); + intakeFrontIn.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); + + intakeFrontOut.whenPressed(new IntakeFrontOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(1)))); + intakeFrontOut.whenReleased(new IntakeFrontOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setFrontMotor(0)))); + + intakeBackIn.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.stopAllMotors()))); + + intakeBackOut.whenPressed(new IntakeBackOutCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(1)))); + intakeBackOut.whenReleased(new IntakeBackOffCommand(robotContainer.m_intakeMotorOnOffSubsystem) + .andThen(new InstantCommand(() -> robotContainer.m_indexerMotorSubsystem.setBackMotor(0)))); + intakeFrontIn.whileHeld( + new IntakeFrontInCommand(robotContainer.m_intakeMotorOnOffSubsystem).andThen(new InstantCommand(() -> { + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + if (robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setFrontMotor(-1); + else + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); + if (!robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + robotContainer.m_indexerMotorSubsystem.setBackMotor(1); + else + robotContainer.m_indexerMotorSubsystem.setBackMotor(0); + }))); + intakeBackIn.whileHeld( + new IntakeBackInCommand(robotContainer.m_intakeMotorOnOffSubsystem).andThen(new InstantCommand(() -> { + robotContainer.m_indexerMotorSubsystem.setMidMotor(-0.2); + if (robotContainer.m_indexerSensorSubsystem.getIndexBackSensorValue()) + robotContainer.m_indexerMotorSubsystem.setBackMotor(-1); + else + robotContainer.m_indexerMotorSubsystem.setBackMotor(0); + if (!robotContainer.m_indexerSensorSubsystem.getIndexFrontSensorValue()) + robotContainer.m_indexerMotorSubsystem.setFrontMotor(1); + else + robotContainer.m_indexerMotorSubsystem.setFrontMotor(0); + }))); + + } - shifter.whenPressed(new DriveShiftToHighGearCommand(robotContainer.m_driveBaseSubsystem)); - shifter.whenReleased(new DriveShiftToLowGearCommand(robotContainer.m_driveBaseSubsystem)); + public void bindClimbControls(RobotContainer robotContainer) { + if (!RobotMap.CLIMB_CONNECTED) { + return; } - // LIFT - if (RobotMap.LIFT_CONNECTED) { - liftButton.whenPressed( - new LiftUpCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); - liftButton.whenReleased( - new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); + climbModeButton.whileHeld(new ClimbJoystickCommand(codriverManualStick, robotContainer.m_climbMotorSubsystem)); + climbModeButton.whenPressed(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem)); + climbModeButton.whenReleased(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem)); + } + + public void bindLiftControls(RobotContainer robotContainer) { + if (!RobotMap.LIFT_CONNECTED) { + return; } - // Trigger intakeUpWhenFiveBalls = new Trigger(RobotState::hasFiveBalls); - if (RobotMap.INTAKE_CONNECTED) { - // intakeUpWhenFiveBalls.whenActive(new - // IntakeBothUpCommand(robotContainer.m_intakeUpDownSubsystem)); + liftButton + .whenPressed(new LiftUpCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); + liftButton.whenReleased( + new LiftDownCommand(robotContainer.m_liftSubsystem, robotContainer.m_indexerMotorSubsystem)); + } + + public void bindDriverControls(RobotContainer robotContainer) { + if (RobotMap.DRIVE_BASE_CONNECTED) { + return; } + robotContainer.m_driveBaseSubsystem.setDefaultCommand(new DriveCommand(robotContainer.m_driveBaseSubsystem, + driverRightStick, driverLeftStick, DriverControls.ALIGN_STICKS.createFrom(driverLeftStick))); + + shifter.whenPressed(new DriveShiftToHighGearCommand(robotContainer.m_driveBaseSubsystem)); + shifter.whenReleased(new DriveShiftToLowGearCommand(robotContainer.m_driveBaseSubsystem)); } } diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 1c92a592..2853a9c0 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -7,20 +7,15 @@ package frc.team2412.robot; -import java.util.Set; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.commands.flywheel.FlywheelShootCommand; import frc.team2412.robot.commands.hood.HoodAdjustCommand; +import frc.team2412.robot.commands.hood.HoodJoystickCommand; import frc.team2412.robot.commands.hood.HoodWithdrawCommand; -import frc.team2412.robot.commands.indexer.IndexShootCommand; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.Logger; @@ -35,9 +30,7 @@ public class Robot extends TimedRobot implements Loggable { public double timeRemaining; - // Have instances of robot container and OI for easy access private RobotContainer m_robotContainer = RobotMap.m_robotContainer; - @SuppressWarnings("unused") private OI m_OI = RobotMap.m_OI; Command autoCommand; @@ -122,11 +115,13 @@ public void autonomousPeriodic() { @Override public void teleopInit() { timeRemaining = 135.0; - //CommandScheduler.getInstance().cancel(autoCommand); + // CommandScheduler.getInstance().cancel(autoCommand); CommandScheduler.getInstance() .schedule(new InstantCommand(() -> m_robotContainer.m_indexerMotorSubsystem.stopAllMotors())); m_robotContainer.m_flywheelSubsystem.setSpeed(-0.25); - //m_robotContainer.m_LimelightSubsystem.stopLimelight(); + + m_robotContainer.m_hoodSubsystem.setDefaultCommand( + new HoodJoystickCommand(m_robotContainer.m_hoodSubsystem, () -> m_OI.codriverStick.getY() * 0.5 + 0.5)); } /** @@ -135,10 +130,6 @@ public void teleopInit() { @Override public void teleopPeriodic() { timeRemaining -= 0.02; - - double val = m_OI.codriverStick.getY() * 0.5 + 0.5; - m_robotContainer.m_hoodSubsystem.setServo(val); - } @Override diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java index 48c56f11..1b3621e5 100644 --- a/src/main/java/frc/team2412/robot/RobotContainer.java +++ b/src/main/java/frc/team2412/robot/RobotContainer.java @@ -20,7 +20,7 @@ // this is the class for containing all the subsystems and OI of the robot public class RobotContainer implements Loggable { - @Log( name = "Limelight Subsystem", tabName = "Turret") + @Log(name = "Limelight Subsystem", tabName = "Turret") public LimelightSubsystem m_LimelightSubsystem; @Log(name = "Turret Subsystem", tabName = "Turret") @@ -78,7 +78,7 @@ public RobotContainer() { m_indexerMotorSubsystem = new IndexerMotorSubsystem(RobotMap.indexFrontMotor, RobotMap.indexMidMotor, RobotMap.indexBackMotor, m_indexerSensorSubsystem); - + } if (RobotMap.LIFT_CONNECTED) { diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java index f8e9a0bc..a51e064c 100644 --- a/src/main/java/frc/team2412/robot/RobotMap.java +++ b/src/main/java/frc/team2412/robot/RobotMap.java @@ -134,7 +134,6 @@ public int getIndexCANID() { public static CANSparkMax rightClimbMotor = CLIMB_CONNECTED ? new CANSparkMax(CANBus.CLIMB2.id, MotorType.kBrushless) : null; - // INDEX SUBSYSTEM // --------------------------------------------------------------------------- diff --git a/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java b/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java index a9aa7f4d..63b6c42d 100644 --- a/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java +++ b/src/main/java/frc/team2412/robot/commands/hood/HoodAdjustCommand.java @@ -3,22 +3,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team2412.robot.subsystems.HoodSubsystem; -public class HoodAdjustCommand extends CommandBase{ - private double m_increment; - private HoodSubsystem m_HoodSubsystem; +public class HoodAdjustCommand extends CommandBase { + private double m_increment; + private HoodSubsystem m_HoodSubsystem; - public HoodAdjustCommand(HoodSubsystem hoodSubsystem, double increment){ - this.m_HoodSubsystem = hoodSubsystem; - this.m_increment = increment; - } + public HoodAdjustCommand(HoodSubsystem hoodSubsystem, double increment) { + this.m_HoodSubsystem = hoodSubsystem; + this.m_increment = increment; + } - public void execute(){ - this.m_HoodSubsystem.add(this.m_increment); - } - - public boolean isFinished(){ - return true; - } + @Override + public void execute() { + this.m_HoodSubsystem.add(this.m_increment); + } + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/team2412/robot/commands/hood/HoodJoystickCommand.java b/src/main/java/frc/team2412/robot/commands/hood/HoodJoystickCommand.java new file mode 100644 index 00000000..e132b7c6 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/hood/HoodJoystickCommand.java @@ -0,0 +1,27 @@ +package frc.team2412.robot.commands.hood; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.subsystems.HoodSubsystem; + +public class HoodJoystickCommand extends CommandBase { + + private HoodSubsystem m_hoodSubsystem; + private DoubleSupplier m_angleSupplier; + + public HoodJoystickCommand(HoodSubsystem hoodSubsystem, DoubleSupplier angleSupplier) { + this.m_angleSupplier = angleSupplier; + this.m_hoodSubsystem = hoodSubsystem; + } + + @Override + public void execute() { + m_hoodSubsystem.setServo(m_angleSupplier.getAsDouble()); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java index 1af0a0fa..6024b665 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexBackShootCommand.java @@ -1,23 +1,19 @@ package frc.team2412.robot.commands.indexer; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team2412.robot.RobotState; import frc.team2412.robot.subsystems.IndexerMotorSubsystem; -import frc.team2412.robot.subsystems.IndexerSensorSubsystem; import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; -//This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor public class IndexBackShootCommand extends CommandBase { - private IndexerSensorSubsystem m_indexerSensorSubsystem; private IndexerMotorSubsystem m_indexerMotorSubsystem; private IntakeOnOffSubsystem m_IntakeOnOffSubsystem; - public IndexBackShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { - m_indexerSensorSubsystem = sensorSubsystem; - m_indexerMotorSubsystem = motorSubsystem; + public IndexBackShootCommand(IndexerMotorSubsystem indexerMotorSubsystem, + IntakeOnOffSubsystem intakeOnOffSubsystem) { + m_indexerMotorSubsystem = indexerMotorSubsystem; m_IntakeOnOffSubsystem = intakeOnOffSubsystem; - addRequirements(sensorSubsystem, motorSubsystem); + addRequirements(indexerMotorSubsystem); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java index 91f78f00..b88dfc46 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexFrontShootCommand.java @@ -1,23 +1,19 @@ package frc.team2412.robot.commands.indexer; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team2412.robot.RobotState; import frc.team2412.robot.subsystems.IndexerMotorSubsystem; -import frc.team2412.robot.subsystems.IndexerSensorSubsystem; import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; -//This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor public class IndexFrontShootCommand extends CommandBase { - private IndexerSensorSubsystem m_indexerSensorSubsystem; private IndexerMotorSubsystem m_indexerMotorSubsystem; private IntakeOnOffSubsystem m_IntakeOnOffSubsystem; - public IndexFrontShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { - m_indexerSensorSubsystem = sensorSubsystem; - m_indexerMotorSubsystem = motorSubsystem; + public IndexFrontShootCommand(IndexerMotorSubsystem indexerMotorSubsystem, + IntakeOnOffSubsystem intakeOnOffSubsystem) { + m_indexerMotorSubsystem = indexerMotorSubsystem; m_IntakeOnOffSubsystem = intakeOnOffSubsystem; - addRequirements(sensorSubsystem, motorSubsystem); + addRequirements(indexerMotorSubsystem); } @Override @@ -31,5 +27,4 @@ public void execute() { public boolean isFinished() { return true; } - } diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java index dde7f95c..4e33ed90 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexShootCommand.java @@ -1,9 +1,8 @@ package frc.team2412.robot.commands.indexer; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.RobotState; import frc.team2412.robot.subsystems.IndexerMotorSubsystem; import frc.team2412.robot.subsystems.IndexerSensorSubsystem; import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; @@ -14,12 +13,21 @@ public class IndexShootCommand extends SequentialCommandGroup { private IndexerMotorSubsystem m_indexerMotorSubsystem; private IntakeOnOffSubsystem m_intakeOnOffSubsystem; - public IndexShootCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeSubsystem) { - m_indexerMotorSubsystem = motorSubsystem; + public IndexShootCommand(IndexerSensorSubsystem indexerSensorSubsystem, IndexerMotorSubsystem indexMotorSubsystem, + IntakeOnOffSubsystem intakeSubsystem) { + m_indexerMotorSubsystem = indexMotorSubsystem; m_intakeOnOffSubsystem = intakeSubsystem; + + addCommands(new IndexFrontShootCommand(indexMotorSubsystem, intakeSubsystem), + new ConditionalCommand(new WaitCommand(3), new WaitCommand(0), + indexerSensorSubsystem::allFrontSensorsOff), + new IndexBackShootCommand(indexMotorSubsystem, intakeSubsystem), new WaitCommand(2)); + } + + @Override + public void initialize() { + super.initialize(); m_indexerMotorSubsystem.setLifting(true); - addCommands(new IndexFrontShootCommand(sensorSubsystem, motorSubsystem, intakeSubsystem), new WaitCommand(3), - new IndexBackShootCommand(sensorSubsystem, motorSubsystem, intakeSubsystem), new WaitCommand(2)); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java index ae5fea7a..7c901f37 100644 --- a/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java +++ b/src/main/java/frc/team2412/robot/commands/indexer/IndexSpitCommand.java @@ -1,25 +1,19 @@ package frc.team2412.robot.commands.indexer; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team2412.robot.RobotState; import frc.team2412.robot.subsystems.IndexerMotorSubsystem; -import frc.team2412.robot.subsystems.IndexerSensorSubsystem; import frc.team2412.robot.subsystems.IntakeOnOffSubsystem; -//This is an example command for this year. Make sure all commands extend CommandBase and they use take all dependencies(fields) through a constructor public class IndexSpitCommand extends CommandBase { - private IndexerSensorSubsystem m_indexerSensorSubsystem; private IndexerMotorSubsystem m_indexerMotorSubsystem; private IntakeOnOffSubsystem m_intakeOnOffSubsystem; - public IndexSpitCommand(IndexerSensorSubsystem sensorSubsystem, IndexerMotorSubsystem motorSubsystem, - IntakeOnOffSubsystem intakeOnOffSubsystem) { - m_indexerSensorSubsystem = sensorSubsystem; + public IndexSpitCommand(IndexerMotorSubsystem motorSubsystem, IntakeOnOffSubsystem intakeOnOffSubsystem) { m_indexerMotorSubsystem = motorSubsystem; m_intakeOnOffSubsystem = intakeOnOffSubsystem; - addRequirements(sensorSubsystem, motorSubsystem, intakeOnOffSubsystem); + addRequirements(motorSubsystem, intakeOnOffSubsystem); } @Override @@ -34,7 +28,6 @@ public void execute() { public void end(boolean cancel) { m_indexerMotorSubsystem.stopAllMotors(); m_intakeOnOffSubsystem.intakeOff(); - // RobotState.m_ballCount = 0; } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java index 5e2d2403..e5b7351a 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothInCommandGroup.java @@ -7,7 +7,8 @@ public class IntakeBothInCommandGroup extends ParallelCommandGroup { public IntakeBothInCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontInCommand(intakeOnOffSubsystem, false), new IntakeBackInCommand(intakeOnOffSubsystem, false)); + addCommands(new IntakeFrontInCommand(intakeOnOffSubsystem, false), + new IntakeBackInCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java index e6827243..f6c77305 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOffCommandGroup.java @@ -8,7 +8,8 @@ public class IntakeBothOffCommandGroup extends ParallelCommandGroup { public IntakeBothOffCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontOffCommand(intakeOnOffSubsystem, false), new IntakeBackOffCommand(intakeOnOffSubsystem, false)); + addCommands(new IntakeFrontOffCommand(intakeOnOffSubsystem, false), + new IntakeBackOffCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java index 3915e95a..09c210ee 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeBothOutCommandGroup.java @@ -7,7 +7,8 @@ public class IntakeBothOutCommandGroup extends ParallelCommandGroup { public IntakeBothOutCommandGroup(IntakeOnOffSubsystem intakeOnOffSubsystem) { - addCommands(new IntakeFrontOutCommand(intakeOnOffSubsystem, false), new IntakeBackOutCommand(intakeOnOffSubsystem, false)); + addCommands(new IntakeFrontOutCommand(intakeOnOffSubsystem, false), + new IntakeBackOutCommand(intakeOnOffSubsystem, false)); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java index 2cae5c43..62b328b2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ClimbMotorSubsystem.java @@ -85,7 +85,7 @@ public void setMotors(double value) { if (value < 0 && m_leftEncoder.getPosition() > 0 || value > 0 && m_leftEncoder.getPosition() < 76) { m_leftClimbMotor.set(value); - }else{ + } else { m_leftClimbMotor.set(0); } System.out.println("right: " + m_encoder.getPosition()); diff --git a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java index bc5790ac..89d68e7e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DriveBaseSubsystem.java @@ -108,20 +108,20 @@ public DriveBaseSubsystem(Solenoid gearShifter, Gyro gyro, WPI_TalonFX leftMotor } public void drive(Joystick rightJoystick, Joystick leftJoystick, Button button) { - if(oneJoystickDrive){ - m_rightMotor1.set(rightJoystick.getY()-Math.pow(rightJoystick.getTwist(),3)); - m_leftMotor1.set(rightJoystick.getY()+Math.pow(rightJoystick.getTwist(), 3)); + if (oneJoystickDrive) { + m_rightMotor1.set(rightJoystick.getY() - Math.pow(rightJoystick.getTwist(), 3)); + m_leftMotor1.set(rightJoystick.getY() + Math.pow(rightJoystick.getTwist(), 3)); } else { - - if (button.get()) { - m_rightMotor1.set(rightJoystick.getY()); - m_leftMotor1.set(rightJoystick.getY()); - } else { - m_rightMotor1.set(rightJoystick.getY()); - m_leftMotor1.set(leftJoystick.getY()); + + if (button.get()) { + m_rightMotor1.set(rightJoystick.getY()); + m_leftMotor1.set(rightJoystick.getY()); + } else { + m_rightMotor1.set(rightJoystick.getY()); + m_leftMotor1.set(leftJoystick.getY()); + } } } - } public void oneJoystickDrive(Joystick joystick) { // m_drive.arcadeDrive(-1 * joystick.getY(), joystick.getTwist(), true); diff --git a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java index e72ea407..78afa86c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IndexerMotorSubsystem.java @@ -14,6 +14,7 @@ public class IndexerMotorSubsystem extends SubsystemBase implements Loggable { + @SuppressWarnings("unused") private double frontTicks, backTicks, midTicks; private CANEncoder m_frontEncoder, m_backEncoder, m_midEncoder; @@ -151,8 +152,6 @@ public void periodic() { System.out.println(m_indexMidMotor.get()); } - - public void setLifting(boolean b) { this.lifting = b; } diff --git a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java index 25fc90bf..5c1c52d4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/TurretSubsystem.java @@ -39,7 +39,8 @@ public TurretSubsystem(WPI_TalonSRX turretMotor, LimelightSubsystem limelightSub initTurretEncoder(); - // setDefaultCommand(new TurretFollowLimelightCommand(this, m_limelightSubsystem)); + // setDefaultCommand(new TurretFollowLimelightCommand(this, + // m_limelightSubsystem)); } public Rotations getCurrentAngle() {