changes

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();
     }
 }