Changes from 3/5 meeting

src/main/java/frc/robot/RobotContainer.java

@@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -246,12 +247,14 @@ public class RobotContainer {
                 () -> true
             )
         );
-        shooter.setDefaultCommand(shooter.maintainSpeed(ShooterSpeeds.kIdleSpeed));
+        shooter.setDefaultCommand(shooter.stop());
         intakeRoller.setDefaultCommand(intakeRoller.stop());
         spindexer.setDefaultCommand(spindexer.stop());
 
         intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
 
+        driver.y().whileTrue(drivetrain.zeroHeading());
+
         driver.leftTrigger().whileTrue(
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
@@ -260,10 +263,11 @@ public class RobotContainer {
             ) 
         );
 
+        driver.leftBumper().whileTrue(intakeRoller.runOut());
+        driver.rightBumper().whileTrue(intakeRoller.runIn());
+
         driver.rightTrigger().whileTrue(spindexer.spinToShooter());
         driver.b().whileTrue(spindexer.spinToIntake());
-
-        /*
         driver.b().whileTrue(
             drivetrain.lockToYaw(
                 () -> {
@@ -274,11 +278,15 @@ public class RobotContainer {
                 driver::getLeftY, 
                 driver::getLeftX
             )
-        );*/
+        );
 
         secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
-        secondary.y().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
+        secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
 
+        secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
+        secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
+
+        /* 
         hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
             Pose2d hubPose = Utilities.getHubPose();
@@ -300,7 +308,7 @@ public class RobotContainer {
                     false
                 );
             }
-        }));
+        }));*/
     }
 
     private void configureNamedCommands() {

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -67,7 +67,7 @@ public class IntakePivotConstants {
         kRightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(false)
-            .follow(kLeftMotorCANID);
+            .inverted(true)
+            ;//.follow(kLeftMotorCANID);
     }
 }

src/main/java/frc/robot/constants/IntakeRollerConstants.java

@@ -11,7 +11,7 @@ public class IntakeRollerConstants {
 
     public static final boolean kInvertMotors = true;
 
-    public static final double kSpeed = .6;
+    public static final double kSpeed = 1;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -8,8 +8,8 @@ import edu.wpi.first.math.util.Units;
 import frc.robot.utilities.PhotonVisionConfig;
 
 public class PhotonConstants {
-    public static final String kCamera1Name = "pv1";
-    public static final String kCamera2Name = "pv2";
+    public static final String kCamera1Name = "CameraPV1";
+    public static final String kCamera2Name = "CameraPV2";
 
     // TODO Need actual values for all of this
     public static final Transform3d kCamera1RobotToCam = new Transform3d();