Merge pull request #9 from FRC7660/Climb-Work

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

@@ -37,6 +37,12 @@ public static enum Mode {
     REPLAY
   }
 
+  public static class Climb {
+    public static int MotorClimbID = 61;
+    public static double climbSpeed = 0.50;
+    public static double climbEncoderLimit = 1;
+  }
+
   public static class Funnel {
     public static int winchID = 51;
     public static double winchSpeed = 0.50; // Clockwise = positive, holds funnel down in position

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

@@ -28,7 +28,9 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.LiftFunnel;
+import frc.robot.commands.LowerClimb;
 import frc.robot.commands.TestAuto;
+import frc.robot.subsystems.Climb;
 import frc.robot.subsystems.Funnel;
 import frc.robot.subsystems.LEDsubsystem.*;
 import frc.robot.subsystems.drive.*;
@@ -51,6 +53,7 @@ public class RobotContainer {
   private final LEDlive ledLive;
   private SwerveDriveSimulation driveSimulation = null;
   private final Funnel funnel = new Funnel();
+  private final Climb climb = new Climb();
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -173,6 +176,9 @@ private void configureButtonBindings() {
     JoystickButton a = new JoystickButton(driver, XboxController.Button.kA.value);
     a.onTrue(new LiftFunnel(funnel));
 
+    JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value);
+    b.onTrue(new LowerClimb(climb));
+
     controller
         .a()
         .whileTrue(

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

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Climb;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LowerClimb extends Command {
+  /** Creates a new LowerClimb. */
+  private final Climb climb;
+
+  public LowerClimb(Climb climb) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.climb = climb;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    System.out.println("Climb is lowering");
+    climb.lower();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    climb.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return climb.climbFinished();
+  }
+}

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

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.sim.SparkMaxSim;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Climb extends SubsystemBase {
+  /** Creates a new Climb. */
+  private SparkMax motorClimb = new SparkMax(Constants.Climb.MotorClimbID, MotorType.kBrushless);
+
+  private RelativeEncoder encoderClimb;
+  private SparkMaxConfig configClimb;
+
+  private SparkMaxSim motorClimbSim;
+
+  public Climb() {
+    if (Constants.currentMode == Constants.Mode.SIM) {
+      motorClimbSim = new SparkMaxSim(motorClimb, DCMotor.getNEO(1));
+    }
+
+    encoderClimb = motorClimb.getEncoder();
+    configClimb = new SparkMaxConfig();
+    configClimb.idleMode(IdleMode.kBrake);
+    motorClimb.configure(
+        configClimb, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
+    encoderClimb.setPosition(0);
+  }
+
+  public Boolean climbFinished() {
+    double position = encoderClimb.getPosition();
+    if (position > Constants.Climb.climbEncoderLimit) {
+      System.out.println("Limit Reached");
+      return true;
+    }
+    return false;
+  }
+
+  public void lower() {
+    motorClimb.set(Constants.Climb.climbSpeed);
+  }
+
+  public void stop() {
+    motorClimb.set(0);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    if (climbFinished()) {
+      motorClimb.set(0);
+    }
+
+    SmartDashboard.putNumber("Climb-Pos", encoderClimb.getPosition());
+  }
+
+  public void simulationPeriodic() {
+    double velo = motorClimb.get() * 1.0;
+    double voltage = RoboRioSim.getVInVoltage();
+    motorClimbSim.iterate(velo, voltage, Constants.simCycleTime);
+  }
+}

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

@@ -53,7 +53,6 @@ public void stop() {
 
   public Boolean limitReached() {
     double position = encoderWinch.getPosition();
-    System.out.println("Value:" + position);
     if (position > Constants.Funnel.limit) {
       System.out.println("Limit Reached");
       return true;
@@ -68,7 +67,7 @@ public Boolean limitReached() {
   public void periodic() {
     // This method will be called once per scheduler run
     if (limitReached()) {
-      stop();
+      motorWinch.set(0);
     }
 
     SmartDashboard.putNumber("Funnel-Pos", encoderWinch.getPosition());