Nosoup831/15/climber

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

@@ -18,6 +18,7 @@ public final class Constants {
     public static final int CAN_ID_BACK_LEFT_DRIVE_MOTOR = 12;
     public static final int CAN_ID_BACK_RIGHT_DRIVE_MOTOR = 10;
     public static final int USB_PORT_DRIVER_CONTROLLER = 0;
+    public static final int USB_PORT_OPERATOR_CONTROLLER = 1;
     public static final double DRIVEBASE_DEADBAND = 0.02; // anything under 1.5% of full joystick away from 0 should be considered 0
     public static final double DRIVEBASE_TURN_SLEW_LIMIT = 2.0; // 0 to 200% in one second. 0 to full in 1/2 second.
     public static final double DRIVEBASE_FWD_BACK_SLEW_LIMIT = 1; // 0 to 100% in one second.
@@ -67,5 +68,58 @@ public final class Constants {
     public static final double INTAKE_SPEED = 0.25;
     public static final double INTAKE_EJECT_SPEED = -0.5;
 
+    //Climber Constants
+    /**
+     * The proportional constant for the climber
+     */
+    public static final double CLIMBER_P_CONSTANT = 0;
+    /**
+     * The integral constant for the climber
+     */
+    public static final double CLIMBER_I_CONSTANT = 0;
+    /**
+     * The derivative constant for the climber
+     */
+    public static final double CLIMBER_D_CONSTANT = 0;
+    /**
+     * The CAN ID of the climber motor
+     */
+    public static final int CAN_ID_CLIMBER_MOTOR = 0;
+    /**
+     * The PID error of the climber
+     */
+    public static final int CLIMBER_PID_ERROR = 10;
+    /**
+     * The max amount of rotations extending the climber
+     */
+    public static final int CLIMBER_SOFT_LIMIT_FORWARD = 20;
+    /**
+     * The maximum a mount of rotations retracting the climber
+     */
+    public static final int CLIMBER_SOFT_LIMIT_BACK = -1;
+    /**
+     * The port number of the double solenoid when it folds the climber up
+     */
+    public static final int DOUBLE_SOLENOID_CLIMBER_UP = 0;
+    /**
+     * The port number of the double solenoid when it folds the climber down
+     */
+    public static final int DOUBLE_SOLENOID_CLIMBER_DOWN = 1;
+    /**
+     * The CAN ID of the back climb motor
+     */
+    public static final int CAN_ID_BACK_CLIMB_MOTOR = 1;
+    /**
+     * The proportional constant for the back climber
+     */
+    public static final int BACK_CLIMBER_P_CONSTANT = 0;
+    /**
+     * The integral constant for the back climber
+     */
+    public static final int BACK_CLIMBER_I_CONSTANT = 0;
+    /**
+     * The derivative constant for the back climber
+     */
+    public static final int BACK_CLIMBER_D_CONSTANT = 0;
 }
 

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

@@ -13,7 +13,14 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.ClimberBackC;
+import frc.robot.commands.ClimberExtendsC;
+import frc.robot.commands.ClimberForwardC;
+import frc.robot.commands.ClimberRetractsC;
+import frc.robot.commands.ClimberForwardC;
+import frc.robot.commands.ClimberBackC;
 import frc.robot.commands.ShooterC;
+import frc.robot.subsystems.ClimberS;
 import frc.robot.subsystems.DrivebaseS;
 import frc.robot.subsystems.ShooterS;
 import frc.robot.subsystems.TurretS;
@@ -35,22 +42,29 @@ public class RobotContainer {
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   private XboxController driverController;
+  private XboxController operatorController;
   private Command xboxDriveCommand;
   private Command xboxShooterCommand;
   private DrivebaseS drivebaseS;
   private ShooterS shooterS;
   private TurretS turretS;
+  private ClimberS climberS;
 
 
   private Command runTurretC;
   private Command turretHomingC;
   private Command turretTurningC;
-
+  private Command climberForwardC;
+  private Command climberBackC;
 
   // Trigger definitions
   private Trigger spinTurretTrigger;
   private Trigger turretHomeTrigger;
   private Trigger turretTurnTrigger;
+  //private Trigger extendsClimber;
+  //private Trigger retractClimber;
+  private Trigger climberForward;
+  private Trigger climberBack;
 
   public RobotContainer() {
     // Configure the button bindings
@@ -70,17 +84,27 @@ public RobotContainer() {
    */
   private void configureButtonBindings() {
 
-    new Trigger(driverController::getAButton).whileActiveOnce(xboxShooterCommand);
-    spinTurretTrigger = new Trigger(driverController::getBButton);
+    //new Trigger(driverController::getAButton).whileActiveOnce(xboxShooterCommand);
+    /*spinTurretTrigger = new Trigger(driverController::getBButton);
     spinTurretTrigger.whileActiveOnce(runTurretC);
     turretHomeTrigger = new Trigger(driverController::getXButton);
     turretHomeTrigger.whenActive(turretHomingC);
     turretTurnTrigger = new Trigger(driverController::getYButton);
-    turretTurnTrigger.whenActive(turretTurningC);
+    turretTurnTrigger.whenActive(turretTurningC);*/
+    //extendsClimber = new Trigger(operatorController::getAButton);
+    //extendsClimber.whenActive(new ClimberExtendsC(climberS));
+    //retractClimber = new Trigger(operatorController::getBButton);
+    //retractClimber.whenActive(new ClimberRetractsC(climberS));
+    climberForward = new Trigger(operatorController::getAButton);
+    climberForward.whileActiveOnce(climberForwardC);
+    climberBack = new Trigger(operatorController::getBButton);
+    climberBack.whileActiveOnce(climberBackC);
+
   }
 
   private void createControllers() {
     driverController = new XboxController(Constants.USB_PORT_DRIVER_CONTROLLER);
+    operatorController = new XboxController(Constants.USB_PORT_OPERATOR_CONTROLLER);
   }
 
   private void createCommands() {
@@ -104,11 +128,15 @@ private void createCommands() {
     turretTurningC = TurretCommandFactory.createTurretTurnC(40, turretS);
 
     SmartDashboard.putData(new InstantCommand(turretS::resetEncoder));
+
+    climberForwardC = new ClimberForwardC(climberS);
+    climberBackC = new ClimberBackC(climberS);
   }
 
   private void createSubsystems() {
     drivebaseS = new DrivebaseS();
     turretS = new TurretS();
+    climberS = new ClimberS();
   }
 
   /**

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

@@ -0,0 +1,40 @@
+// 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.CommandBase;
+import frc.robot.subsystems.ClimberS;
+
+public class ClimberBackC extends CommandBase {
+  ClimberS climber;
+  /** Creates a new ClimberBackC. */
+  public ClimberBackC(ClimberS climber) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.climber = climber;
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    climber.climberDown();
+  }
+
+  // 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) {
+    climber.climberOff();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,43 @@
+// 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.CommandBase;
+import frc.robot.subsystems.ClimberS;
+
+/**
+ * Extends the back two arms
+ */
+public class ClimberExtendsArmTwoC extends CommandBase {
+  /** Creates a new ClimberExtendsArmTwoC. */
+  ClimberS climber;
+  public ClimberExtendsArmTwoC(ClimberS climbers) {
+    this.climber = climber;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    climber.setClimberRotations(9);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    climber.stopMotorArmTwo();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return climber.atTarget();
+  }
+}

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

@@ -0,0 +1,43 @@
+// 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.CommandBase;
+import frc.robot.subsystems.ClimberS;
+
+/**
+ * Extends the climber a certain amount of rotations
+ */
+public class ClimberExtendsC extends CommandBase {
+  /** Creates a new ClimberC. */
+  private ClimberS climber;
+  public ClimberExtendsC(ClimberS climber) {
+    this.climber = climber;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    climber.setClimberRotations(9);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    climber.stopMotor();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return climber.atTarget();
+  }
+}

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

@@ -0,0 +1,30 @@
+// 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.InstantCommand;
+import frc.robot.subsystems.ClimberS;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+
+/**
+ * Folds the Climber down 
+ */
+public class ClimberFoldDownC extends InstantCommand {
+  ClimberS climber;
+  public ClimberFoldDownC(ClimberS climbers) {
+    this.climber = climber;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    climber.climberDown();
+  }
+}

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

@@ -0,0 +1,30 @@
+// 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.InstantCommand;
+import frc.robot.subsystems.ClimberS;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+
+/**
+ * Folds the Climber up 
+ */
+public class ClimberFoldUpC extends InstantCommand {
+  ClimberS climber;
+  public ClimberFoldUpC(ClimberS climbers) {
+    this.climber = climber;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    climber.climberUp();
+  }
+}

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

@@ -0,0 +1,41 @@
+// 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.CommandBase;
+import frc.robot.subsystems.ClimberS;
+
+public class ClimberForwardC extends CommandBase {
+  ClimberS climber;
+  /** Creates a new ClimberForwardC. */
+  public ClimberForwardC(ClimberS climber) {
+    this.climber = climber;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.climber);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    climber.climberUp();  
+  }
+
+  // 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) {
+    climber.climberOff();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}