Emergency intake changes

src/main/java/frc/robot/Constants.java

@@ -8,6 +8,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.measure.Angle;
 import java.security.PublicKey;
+import java.util.Queue;
 
 import com.ctre.phoenix6.signals.InvertedValue;
 
@@ -105,5 +106,6 @@ public final class Constants {
 
     public static final int SPINDEXER_MOTOR_ID = 8;
     public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
+    public static final double SPINDEXER_MOTOR_SPEED = 0;
   }
 }

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

@@ -40,7 +40,7 @@ public class RobotContainer {
 
   public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
 
-  //public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+  public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
 
   public final TurretSubsystem turretSubsystem = new TurretSubsystem();
 
@@ -75,11 +75,11 @@ public class RobotContainer {
     m_driverController.a().onTrue(intakeSubsystem.retractCommand());
     m_driverController.b().onTrue(intakeSubsystem.reverseCommand());*/
 
-    /*m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
+    m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
     m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
     m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand());
 
-    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));*/
+    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
   }
 
   /**

src/main/java/frc/robot/commands/ManualHopperCommand.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands;
 
+import frc.robot.Constants;
 import frc.robot.subsystems.HopperSubsystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,7 +23,7 @@ public class ManualHopperCommand extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_subsystem.spin(0);
+    m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/ManualIntake.java

@@ -36,11 +36,11 @@ public class ManualIntake extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(m_XboxController.getRightBumper()==true)
+    if(m_XboxController.getLeftBumper()==true)
     {
       m_subsystem.extend();
     }
-    else if(m_XboxController.getLeftBumper()==true)
+    else if(m_XboxController.getRightBumper()==true)
     {
       m_subsystem.retract();
     }

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -58,6 +58,9 @@ public class TurretSubsystem extends TestableSubsystem {
 
   public void setTargetElevation(Angle elev)
   {
+    if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){
+      elev=Units.Degrees.of(300).minus(elev);
+    }
     hood.setAngle(elev.in(Units.Degrees));
   }