Climb subsystem

.vscode/settings.json

@@ -58,5 +58,6 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "wpilib.autoStartRioLog": true,
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable",
+  "java.checkstyle.configuration": "/google_checks.xml"
 }

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

@@ -4,8 +4,15 @@
 
 package frc.robot;
 
+import java.util.OptionalInt;
+
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.util.Motors.LoggedSparkMaxConfig;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -20,7 +27,42 @@ public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
   }
 
-  /** Constants for the swerve subsystem. */
+  public static class BackAlgaeConstants{
+    public static final int ROLLER_MOTOR_ID = 1;//Placeholder
+    public static final int PIVOT_MOTOR_ID = 2;//Placeholder
+    public static final double PIVOT_P = 0.01;//placeholder value
+    public static final double PIVOT_I = 0;//placeholder value
+    public static final double PIVOT_D = 0;//placeholder value
+
+    public static final int SENSOR_ID = 0;//placeholder value
+    public static final double PIVOT_POSITION_1 = 0;//placeholder value
+    public static final double PIVOT_POSITION_2 = 0;//placeholder value
+  }
+
+  public static class ClimbConstants{
+    public static final int TOP_MOTOR_ID = 9;//Placeholder
+    public static final int BOT_MOTOR_ID = 10;//Placeholder
+    public static final LoggedSparkMaxConfig TOP_MOTOR_CONFIG = 
+      new LoggedSparkMaxConfig(
+        TOP_MOTOR_ID, 
+        new ClosedLoopConfig()
+          .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+          .outputRange(0, 1),
+        new EncoderConfig(),
+        OptionalInt.empty() 
+      );
+    public static final LoggedSparkMaxConfig BOT_MOTOR_CONFIG =
+      new LoggedSparkMaxConfig(
+        BOT_MOTOR_ID,
+        new ClosedLoopConfig()
+          .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+          .outputRange(0, 1),
+        new EncoderConfig(),
+        OptionalInt.of(TOP_MOTOR_ID)
+      );
+  }
+
+    /** Constants for the swerve subsystem. */
   public static class SwerveConstants {
     public static final int FL_DRIVE = 0;
     public static final int FL_STEER = 1;
@@ -46,22 +88,32 @@ public static class SwerveConstants {
 
     public static final double DRIVE_GEAR_REDUCTION = 9. * 20. / 26.;
     public static final double DRIVE_WHEEL_CIRCUMFERENCE = Units.inchesToMeters(4 * Math.PI);
-    public static final double MAX_VEL = 4200. / 6.923 / 60. * 2. * 2. * Math.PI * .0254; // Kraken speed / gear ratio / reduced to per second * circumference * convert to meters
+    public static final double MAX_VEL = 6000. / 6.923 / 60. * 2. * 2. * Math.PI * .0254; // Kraken speed / gear ratio / reduced to per second * circumference * convert to meters
     public static final double MAX_OMEGA = MAX_VEL / FL_POS.getNorm();
 
-    public static final double DRIVE_P = 0.32; // temporary value
-    public static final double DRIVE_I = 0; 
-    public static final double DRIVE_D = 0;
-    public static final double DRIVE_S = 0.1499;
-    public static final double DRIVE_V = 0.112;
+    public static final double[] DRIVE_P = new double[] {0.32, 0.32, 0.32, 0.32};
+    public static final double[] DRIVE_I = new double[] {0, 0, 0, 0}; 
+    public static final double[] DRIVE_D = new double[] {0, 0, 0, 0};
+    public static final double[] DRIVE_S = new double[] {0.1499, 0.1499, 0.1499, 0.1499};
+    public static final double[] DRIVE_V = new double[] {0.112, 0.112, 0.112, 0.112};
 
-    public static final double STEER_P = 5.8;
-    public static final double STEER_I = 0;
-    public static final double STEER_D = 0;
-    public static final double STEER_FF = 0;
+    public static final double[] STEER_P = new double[] {5.4, 5.4, 5.4, 5.4};
+    public static final double[] STEER_I = new double[] {0, 0, 0, 0};
+    public static final double[] STEER_D = new double[] {0, 0, 0, 0};
+    public static final double[] STEER_FF = new double[] {0.036, 0.024, 0.0182, 0.05};
   }
 
   public static class LoggingConstants{
     public static final String SWERVE_TABLE = "SwerveStats";
+    public static final String REV_TABLE = "MotorStats";
+    public static final String CTRE_TABLE = "MotorStats";
+  }
+
+  public static class DebugConstants{
+    public static final boolean STATE_DEBUG = false;
+    public static final boolean DRIVE_DEBUG = false;
+    public static final boolean STEER_DEBUG = false;
+    public static final boolean REV_DEBUG = false;
+    public static final boolean CTRE_DEBUG = false;
   }
 }

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

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import frc.robot.Constants.ClimbConstants;
 import frc.robot.controllers.BaseDriveController;
 import frc.robot.controllers.DualJoystickDriveController;
 import frc.robot.controllers.PS5DriveController;
