3/13's changes

src/main/java/frc4388/robot/Constants.java

@@ -67,7 +67,7 @@ public final class Constants {
         public static final class PIDConstants {
             public static final int SWERVE_SLOT_IDX = 0;
             public static final int SWERVE_PID_LOOP_IDX = 1;
-            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.2, 0.0, 0, 0.0);
+            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.2, 0, 0.0);
         }
     
         public static final class AutoConstants {
@@ -191,8 +191,8 @@ public final class Constants {
 
     public static final class ClimbConstants {
         public static final int CLIMB_MOTOR_ID = 19;
-        public static final double CLIMB_OUT_SPEED = 0.3;
+        public static final double CLIMB_IN_SPEED = -1.0;
-        public static final double CLIMB_IN_SPEED = -0.3;
+        public static final double CLIMB_OUT_SPEED = 0.87;
     }
 
     public static final class OIConstants {

src/main/java/frc4388/robot/RobotContainer.java

@@ -327,14 +327,6 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
 
-        new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
 
@@ -430,8 +422,12 @@ public class RobotContainer {
             .onTrue(emergencyRetract);
 
         new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotShooter.idle()))
+            .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-            .onFalse(new InstantCommand(() -> m_robotShooter.stop()));
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
+            .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
 
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))

src/main/java/frc4388/robot/RobotMap.java

@@ -149,7 +149,7 @@ public class RobotMap {
         rightFrontSteer.setNeutralMode(NeutralMode.Brake);
         leftBackSteer.setNeutralMode(NeutralMode.Brake);
         rightBackSteer.setNeutralMode(NeutralMode.Brake);
-
+        
         // initialize SwerveModules
         this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
         this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -7,10 +7,12 @@ package frc4388.robot.subsystems;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
 import frc4388.robot.Constants.ClimbConstants;
+import frc4388.robot.Constants.IntakeConstants;
 
 //! 6.5C Scoring Criteria for Stage
 
@@ -21,6 +23,7 @@ public class Climber extends SubsystemBase {
   public Climber(TalonFX climbMotor) {
     this.climbMotor = climbMotor;
     this.climbMotor.setInverted(true);
+    
     var slot0Configs = new Slot0Configs();
     slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
     slot0Configs.kI = 0.0; // no output for integrated error
@@ -30,15 +33,17 @@ public class Climber extends SubsystemBase {
   }
 
   public void climbOut() {
-    PositionVoltage request = new PositionVoltage(0);
+    //PositionVoltage request = new PositionVoltage(0);
-    climbMotor.setControl(request.withPosition(-520)); 
+    //climbMotor.setControl(request.withPosition(-520)); 
-    //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    
+    climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED);
   }
 
   public void climbIn() {
-    PositionVoltage request = new PositionVoltage(-520);
+    //PositionVoltage request = new PositionVoltage(-520);
-    climbMotor.setControl(request.withPosition(0)); 
+    //climbMotor.setControl(request.withPosition(0)); 
-    // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    ;
   }
 
   public void stopClimb() {
@@ -48,5 +53,6 @@ public class Climber extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue());
   }
 }