Ast 41 climber

simgui-ds.json

@@ -91,8 +91,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000005e0400008e02000000000000",
-      "useGamepad": true
+      "guid": "030000004c050000c405000000000000"
     }
   ]
 }

simgui.json

@@ -2,21 +2,32 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/ClimberSubsystem": "Subsystem",
       "/LiveWindow/DriveSubsystem": "Subsystem",
+      "/LiveWindow/IndexerSubsystem": "Subsystem",
+      "/LiveWindow/IntakeSubsystem": "Subsystem",
       "/LiveWindow/LEDSubsystem": "Subsystem",
+      "/LiveWindow/ShooterSubsystem": "Subsystem",
+      "/LiveWindow/TurretSubsystem": "Subsystem",
       "/LiveWindow/Ungrouped/DifferentialDrive[1]": "DifferentialDrive",
+      "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input",
       "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[3]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[4]": "PIDController",
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
       "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
       "/Shuffleboard/Dashboard/Field View": "Field2d",
-      "/Shuffleboard/Dashboard/Gyro": "Gyro"
+      "/Shuffleboard/Dashboard/Gyro": "Gyro",
+      "/SmartDashboard/Elevator Sim": "Mechanism2d"
     },
     "windows": {
       "/Shuffleboard/Dashboard/Field View": {
-        "image": "/Users/siddharthlohani/Downloads/2022-field.png",
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Elevator Sim": {
         "window": {
           "visible": true
         }
@@ -27,6 +38,11 @@
     "Shuffleboard": {
       "Dashboard": {
         "open": true
+      }
+    },
+    "SmartDashboard": {
+      "Elevator Sim": {
+        "open": true
       },
       "open": true
     },

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

@@ -33,7 +33,7 @@ public static final class RobotMap {
         public static final int kLeftDriveCAN1 = 4;
         public static final int kLeftDriveCAN2 = 5;
         public static final int kLeftDriveCAN3 = 6;
-        
+
         public static final int kFeederCAN = 10;
         public static final int kFlywheelCAN = 11;
         public static final int kTurretCANId = 20;
@@ -65,7 +65,7 @@ public static final class DriveConstants {
     }
 
     public static final class LEDConstants {
-        public static final int kPwmPort = 0; 
+        public static final int kPwmPort = 0;
         public static final int kLength = 144;
         public static final double kInterval = Units.secondsToMilliseconds(0.1);
         public static final double kIntervalTrail = Units.secondsToMilliseconds(0.1);
@@ -82,11 +82,13 @@ public static final class StatusConstants {
         public static final String kShooter = "shooter";
         public static final String kIntake = "intake";
     }
+
     public static final class TurretConstants {
         public static final double kVisionThreshold = 5;
 
         public static final double kMaxSpeed = 0.05;
     }
+
     public static final class Limelight {
         public static final double kLimeLightMountAngleDegrees = 25.0;
         public static final double kLimelightLensHeightInches = 20.0;
@@ -95,13 +97,13 @@ public static final class Limelight {
 
     public static final class Shooter {
         public static final int kPIDLoopIDx = 0;
-        public static final double kGains_VelocitkF = 0.048973143759; //Feed Forward
+        public static final double kGains_VelocitkF = 0.048973143759; // Feed Forward
         public static final double kGains_VelocitkP = 0.085035;
         public static final double kGains_VelocitkI = 0.00035;
         public static final double kGains_VelocitkD = 0;
-        public static final double kConversionFactor = 2048/600;
+        public static final double kConversionFactor = 2048 / 600;
         public static final int kTimeoutMs = 0;
-        
+
         public static final int kMaxSpeed = 20000;
 
         public static final double kFlywheelA = 0;
@@ -111,7 +113,17 @@ public static final class Shooter {
         public static final double kFeederA = 0;
         public static final double kFeederB = 1;
         public static final double kFeederC = 0;
+    }
 
+    public static final class Climber {
+        public static final float kUpperLimit = 500; // unknown
+        public static final float kLowerLimit = 0; // unknown
+        public static final double kElevatorSpeed = 0.2;
+        public static final int kLimitSwitch_Port = 0; // unknown
+        public static final int kClimberMotor_Port = 11;
+        public static final double kP = 1; // proportional to distance to target
+        public static final double kI = 0.0;
+        public static final double kD = 0.0;
     }
 
     public static final class Indexer {

src/main/java/frc/robot/Robot.java

@@ -13,9 +13,12 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the
+ * name of this class or
+ * the package after creating this project, you must also update the
+ * build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -24,40 +27,55 @@ public class Robot extends TimedRobot {
   private RobotContainer m_robotContainer;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
+   * This function is run when the robot is first started up and should be used
+   * for any
    * initialization code.
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   * This function is called every robot packet, no matter the mode. Use this for
+   * items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and
+   * test.
    *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and
    * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // and running subsystem periodic() methods. This must be called from the
+    // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -70,7 +88,8 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
   public void teleopInit() {
@@ -85,7 +104,8 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
   public void testInit() {

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

@@ -11,6 +11,9 @@
 import frc.robot.commands.indexer.ShootIndexer;
 import frc.robot.commands.indexer.LoadIndexer;
 import frc.robot.commands.auto.DriveToDistance;
+import frc.robot.commands.climber.Calibrate;
+import frc.robot.commands.climber.ElevatorDown;
+import frc.robot.commands.climber.ElevatorUp;
 import frc.robot.commands.drive.SimDrive;
 import frc.robot.commands.intake.IntakeRun;
 import frc.robot.commands.intake.ToggleIntake;
@@ -23,21 +26,19 @@
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.subsystems.ClimberSubsystem;
 
 public class RobotContainer {
 
   /* GAMEPADS */
   private static final PS4Controller driverGamepad = new PS4Controller(
-    Constants.RobotMap.kDriverControllerPort
-  );
+      Constants.RobotMap.kDriverControllerPort);
   private static final PS4Controller operatorGamepad = new PS4Controller(
-    Constants.RobotMap.kOperatorControllerPort
-  );
+      Constants.RobotMap.kOperatorControllerPort);
 
   private static final JoystickButton triangleButton = new JoystickButton(
-    operatorGamepad,
-    PS4Controller.Button.kTriangle.value
-  );
+      operatorGamepad,
+      PS4Controller.Button.kTriangle.value);
 
   private static final JoystickButton m_crossButton = new JoystickButton(driverGamepad, PS4Controller.Button.kCross.value);
   private static final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
@@ -48,40 +49,50 @@ public class RobotContainer {
   private final TurretSubsystem m_turretSubsystem = new TurretSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final IndexerSubsystem m_indexerSubsystem = new IndexerSubsystem();
+  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 
   private final JoystickButton m_circleButton = new JoystickButton(
-    driverGamepad,
-    PS4Controller.Button.kCircle.value
-  );
+      driverGamepad,
+      PS4Controller.Button.kCircle.value);
+
+  private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value);
+  private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value);
+  // private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
+  private final JoystickButton m_l1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kL1.value);
+  private final JoystickButton m_r1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kR1.value);
 
   public RobotContainer() {
     configureButtonBindings();
     CommandScheduler
-      .getInstance()
-      .schedule(new ToggleLED(m_ledSubsystem, true));
+        .getInstance()
+        .schedule(new ToggleLED(m_ledSubsystem, true));
+    CommandScheduler.getInstance().schedule(false, new Calibrate(m_climberSubsystem));
     m_shooterSubsystem.setDefaultCommand(new ManualShoot(m_shooterSubsystem, 40, 40));
     m_driveSubsystem.setDefaultCommand(
-      new SimDrive(
-        m_driveSubsystem,
-        2,
-        driverGamepad::getR2Axis,
-        driverGamepad::getL2Axis,
-        driverGamepad::getLeftX,
-        driverGamepad::getRightX
-      )
-    );
+        new SimDrive(
+            m_driveSubsystem,
+            2,
+            driverGamepad::getR2Axis,
+            driverGamepad::getL2Axis,
+            driverGamepad::getLeftX,
+            driverGamepad::getRightX));
     m_intakeSubsystem.setDefaultCommand(new IntakeRun(m_intakeSubsystem));
     m_indexerSubsystem.setDefaultCommand(new LoadIndexer(m_indexerSubsystem));
   }
 
   private void configureButtonBindings() {
     triangleButton.whileHeld(new AutoAimTurret(m_turretSubsystem, 0.05));
     m_circleButton.whenPressed(new ToggleIntake(m_intakeSubsystem));
+    // X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
+    m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up
+    m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down
+    m_l1Button.toggleWhenPressed(new ElevatorDown(m_climberSubsystem));
+    m_r1Button.toggleWhenPressed(new ElevatorUp(m_climberSubsystem));
     m_crossButton.whileHeld(new ShootIndexer(m_indexerSubsystem));
   }
 
   public Command getAutonomousCommand() {
-    return new DriveToDistance(m_driveSubsystem, 2);
+    return new ElevatorUp(m_climberSubsystem);
   }
 
 

src/main/java/frc/robot/commands/climber/Calibrate.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.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ClimberSubsystem;
+
+// for setting soft limits
+public class Calibrate extends CommandBase {
+  /** Creates a new Calibrate. */
+  private ClimberSubsystem m_climberSubsystem;
+
+  public Calibrate(ClimberSubsystem climbSubsystem) {
+    m_climberSubsystem = climbSubsystem;
+    addRequirements(m_climberSubsystem);
+  }
+
+  // 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() {
+    m_climberSubsystem.setSpeed(-0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_climberSubsystem.stop();
+    m_climberSubsystem.reset();
+    m_climberSubsystem.setSoftLimits();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_climberSubsystem.fullyRetracted();
+  }
+}