@@ -13,12 +14,15 @@
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.PS5Controller;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem;
-import frc.robot.subsystems.PhoenixLoggingSubsystem.PhoenixLoggingSubsystem;
+import frc.robot.subsystems.ClimbSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 
 /**
@@ -30,15 +34,28 @@
 public class RobotContainer {
 
   private BaseDriveController driveController;
+  private CommandPS5Controller mechController = new CommandPS5Controller(1);
 
   private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
+  private final ClimbSubsystem climbSubsystem = new ClimbSubsystem();
 
-  private final FieldManagementSubsystem fieldManagementSubsystem = new FieldManagementSubsystem();
-  private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = new PhoenixLoggingSubsystem(fieldManagementSubsystem);
+  private final Trigger createTrigger;
+  private final Trigger optionTrigger;
+  private final Trigger xbutton;
+
+  // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem =
+    // new PhoenixLoggingSubsystem(fieldManagementSubsystem);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     constructDriveController(); 
+
+    createTrigger = new Trigger(mechController.create());
+    optionTrigger = new Trigger(mechController.options());
+    xbutton = new Trigger(mechController.cross());
+
+    climbSubsystem.resetEncoderPos();
+
     startLog();
     configureBindings();
   }
@@ -58,6 +75,15 @@ private void configureBindings() {
       * the robot is controlled along its own axes, otherwise controls apply to the field axes by default. If the
       * swerve aim button is held down, the robot will rotate automatically to always face a target, and only
       * translation will be manually controllable. */
