basic climber code (DRIVE STILL NOT WORKING)

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

@@ -29,6 +29,7 @@ import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 import java.util.stream.Collectors;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.diffplug.common.base.Errors;
 import com.pathplanner.lib.PathPlanner;
@@ -114,9 +115,10 @@ public class RobotContainer {
   public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
   public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
   public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
-  private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30);
-  private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31);
-  public final Climber m_robotClimber = new Climber(testElbowMotor);
+
+  private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
+  private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
+  public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
 
   // Controllers
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -145,7 +147,6 @@ public class RobotContainer {
    */
   public RobotContainer() {
     configureButtonBindings();
-    // testShoulderMotor.setNeutralMode(NeutralMode.Brake);
     /* Default Commands */
 
       // Swerve Drive with Input
@@ -170,9 +171,9 @@ public class RobotContainer {
     //                       m_robotBoomBoom, 
     //                       m_robotTurret).withName("Storage ManageStorage defaultCommand"));
     
-    // m_robotClimber.setDefaultCommand(
-    //   new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber)
-    // );
+    m_robotClimber.setDefaultCommand(
+      new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber)
+    );
       // Storage Management
     /*m_robotStorage.setDefaultCommand(
         new RunCommand(() -> m_robotStorage.manageStorage(), 
@@ -186,7 +187,6 @@ public class RobotContainer {
         new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), 
         m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
 
-
     m_robotHood.setDefaultCommand(
        new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
     // m_robotTurret.setDefaultCommand(
@@ -215,11 +215,19 @@ public class RobotContainer {
     new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
       .whenPressed(m_robotSwerveDrive::resetGyro);
       // Left Bumper > Shift Down
-    new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
-        .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
-      // Right Bumper > Shift Up
-    new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
-        .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
+    // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
+    //     .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
+    //   // Right Bumper > Shift Up
+    // new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
+    //     .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
+
+    new JoystickButton(getDriverController(), XboxController.Axis.kLeftTrigger.value)
+      .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.1), m_robotClimber))
+      .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0)));
+      
+      new JoystickButton(getDriverController(), XboxController.Axis.kRightTrigger.value)
+      .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(-0.1), m_robotClimber))
+      .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0)));
     
     // new JoystickButton(getDriverController(), XboxController.Button.kA.value)
     // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025));
@@ -238,6 +246,8 @@ public class RobotContainer {
     //     .whenPressed(() -> m_robotMap.leftBack.reset())
     //     .whenPressed(() -> m_robotMap.rightBack.reset()); 
 
+
+
     /* Operator Buttons */
 
       // X > Extend Intake

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

@@ -4,21 +4,41 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Climber extends SubsystemBase {
+
+  WPI_TalonFX m_climberShoulder;
   WPI_TalonFX m_climberElbow;
+
   /** Creates a new Climber. */
-  public Climber(WPI_TalonFX climberElbow) {
+  public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) {
+    m_climberShoulder = climberShoulder;
     m_climberElbow = climberElbow;
+
+    m_climberShoulder.configFactoryDefault();
+    m_climberElbow.configFactoryDefault();
+    m_climberShoulder.setNeutralMode(NeutralMode.Brake);
+    m_climberElbow.setNeutralMode(NeutralMode.Brake);
   }
 
-  public void runWithInput(double input){
+  public void runShoulderWithInput(double input) {
+    m_climberShoulder.set(input);
+  }
+
+  public void runElbowWithInput(double input){
     m_climberElbow.set(input);
   }
+
+  public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) {
+    m_climberShoulder.set(shoulderInput);
+    m_climberElbow.set(elbowInput);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

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

@@ -122,7 +122,7 @@ public class SwerveDrive extends SubsystemBase {
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2)))
+            rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)))
         : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
         chassisSpeeds);