Climb merge

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

@@ -65,10 +65,24 @@ public static class RollerandPivotConstants {
     public static final double pivotclockwise = 1;
     public static final double pivotcounterclockwise = -1;
     public static final double pastsensortime = 0.5;
+  }
+
 
 
 
+  public static class ClimbConstants {
+    public static final int LEFT_WINCH_MOTOR_ID = 21;
+    public static final int LEFT_ZERO_LIMIT_ID = 11;
 
+    public static final int RIGHT_WINCH_MOTOR_ID = 22;
+    public static final int RIGHT_ZERO_LIMIT_ID = 12;
+
+    public static final double WINCH_REDUCTION = 9.49;
+    public static final double AXLE_PERIMETER_METERS = 6 * Units.inchesToMeters(.289) ;
+
+    public static final double EXTENSION_LIMIT_METERS = Units.inchesToMeters(39);
+    public static final double EXTENSION_TOLERANCE_METERS = 0.01;
 
+    public static final double MAX_WINCH_POWER = 0.6;
   }
 }

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

@@ -13,6 +13,9 @@
 import frc.robot.controllers.BaseDriveController;
 import frc.robot.controllers.DualJoystickDriveController;
 import frc.robot.controllers.XboxDriveController;
+import frc.robot.commands.climb.ClimbLowerCommand;
+import frc.robot.commands.climb.ClimbRaiseCommand;
+import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.swerve.BaseSwerveSubsystem;
 import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem;
 import frc.robot.subsystems.swerve.SwerveModule;
