From 776024272d2e7c5f641f5f4efaee1b4504bd5db2 Mon Sep 17 00:00:00 2001
From: Ani-8712 <114625811+Ani-8712@users.noreply.github.com>
Date: Mon, 19 Aug 2024 23:55:30 -0700
Subject: [PATCH] first time in 5 months this code acutally builds

---
 .../frc2024/commands/DrivetrainCommands.java  |  8 ++--
 .../frc2024/robot/RobotContainer.java         |  3 --
 .../frc2024/subsystems/Superstructure.java    | 41 ++++++++++---------
 3 files changed, 25 insertions(+), 27 deletions(-)

diff --git a/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
index 29d42d9..9d9b14c 100644
--- a/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
+++ b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
@@ -60,7 +60,7 @@ public Command driveVisionTeleop(
                             -turnSpeedFunction.getAsDouble() * maxRotationRadpS * triggerSlow;
 
                     if (mode == DriveMode.SHOOT_AT_AMP && enabeld) {
-                        motionXComponent = autoDriveTwist2d.dx + motionXComponent *0.1;
+                        motionXComponent = autoDriveTwist2d.dx + motionXComponent * 0.1;
                         motionTurnComponent = autoDriveTwist2d.dtheta + motionTurnComponent * 0.1;
 
                         var translation = new Translation2d(motionXComponent, motionYComponent);
@@ -68,8 +68,8 @@ public Command driveVisionTeleop(
                         var rotation = motionTurnComponent;
                         swerve.driveFieldOriented(translation.getX(), translation.getY(), rotation);
                     } else if (mode == DriveMode.INTAKE_IN_AUTO && enabeld) {
-                        motionXComponent = autoDriveTwist2d.dx  + 0.1 * motionXComponent;
-                        motionYComponent = autoDriveTwist2d.dy + 0.1 * motionYComponent;
+                        motionXComponent = autoDriveTwist2d.dx;
+                        motionYComponent = autoDriveTwist2d.dy;
 
                         var translation = new Translation2d(motionXComponent, motionYComponent);
 
@@ -77,7 +77,7 @@ public Command driveVisionTeleop(
                         swerve.drive(translation.getX(), translation.getY(), rotation);
                     } else {
                         if (mode != DriveMode.NONE && enabeld) {
-                            motionTurnComponent = autoDriveTwist2d.dtheta + motionTurnComponent * 0.1;
+                            motionTurnComponent = autoDriveTwist2d.dtheta;
                         }
                         SmartDashboard.putNumber("theta", autoDriveTwist2d.dtheta);
 
diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java
index e3767a9..af8c48d 100644
--- a/src/main/java/team3647/frc2024/robot/RobotContainer.java
+++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java
@@ -239,9 +239,6 @@ private void configureButtonBindings() {
         mainController.dPadDown.and(goodToClimb).whileTrue(climbCommands.goDown());
         mainController.dPadDown.onFalse(climbCommands.kill());
 
-
-        
-
         climbing.onTrue(superstructure.setIsClimbing());
         climbing.onFalse(superstructure.setIsNotClimbing());
 
diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java
index 98d2c01..cb393ab 100644
--- a/src/main/java/team3647/frc2024/subsystems/Superstructure.java
+++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java
@@ -226,25 +226,27 @@ public boolean swerveReady() {
         return swerveAimed.getAsBoolean();
     }
 
-    public boolean readyForShot(){
+    public boolean readyForShot() {
         return aimedAtSpeaker() && !dontShoot.getAsBoolean();
     }
 
     public Command shoot() {
         return Commands.parallel(
-                prep(), spinUp()
-                // Commands.sequence(
-                //         // Commands.waitSeconds(2.5),
-                //         Commands.waitUntil(
-                //                         () ->
-                //                                 shooterLeft.velocityReached(30, 2)
-                //                                         && pivot.angleReached(
-                //                                                 pivotAngleSupplier.getAsDouble(),
-                // 5)
-                //                                         && swerveAimed.getAsBoolean())
-                //                 .withTimeout(1.2),
-                //         feed())
-                ).until(this::readyForShot)
+                        prep(), spinUp()
+                        // Commands.sequence(
+                        //         // Commands.waitSeconds(2.5),
+                        //         Commands.waitUntil(
+                        //                         () ->
+                        //                                 shooterLeft.velocityReached(30, 2)
+                        //                                         && pivot.angleReached(
+                        //
+                        // pivotAngleSupplier.getAsDouble(),
+                        // 5)
+                        //                                         && swerveAimed.getAsBoolean())
+                        //                 .withTimeout(1.2),
+                        //         feed())
+                        )
+                .until(this::readyForShot)
                 .andThen(stowFromShoot());
     }
 
@@ -381,14 +383,13 @@ public Command stowFromShoot() {
                         .withTimeout(0.1));
     }
 
-    public Command stowNoShoot(){
+    public Command stowNoShoot() {
         return Commands.parallel(
-                                pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()),
-                                shooterCommands.kill(),
-                                kickerCommands.kill())
-                        .withTimeout(0.1);
+                        pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()),
+                        shooterCommands.kill(),
+                        kickerCommands.kill())
+                .withTimeout(0.1);
     }
-    
 
     public Command stowFromAmpShoot() {
         return Commands.sequence(