diff options
Diffstat (limited to 'src/main/java/frc/robot/RobotContainer.java')
| -rw-r--r-- | src/main/java/frc/robot/RobotContainer.java | 74 |
1 files changed, 40 insertions, 34 deletions
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 88e493c..9a6241a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,32 +4,39 @@ package frc.robot; -import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase; - import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Chain; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Hooks; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.MainMech; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.MainMech.MechCmd; public class RobotContainer { private final Drivetrain m_drive = new Drivetrain(); - private final Chain m_chain = new Chain(); - private final Arm m_arm = new Arm(); + private final MainMech m_mech = new MainMech(); private final Intake m_intake = new Intake(); private final Shooter m_shooter = new Shooter(); private final Hooks m_hooks = new Hooks(); - private final GenericHID m_controller = Constants.controller; + private final GenericHID m_controller1 = Constants.controller1; + private final GenericHID m_controller2 = Constants.controller2; + SendableChooser<Command> m_chooser = new SendableChooser<>(); public RobotContainer() { + m_chooser.setDefaultOption("None", Commands.none()); + m_chooser.addOption("Shoot", + Commands.parallel( + m_shooter.run(() -> m_shooter.atPercentage(1)), + Commands.none().withTimeout(500).andThen(m_intake.run(() -> m_intake.atPercentage(1))))); + SmartDashboard.putData(m_chooser); configureBindings(); } @@ -63,21 +70,23 @@ public class RobotContainer { m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0))); m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0))); - m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees( - deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2))))); m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage( - deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)), - deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5))))); - m_chain.setDefaultCommand(m_chain.run(() -> - m_chain.atPercentage( - Constants.controller2.getRawAxis(0) - * (-0.5) + 0.5 - ))); + deadzone(-0.1, 0.1, m_controller2.getRawAxis(4)), + deadzone(-0.1, 0.1, m_controller2.getRawAxis(5))))); + + // m_mech.setDefaultCommands( + // m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle( + // deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.3)) , + // m_mech.m_chain.run(() -> m_mech.m_chain.atPercentage( + // m_controller2.getRawAxis(0) + // * (-0.5) + 0.5)) + // ); + m_mech.setDefaultCommand(m_mech.run(() -> m_mech.toState(MechCmd.MANUAL).execute())); m_drive.setDefaultCommand(m_drive.run(() -> { double[] inputs = correctInputs( - m_controller.getRawAxis(2), - m_controller.getRawAxis(1)); + m_controller1.getRawAxis(2), + m_controller1.getRawAxis(1)); m_drive.tank( inputs[0] * Constants.DriveMaxSpeed, inputs[1] * Constants.DriveMaxSpeed); @@ -93,21 +102,18 @@ public class RobotContainer // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory)); - new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3))); - new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1))); - new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1))); - new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1))); - - new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue( - Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)), - Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9))) - .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue( - // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20)) - // .andThen(() -> - // m_chain.run(() -> m_chain.atPercentage(0.9)).until(() -> - // m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40)))) - // )); - // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen()); + new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3))); + new Trigger(() -> m_controller1.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.shoot())); + new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1))); + new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.6))); + + new Trigger(() -> m_controller2.getRawButton(1)).whileTrue(m_mech.toState(MechCmd.MANUAL)); + new Trigger(() -> m_controller2.getRawAxis(6) > 0).whileTrue(m_mech.toState(MechCmd.INTAKE)); + new Trigger(() -> m_controller2.getRawAxis(7) > 0).whileTrue(m_mech.toState(MechCmd.DEFAULT)); + new Trigger(() -> m_controller2.getRawButton(4)).whileTrue(m_mech.toState(MechCmd.AMP)); + new Trigger(() -> m_controller2.getRawButton(3)).whileTrue(m_mech.toState(MechCmd.SPEAKER)); + + // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse)); // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward)); // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse)); @@ -121,6 +127,6 @@ public class RobotContainer public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return m_chooser.getSelected(); } } |