+
+      //HYPOTHETICAL BACK ALGAE CODE
+      /*
+       * Bind a button to move pivot into intaking position, rollers start rolling automatically
+       *  Rollers stop when you press the button again or when distance  sensor detects ball
+       * 
+       * Bind a button to move pivot into outtaking position
+       * Pressing that button twice spits the algae out
+       */
     swerveSubsystem.setDefaultCommand(
       new RunCommand(() -> {
         swerveSubsystem.setDrivePowers(
@@ -77,8 +103,45 @@ private void configureBindings() {
       },
       swerveSubsystem
     );
+
+    // climbSubsystem.setDefaultCommand(
+    //   new InstantCommand(() -> {
+    //     climbSubsystem.setSpeed(mechcontroller.getL2Axis());
+    //     setRum(mechcontroller.getL2Axis());
+
+    //   }, climbSubsystem)
+    // );
+
+    createTrigger.and(optionTrigger).whileTrue(
+      new RunCommand(() -> {
+        climbSubsystem.setSpeed(0.3);
+        System.out.println(climbSubsystem.getPosition());
+
+      }, climbSubsystem)
+    ).onFalse(
+      new RunCommand(() -> {
+        climbSubsystem.setSpeed(0);
+
+      }, climbSubsystem)
+    );
+
+    xbutton.whileTrue(
+      new RunCommand(() -> {
+        climbSubsystem.setSpeed(-0.3);
+        System.out.println(climbSubsystem.getPosition());
+
+      }, climbSubsystem)
+    ).onFalse(
+      new RunCommand(() -> {
+        climbSubsystem.setSpeed(0);
+
+      }, climbSubsystem)
+    );
   }
 
+
+
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
@@ -93,6 +156,7 @@ public Command getAutonomousCommand() {
    * 0
    */
   private void constructDriveController(){
+    DriverStation.refreshData();
     if(DriverStation.getJoystickName(0).equals("Controller (Xbox One For Windows)")) {
         driveController = new XboxDriveController();
     }
@@ -109,8 +173,7 @@ else if(DriverStation.getJoystickName(0).equals("DualSense Wireless Controller")
    * Starts datalog at /media/sda1/robotLogs
    */
   private void startLog(){
-    DataLogManager.start("/media/sda1/robotLogs");
+    DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
-
   }
 }

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

@@ -0,0 +1,124 @@
+// 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 edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_CONFIG;
+import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_ID;
+import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_CONFIG;
+import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_ID;
+import static frc.robot.Constants.DebugConstants.REV_DEBUG;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SoftLimitConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import frc.robot.util.Motors.LoggedSparkMax;
+
+public class ClimbSubsystem extends SubsystemBase {
+
+  private final SparkMax topMotor;
+  private final SparkMax botMotor;
+  private double targetSpeed = 0;
+
+  private ClosedLoopConfig closedLoopConfig;
+  private EncoderConfig encoderConfig;
+  private SoftLimitConfig softLimitConfig;
+
+  private RelativeEncoder encoder;
+  private SparkMaxConfig topSparkMaxConfig, botSparkMaxConfig;
+
+  /** Creates a new ExampleSubsystem. */
+  public ClimbSubsystem() {
+    topMotor = new SparkMax(TOP_MOTOR_ID, MotorType.kBrushless);
+    botMotor = new SparkMax(BOT_MOTOR_ID, MotorType.kBrushless);
+
+
+    closedLoopConfig = new ClosedLoopConfig()
+      .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+      .outputRange(0, 1);
+    encoderConfig = new EncoderConfig();
+    encoder = topMotor.getEncoder();
+
+    softLimitConfig = new SoftLimitConfig();
+      softLimitConfig.forwardSoftLimitEnabled(true)
+                     .forwardSoftLimit(67)
+                     .reverseSoftLimitEnabled(true)
+                     .reverseSoftLimit(0);
+
+
+    topSparkMaxConfig = new SparkMaxConfig();
+    topSparkMaxConfig.apply(closedLoopConfig)
+                  .apply(softLimitConfig)
+                  .inverted(true);
+
+    botSparkMaxConfig = new SparkMaxConfig();
+    botSparkMaxConfig.apply(closedLoopConfig)
+                  .inverted(true)
+                  .apply(softLimitConfig)
+                  .follow(TOP_MOTOR_ID);
+
+    topMotor.configure(topSparkMaxConfig,
+    ResetMode.kResetSafeParameters, PersistMode.kPersistParameters
+    );
+    botMotor.configure(botSparkMaxConfig,
+    ResetMode.kResetSafeParameters, PersistMode.kPersistParameters
+    );
+
+  }
+
+  @Override
+  public void periodic() {
+    topMotor.set(targetSpeed);
+    // topMotor.logStats();
+    // botMotor.logStats();
+    if(REV_DEBUG){
+      // topMotor.publishStats();
+      // botMotor.publishStats();
+    }
+  }
+
+  /**
+   * Set encoder position to 0
+   */
+  public void resetEncoderPos(){
+    encoder.setPosition(0);
+  }
+
+
+  /**
+   * Gets the position of the top motor
+   * @return position of the top motor in rotations after taking the position
+   * conversion factor into account
+   */
+  public double getPosition() {
+    return encoder.getPosition();
+  }
+  /**
+   * Gets the velocity of the top motor
+   * @return velocity of the top motor in RPM after taking the velocity
+   * conversion factor into account
+   */
+  public double getVelocity() {
+    return encoder.getVelocity();
+  }
+
+  /**
+   * Sets the speed of the motors
+   * @param speed target speed from -1.0 to 1.0
+   */
+  public void setSpeed(double speed){
+    targetSpeed = speed;
+  }
+}