From 0829186f9523ecc5f812a15b8246b3035933e3d6 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Sat, 30 Sep 2023 15:26:00 +0300 Subject: [PATCH 1/7] rotem's changes --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- .../frc/robot/subsystems/ArmFunnelSuperStructure.java | 4 ++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22c4f2a..964b2f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,6 +95,8 @@ public RobotContainer() { setCubeInternalState(); m_PDH.setSwitchableChannel(false); + + SmartDashboard.putNumber("cataAngle", SmartDashboard.getNumber("cataAngle", 0)); } /** @@ -178,8 +180,12 @@ private void configureBindings() { m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD))); _operatorController.cross() - .onTrue(m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.REJECT, FunnelState.OPEN) - .andThen(this.getCollectSequence())); + .onTrue(Commands.sequence( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.MID_CUBE, FunnelState.OPEN), + Commands.waitUntil( + () -> m_ArmFunnelSuperStructure.getArmAngle() > SmartDashboard.getNumber("cataAngle", + 0)), + m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN))); } diff --git a/src/main/java/frc/robot/subsystems/ArmFunnelSuperStructure.java b/src/main/java/frc/robot/subsystems/ArmFunnelSuperStructure.java index 2e8ef5b..a359fe2 100644 --- a/src/main/java/frc/robot/subsystems/ArmFunnelSuperStructure.java +++ b/src/main/java/frc/robot/subsystems/ArmFunnelSuperStructure.java @@ -100,4 +100,8 @@ public CommandBase overrideCommand() { m_arm.getSetStateCommand(ArmState.MID_CONE, ArmTrapProfile.FAST), m_funnel.setFunnelStateCommand(FunnelState.CLOSED)); } + + public double getArmAngle() { + return m_arm.getAngle(); + } } From 56cc3ee1417cab6e3531327ce7bd61a1009c7448 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Tue, 3 Oct 2023 15:17:54 +0300 Subject: [PATCH 2/7] working catapult auto sequence!! --- networktables.json | 1 + src/main/java/frc/robot/RobotContainer.java | 33 +++++++++++++++++---- 2 files changed, 28 insertions(+), 6 deletions(-) create mode 100644 networktables.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 964b2f8..4b35aa5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,7 +155,8 @@ private void configureBindings() { // Go to collect state sequence _operatorController.povDown().onTrue( - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED) + m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD).andThen( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED)) .andThen(m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN))); _driverController.circle().whileTrue( @@ -181,11 +182,15 @@ private void configureBindings() { _operatorController.cross() .onTrue(Commands.sequence( - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.MID_CUBE, FunnelState.OPEN), - Commands.waitUntil( - () -> m_ArmFunnelSuperStructure.getArmAngle() > SmartDashboard.getNumber("cataAngle", - 0)), - m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN))); + new InstantCommand(() -> m_manipulator.setManipulatorState(ManipulatorState.HOLD)), + m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.OPEN).alongWith( + (new WaitCommand(0.5).andThen(m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN)))) + //.alongWith( + // Commands.waitUntil( + // () -> m_ArmFunnelSuperStructure.getArmAngle() > SmartDashboard.getNumber("cataAngle", + // 0))), + //m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN) + )); } @@ -215,6 +220,8 @@ private void addToChooser(String pathName) { private void initChooser() { SmartDashboard.putData("autonomous", this.chooser); + this.chooser.addOption("catapult", getCatapultSequence() + .andThen(m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED))); // addToChooser("engage"); // addToChooser("1-gp-engage"); addToChooser("1-gp-leaveCommunity"); @@ -264,6 +271,20 @@ public void updateTelemetry() { m_drivetrain.updateTelemetry(); } + private CommandBase getCatapultSequence() { + return Commands.sequence( + // new InstantCommand(() -> setCubeInternalState()), + m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD), + //getCollectSequence().withTimeout(0.4), + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.COLLECT), + new WaitCommand(0.2), + + m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT), + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED) + + ); + } + private CommandBase getEngageSequence() { return Commands.sequence( _autoFactory.createAuto("engage-gyro"), From 7fdeb6d8c0bbe5a79bea1dcc88be211d4a6b141b Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Tue, 3 Oct 2023 19:11:52 +0300 Subject: [PATCH 3/7] corrected catapult daly and removed commented out lines --- src/main/java/frc/robot/RobotContainer.java | 27 +++++---------------- 1 file changed, 6 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b35aa5..2fc078e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,8 +155,8 @@ private void configureBindings() { // Go to collect state sequence _operatorController.povDown().onTrue( - m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD).andThen( - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED)) + m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD).andThen( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED)) .andThen(m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN))); _driverController.circle().whileTrue( @@ -179,19 +179,7 @@ private void configureBindings() { new WaitCommand(0.5), m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED), m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD))); - - _operatorController.cross() - .onTrue(Commands.sequence( - new InstantCommand(() -> m_manipulator.setManipulatorState(ManipulatorState.HOLD)), - m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.OPEN).alongWith( - (new WaitCommand(0.5).andThen(m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN)))) - //.alongWith( - // Commands.waitUntil( - // () -> m_ArmFunnelSuperStructure.getArmAngle() > SmartDashboard.getNumber("cataAngle", - // 0))), - //m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN) - )); - + } private void setCubeInternalState() { @@ -273,12 +261,9 @@ public void updateTelemetry() { private CommandBase getCatapultSequence() { return Commands.sequence( - // new InstantCommand(() -> setCubeInternalState()), - m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD), - //getCollectSequence().withTimeout(0.4), - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.COLLECT), - new WaitCommand(0.2), - + m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD), + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.COLLECT), + new WaitCommand(0.3), m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT), m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED) From accb00be317c0788fed758f3ba508b693fe91151 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Tue, 3 Oct 2023 19:30:01 +0300 Subject: [PATCH 4/7] added a catapult-engage path and a catapult-mobility-engage path --- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2fc078e..167e77b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -179,7 +179,7 @@ private void configureBindings() { new WaitCommand(0.5), m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED), m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD))); - + } private void setCubeInternalState() { @@ -229,8 +229,13 @@ private void initChooser() { this.chooser.addOption("cube-engage-gyro", getAutoCubeSequence().andThen(getEngageSequence())); // only engage this.chooser.addOption("engage-gyro", getEngageSequence()); + this.chooser.addOption("cube-mobility-engage", getAutoCubeSequence().andThen(getMobilityEngageSequence())); + this.chooser.addOption("catapult-mobility-engage", getCatapultSequence().andThen(getMobilityEngageSequence())); + + this.chooser.addOption("catapult-engage", getCatapultSequence().andThen(getEngageSequence())); + // taxi this.chooser.addOption("taxi", _autoFactory.createAuto("engage-gyro")); From 21e6fc6b3211de2a3427c07559d4f1677ce2bcd2 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Tue, 3 Oct 2023 19:34:34 +0300 Subject: [PATCH 5/7] removed the lowering of the arm from catapult sequence --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 167e77b..02b5c76 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -269,8 +269,8 @@ private CommandBase getCatapultSequence() { m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD), m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.COLLECT), new WaitCommand(0.3), - m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT), - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED) + m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT) + ); } From 7e8738e73fa2c3e331a0aea374f881dbf1286438 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Tue, 3 Oct 2023 20:05:19 +0300 Subject: [PATCH 6/7] added teleop catapult --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02b5c76..33c2016 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -179,6 +179,23 @@ private void configureBindings() { new WaitCommand(0.5), m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED), m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD))); + // catapult + _operatorController.povLeft() + .onTrue( + m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT) + .andThen( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, + FunnelState.CLOSED))); + + // catapult collect + _operatorController.triangle() + .onTrue( + m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD).andThen( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.OPEN))); + + // close funnle without opening manipulator + _operatorController.povRight().onTrue( + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED)); } @@ -270,7 +287,6 @@ private CommandBase getCatapultSequence() { m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.COLLECT), new WaitCommand(0.3), m_ArmFunnelSuperStructure.generateSetStateCommand(ArmState.MID_CUBE, FunnelState.COLLECT) - ); } From 9c84a108866d3db9c5c2b37d3f643f7acbd044b0 Mon Sep 17 00:00:00 2001 From: yoavpeles Date: Wed, 4 Oct 2023 07:03:00 +0300 Subject: [PATCH 7/7] changed back the povDown botton --- src/main/java/frc/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 33c2016..d668df2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,8 +155,7 @@ private void configureBindings() { // Go to collect state sequence _operatorController.povDown().onTrue( - m_manipulator.setManipulatorStateCommand(ManipulatorState.HOLD).andThen( - m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED)) + m_ArmFunnelSuperStructure.getSetStateCommand(ArmState.COLLECT, FunnelState.CLOSED) .andThen(m_manipulator.setManipulatorStateCommand(ManipulatorState.OPEN))); _driverController.circle().whileTrue(