fffLori sucks at coding

RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java

@@ -94,7 +94,8 @@ public RobotContainer() {
         swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, m_driverController, 1));
         conveyor.setDefaultCommand(new AutoIndexConveyor(conveyor));
         intake.setDefaultCommand(new RunIntake(intake, m_operatorController));
-        turret.setDefaultCommand(new SpinTurret(turret, false, 0));
+        turret.setDefaultCommand(new SpinTurret(turret, vision, 2, 0));
+        shooter.setDefaultCommand(new StopShooter(shooter));
 
         // vision.setDefaultCommand(new RunCommand(vision::runVision, vision));
 
@@ -115,21 +116,37 @@ public RobotContainer() {
      * calling passing it to a {@link JoystickButton}.
      */
     private void configureButtonBindings() {
-        //starts with 1
-        JoystickButton butA = new JoystickButton(m_operatorController, 1);
-        JoystickButton butB = new JoystickButton(m_operatorController, 2); 
-        JoystickButton butY = new JoystickButton(m_operatorController, 3);
+
+        JoystickButton butA  = new JoystickButton(m_operatorController, 1);
+        JoystickButton butB  = new JoystickButton(m_operatorController, 2); 
+        JoystickButton butX  = new JoystickButton(m_operatorController, 3);
+        JoystickButton butY  = new JoystickButton(m_operatorController, 4);
+        JoystickButton lBump = new JoystickButton(m_operatorController, 5);
+        JoystickButton rBump = new JoystickButton(m_operatorController, 6);
+        JoystickButton lWing = new JoystickButton(m_operatorController, 7);
+        JoystickButton rWing = new JoystickButton(m_operatorController, 8);
+        JoystickButton lAnal = new JoystickButton(m_operatorController, 9);
+        JoystickButton rAnal = new JoystickButton(m_operatorController, 10);
+
 
         JoystickButton butXd = new JoystickButton(m_driverController, 3);  
         JoystickButton butAd = new JoystickButton(m_driverController, 1);  
         JoystickButton butBd = new JoystickButton(m_driverController, 2);
 
         JoystickButton gyro = new JoystickButton(m_driverController, 8);
 
-        JoystickButton rBump = new JoystickButton(m_operatorController, 6);
-        JoystickButton lBump = new JoystickButton(m_operatorController, 5);
-        JoystickButton lAnal = new JoystickButton(m_operatorController, 9);
-        JoystickButton rAnal = new JoystickButton(m_operatorController, 10);
+        butA.whenPressed(new ExtendIntake(intake, m_operatorController));
+        butB.whileHeld(new SpinTurret(turret, vision, 1, 0.25));
+        butX.whileHeld(new SpinTurret(turret, vision, 1, -0.25));
+        butY.whileHeld(new RunShooter(shooter));
+        lBump.whileHeld(new ControlConveyor(conveyor, 1));
+        rBump.whileHeld(new ControlConveyor(conveyor, -1));
+
+        // rWing for vision
+        lAnal.whileHeld(new MoveHood(shooter, 1));
+        rAnal.whileHeld(new MoveHood(shooter, -1));
+
+
 
         // B
         JoystickButton turbo = new JoystickButton(m_driverController, 2);
@@ -154,37 +171,6 @@ private void configureButtonBindings() {
         SmartDashboard.putData("Slalom Path", Slolam);
 
 
-        // A button 
-        butA.whileHeld(new ExtendIntake(intake, m_operatorController));
-        butA.whenReleased(new RetractIntake(intake));
-
-        // right bumper
-        rBump.whileHeld(new RunShooter(shooter));
-        rBump.whenReleased(new StopShooter(shooter));
-
-        // left analog center
-        lAnal.whileHeld(new MoveHood(shooter, -1));
-
-
-        // right analog center
-        rAnal.whileHeld(new MoveHood(shooter, 1));
-
-        // right bumper
-        rBump.whileHeld(new FeedShooter(conveyor, shooter));
-        rBump.whenReleased(new AutoIndexConveyor(conveyor));
-
-        // left bumper
-        lBump.whileHeld(new ControlConveyor(conveyor));
-        lBump.whenReleased(new AutoIndexConveyor(conveyor));
-
-        // B button
-        butB.whileHeld(new SpinTurret(turret, true, 0.25));
-        butB.whenReleased(new SpinTurret(turret, true, 0));
-
-        // Y button
-        butY.whileHeld(new SpinTurret(turret, true, -0.25));
-        butY.whenReleased(new SpinTurret(turret, true, 0));
-
         // driver X button - slow
         butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 0.35));
         butAd.whenPressed(new InstantCommand(swerveDrive::zeroWheels));
@@ -195,11 +181,6 @@ private void configureButtonBindings() {
 
         gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading));
 
