orks

1 files changed, 52 insertions, 23 deletions

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 87c4e60..88e493c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,26 +4,28 @@
 
 package frc.robot;
 
-import java.util.List;
+import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-
+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.Shooter;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
-    // private final Chain m_chain = new Chain();
-
+    private final Chain m_chain = new Chain();
+    private final Arm m_arm = new Arm();
+    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;
 
     public RobotContainer() 
@@ -31,7 +33,7 @@ public class RobotContainer
         configureBindings();
     }
 
-    private double[] correctInputs(double in1, double in2)  
+    private double[] correctInputs(double in1, double in2) 
     {
         double p1 = processInput(in1);
         double p2 = processInput(in2);
@@ -59,6 +61,19 @@ public class RobotContainer
 
     private void configureBindings() 
     {   
+        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
+            )));
+
         m_drive.setDefaultCommand(m_drive.run(() -> {
             double[] inputs = correctInputs(
                     m_controller.getRawAxis(2),
@@ -68,25 +83,39 @@ public class RobotContainer
                 inputs[1] * Constants.DriveMaxSpeed);
         }));
 
-        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-            new Pose2d(0, 0, new Rotation2d(0)),
-            // List.of(),
-            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
-            new Pose2d(3, 0, new Rotation2d(0)), 
-            m_drive.trajectoyConfig);
 
-        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+        //     new Pose2d(0, 0, new Rotation2d(0)),
+        //      List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+        //     new Pose2d(3, 0, new Rotation2d(0)),     CameraServer camere = CameraServer.startAutomaticCapture();
 
-        // m_chain.setDefaultCommand(m_chain.run(() ->
-        // m_chain.atPercentage(
-        //     processInput(m_controller.getRawAxis(2))
-        // )));
+        //     m_drive.trajectoyConfig);
 
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
 
-        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+        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_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));
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kReverse));
+        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kReverse));
+
 
     }