@@ -29,6 +32,8 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import static frc.robot.Constants.SwerveConstants.*;
 
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
@@ -41,6 +46,8 @@ public class RobotContainer {
     private final ShooterFeederSubsystem feederSubsystem;
     private final ShooterPivotSubsystem shooterPivotSubsystem;
 
+    private final ClimbSubsystem climbSubsystem;
+
 
     private final XboxController mechController = new XboxController(2);
     private final JoystickButton aButton = new JoystickButton(mechController, XboxController.Button.kA.value);
@@ -65,6 +72,8 @@ public RobotContainer() {
 
       shooterPivotSubsystem = new ShooterPivotSubsystem(false);
       shooterSubsystem = new ShooterFlywheelSubsystem();
+
+      climbSubsystem = new ClimbSubsystem();
 
       traj = Choreo.getTrajectory("Curve");
 
@@ -173,7 +182,7 @@ private void configureBindings() {
         }
 
 
-    } 
+    }
 
   public Command getAutonomousCommand() {
     if(baseSwerveSubsystem instanceof SwerveSubsystem){

src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java

@@ -0,0 +1,33 @@
+package frc.robot.commands.climb;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climb.ClimbSubsystem;
+
+public class ClimbLowerCommand extends Command {
+    private ClimbSubsystem climbSubsystem;
+
+    public ClimbLowerCommand(ClimbSubsystem climbSubsystem){
+        this.climbSubsystem = climbSubsystem;
+        this.addRequirements(climbSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        System.out.println("LOWERING CLIMB...");
+        climbSubsystem.goToExtension(0);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return climbSubsystem.isAtExtension();
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public void end(boolean interrupted) {
+        System.out.println(interrupted ? "CLIMB LOWERING INTERRUPTED!" : "CLIMB LOWERED!");
+    }
+
+}

src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java

@@ -0,0 +1,34 @@
+package frc.robot.commands.climb;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climb.ClimbSubsystem;
+import static frc.robot.Constants.ClimbConstants.*;
+
+public class ClimbRaiseCommand extends Command {
+    private ClimbSubsystem climbSubsystem;
+
+    public ClimbRaiseCommand(ClimbSubsystem climbSubsystem){
+        this.climbSubsystem = climbSubsystem;
+        this.addRequirements(climbSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        System.out.println("RAISING CLIMB...");
+        climbSubsystem.goToExtension(EXTENSION_LIMIT_METERS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return climbSubsystem.isAtExtension();
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public void end(boolean interrupted) {
+        System.out.println(interrupted ? "CLIMB RAISING INTERRUPTED!" : "CLIMB RAISED!");
+    }
+
+}

src/main/java/frc/robot/subsystems/climb/ClimbArm.java

@@ -0,0 +1,66 @@
+package frc.robot.subsystems.climb;
+
+import static frc.robot.Constants.ClimbConstants.*;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkBase.SoftLimitDirection;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.util.MotorUtil;
+
+public class ClimbArm {
+    private final CANSparkMax winchMotor;
+    private RelativeEncoder extensionEncoder;
+
+    private final DigitalInput zeroLimitSwitch;
+
+    private double desiredExtension = 0;
+
+    public ClimbArm(int WINCH_MOTOR_ID, int ZERO_LIMIT_ID) {
+        winchMotor = MotorUtil.createSparkMax(WINCH_MOTOR_ID, (sparkMax) -> {
+            sparkMax.setIdleMode(IdleMode.kBrake); 
+            sparkMax.setInverted(false);
+
+            extensionEncoder = sparkMax.getEncoder();
+            extensionEncoder.setPositionConversionFactor(AXLE_PERIMETER_METERS / WINCH_REDUCTION);
+            extensionEncoder.setPosition(0);
+
+            sparkMax.setSoftLimit(SoftLimitDirection.kForward, (float) EXTENSION_LIMIT_METERS);
+            sparkMax.enableSoftLimit(SoftLimitDirection.kForward, true);
+            sparkMax.setSoftLimit(SoftLimitDirection.kReverse, 0);
+            sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true);
+        });
+
+        zeroLimitSwitch = new DigitalInput(LEFT_ZERO_LIMIT_ID);
+    }
+
+    public void update() {
+        if (zeroLimitSwitch != null && zeroLimitSwitch.get())
+            resetEncoder();
+
+        if (extensionEncoder.getPosition() == 0 && !zeroLimitSwitch.get())
+            System.out.println(this.toString() + "encoder uncalibrated!");
+
+        if (isAtExtension())
+            winchMotor.set(0);
+        else
+            winchMotor.set(desiredExtension > extensionEncoder.getPosition()
+                ? MAX_WINCH_POWER : -MAX_WINCH_POWER);
+
+    }
+
+    public void goToExtension(double desiredHeight) {
+        desiredExtension = MathUtil.clamp(desiredHeight, 0, EXTENSION_LIMIT_METERS);
+    }
+
+    public boolean isAtExtension() {
+        return Math.abs(extensionEncoder.getPosition() - desiredExtension) < EXTENSION_TOLERANCE_METERS; 
+    }
+
+    private void resetEncoder() {
+        extensionEncoder.setPosition(0);
+    }
+}

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.climb;
+
+import static frc.robot.Constants.ClimbConstants.*;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ClimbSubsystem extends SubsystemBase{
+    private final ClimbArm leftClimbArm;
+    private final ClimbArm rightClimbArm;
+
+    public ClimbSubsystem() {
+        leftClimbArm = new ClimbArm(LEFT_WINCH_MOTOR_ID, LEFT_ZERO_LIMIT_ID);
+        rightClimbArm = new ClimbArm(RIGHT_WINCH_MOTOR_ID, RIGHT_ZERO_LIMIT_ID);
+    }
+
+    @Override
+    public void periodic() {
+        leftClimbArm.update();
+        rightClimbArm.update();
+    }
+
+    public void goToExtension(double height) {
+        leftClimbArm.goToExtension(height);
+        rightClimbArm.goToExtension(height);
+    }
+
+    public boolean isAtExtension() {
+        return leftClimbArm.isAtExtension() && rightClimbArm.isAtExtension();
+    }
+}

src/main/java/frc/robot/subsystems/swerve/SwerveModule.java

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.swerve;
 
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.RelativeEncoder;
+// import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.CANSparkBase.ControlType;
@@ -23,7 +23,7 @@ public class SwerveModule {
     private final SwerveDriveMotor driveMotor;
 
     private final CANSparkMax steerMotor;
-    private RelativeEncoder steerRelativeEncoder;
+    // private RelativeEncoder steerRelativeEncoder;
     private SparkAnalogSensor steerAbsoluteEncoder;
     private SparkPIDController steerPidController;