-        //new JoystickButton(m_operatorController, 4).whenPressed(new RunCommand(() -> conveyor.manualControl(-), conveyor))
-        //        .whenReleased(new RunCommand(conveyor::autoIndex, conveyor));
-        // should be start button for camera to find target idk what number is so fix it
-        // new JoystickButton(m_operatorController, 7).whenHeld(new InstantCommand(turret::visionTurret, turret));
-
 }
 
 public static String getCoords() {

RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java

@@ -16,10 +16,10 @@ public class ControlConveyor extends CommandGroup {
   /**
    * Creates a new ControlConveyor.
    */
-  public ControlConveyor(Conveyor subsystem) {
+  public ControlConveyor(Conveyor subsystem, double speed) {
     requires(subsystem);
     m_conveyor = subsystem;
-    m_speed = 1;
+    m_speed = speed;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 

RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java

@@ -112,7 +112,6 @@ public void execute() {
     // fill with correct button
     if (m_joystick.getRawButtonPressed(6)) {
       // to zero all wheels
-      SmartDashboard.putNumber("BUTTON PRESSED", 5);
     }
 
     // sidestep

RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java

@@ -20,6 +20,7 @@ public class MoveHood extends CommandGroup {
   public MoveHood(Shooter subsystem, double pos) {
     requires(subsystem);
     m_shooter = subsystem;
+    m_pos = pos;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 

RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java

@@ -9,14 +9,16 @@
 
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import frc.robot.subsystems.Turret;
+import frc.robot.subsystems.Vision;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
 public class SpinTurret extends InstantCommand {
-  public SpinTurret(Turret subsystem, boolean manual, double speed) {
-    super(subsystem, () -> {subsystem.spin(manual, speed);});
-    requires(subsystem);
+  public SpinTurret(Turret subsystem1, Vision subsystem2, int mode, double speed) {
+    super(subsystem1, () -> {subsystem1.spin(subsystem2, mode, speed);});
+    requires(subsystem1);
+    requires(subsystem2);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 

RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java

@@ -178,6 +178,7 @@ public void resetEncoders() {
    */
   public void zeroHeading() {
     navX.reset();
+    resetEncoders();
     gyroReset = true;
   }
 

RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java

@@ -30,7 +30,7 @@ public class Turret extends Subsystem {
   // transfer to robot container
   private static final int motor = 10;
   private final CANSparkMax turretMotor;
-  private final CANPIDController m_turretPIDController;
+  private final PIDController m_turretPIDController;
   private CANEncoder turretEncoder;
   public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, setPoint, rotations;
   private CANDigitalInput m_reverseLimit;
@@ -63,7 +63,7 @@ public Turret(SwerveDrive swerve) {
     turretEncoder = turretMotor.getEncoder();
     turretEncoder.setPosition(0);
 
-    m_turretPIDController = turretMotor.getPIDController();
+    m_turretPIDController = new PIDController(0, 0, 0);
 
     // PID coefficients
     kP = 0.1;
@@ -78,9 +78,9 @@ public Turret(SwerveDrive swerve) {
     m_turretPIDController.setP(kP);
     m_turretPIDController.setI(kI);
     m_turretPIDController.setD(kD);
-    m_turretPIDController.setIZone(kIz);
-    m_turretPIDController.setFF(kFF);
-    m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput);
+    // m_turretPIDController.setIZone(kIz);
+    // m_turretPIDController.setFF(kFF);
+    // m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput);
 
     // m_reverseLimit = turretMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     // m_reverseLimit.enableLimitSwitch(true);
@@ -95,7 +95,7 @@ public void periodic() {
   }
 
   public void setPosition(double setpoint) {
-    m_turretPIDController.setReference(setpoint, ControlType.kPosition);
+    turretMotor.set(m_turretPIDController.calculate(turretEncoder.getPosition(), 0));
     SmartDashboard.putNumber("SetPoint", setpoint);
     SmartDashboard.putNumber("ProcessVariable", turretEncoder.getPosition());
   }
@@ -104,7 +104,7 @@ public void stop() {
     turretMotor.set(0);
   }
 
-  public void spin(boolean manual, double speed) {
+  public void spin(Vision vision, int mode, double speed) {
     double robotHeading = swerve.getHeading();
 
     double targetPosition = 0;
@@ -115,12 +115,14 @@ public void spin(boolean manual, double speed) {
       targetPosition = 360 - robotHeading;
     } 
 
-    if (manual)
+    if (mode == 1)
       turretMotor.set(speed);
-     else
-      // turretMotor.set(0);
-      setPosition(targetPosition);
-        // m_turretPIDController.setReference(targetPosition, ControlType.kPosition);
+     else if (mode == 2) {
+
+       setPosition(targetPosition);
+     } else if (mode == 3) {
+       // vision stuff
+     }
   }
 
   @Override