Climb subsystem

.vscode/settings.json

@@ -58,5 +58,5 @@
     "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 -Xmx4G -Xms100m -Xlog:disable"
 }

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(),
+        null
+      );
+    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

@@ -17,8 +17,6 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 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.swerve.SwerveSubsystem;
 
 /**
@@ -33,8 +31,8 @@ public class RobotContainer {
 
   private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
 
-  private final FieldManagementSubsystem fieldManagementSubsystem = new FieldManagementSubsystem();
-  private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = new PhoenixLoggingSubsystem(fieldManagementSubsystem);
+  // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem =
+    // new PhoenixLoggingSubsystem(fieldManagementSubsystem);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -58,6 +56,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(
@@ -93,6 +100,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 +117,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,52 @@
+// 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.wpilibj2.command.SubsystemBase;
+import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_CONFIG;
+import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_CONFIG;
+import static frc.robot.Constants.DebugConstants.REV_DEBUG;
+import frc.robot.util.Motors.LoggedSparkMax;
+
+public class ClimbSubsystem extends SubsystemBase {
+
+  private final LoggedSparkMax topMotor;
+  private final LoggedSparkMax botMotor;
+  private double targetSpeed;
+
+  /** Creates a new ExampleSubsystem. */
+  public ClimbSubsystem() {
+    topMotor = new LoggedSparkMax(TOP_MOTOR_CONFIG);
+    botMotor = new LoggedSparkMax(BOT_MOTOR_CONFIG);
+  }
+
+  @Override
+  public void periodic() {
+    topMotor.set(targetSpeed);
+    topMotor.logStats();
+    botMotor.logStats();
+    if(REV_DEBUG){
+      topMotor.publishStats();
+      botMotor.publishStats();
+    }
+  }
+
+  /**
+   * 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 topMotor.getPosition();
+  }
+
+  /**
+   * Sets the speed of the motors
+   * @param speed target speed from -1.0 to 1.0
+   */
+  public void setSpeed(double speed){
+    targetSpeed = speed;
+  }
+}

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

@@ -1,10 +1,15 @@
 package frc.robot.subsystems.PhoenixLoggingSubsystem;
 
+import javax.xml.crypto.Data;
+
 import com.ctre.phoenix6.SignalLogger;
 
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem;
 import frc.robot.subsystems.FieldManagementSubsystem.MatchStatus;
+import frc.robot.subsystems.FieldManagementSubsystem.RobotStatus;
 
 public class PhoenixLoggingSubsystem extends SubsystemBase{
 
@@ -13,19 +18,31 @@ public class PhoenixLoggingSubsystem extends SubsystemBase{
 
     public PhoenixLoggingSubsystem(FieldManagementSubsystem fieldManagementSubsystem){
         this.fieldManagementSubsystem = fieldManagementSubsystem;
-        SignalLogger.setPath("/media/sda1/ctre-logs/");
+        SignalLogger.setPath("/u/ctre-logs/");
+        SignalLogger.start();
     }
 
     @Override
     public void periodic(){
-        MatchStatus currentMatchStatus = fieldManagementSubsystem.getMatchStatus();
-        if(currentMatchStatus == MatchStatus.AUTON
-            || currentMatchStatus == MatchStatus.TELEOP
-            && !isLogging){
-            SignalLogger.start();
-        }
-        else if(currentMatchStatus == MatchStatus.ENDED){
-            SignalLogger.stop();
-        }
+        // MatchStatus currentMatchStatus = fieldManagementSubsystem.getMatchStatus();
+        // if(currentMatchStatus == MatchStatus.AUTON
+        //     || currentMatchStatus == MatchStatus.TELEOP
+        //     && !isLogging){
+        //     SignalLogger.start();
+        //     DataLogManager.start();
+        //     // DriverStation.startDataLog(DataLogManager.getLog());
+        //     isLogging = true;
+        // }
+        // else if(currentMatchStatus == MatchStatus.ENDED){
+        //     SignalLogger.stop();
+        //     DataLogManager.stop();
+        //     isLogging = false;
+        // }
+        // else if(fieldManagementSubsystem.getRobotStatus() == RobotStatus.DISABLED){
+        //     DataLogManager.stop();
+        //     SignalLogger.stop();
+        //     isLogging = false;
+        // }
+        // System.out.println("Log Dir: " + DataLogManager.getLogDir());
     }
 }

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

@@ -20,6 +20,9 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import frc.robot.util.GRTUtil;
 
 public class KrakenDriveMotor {
 
@@ -43,6 +46,14 @@ public class KrakenDriveMotor {
     private StatusSignal<Current> supplyCurrentSignal;
     private StatusSignal<Current> statorCurrentSignal; //torqueCurrent is Pro
 
+    private DoubleLogEntry positionLogEntry;
+    private DoubleLogEntry veloErrorLogEntry;
+    private DoubleLogEntry veloLogEntry;
+    private DoubleLogEntry targetVeloEntry;
+    private DoubleLogEntry appliedVoltsLogEntry;
+    private DoubleLogEntry supplyCurrLogEntry;
+    private DoubleLogEntry statorCurrLogEntry;
+    private DoubleLogEntry temperatureLogEntry;
 
     /** A kraken drive motor for swerve.
      *
@@ -64,6 +75,7 @@ public KrakenDriveMotor(int canId) {
 
         initNT(canId);
         initSignals();
+        initLogs(canId);
     }
 
     /**
@@ -97,6 +109,21 @@ private void initSignals(){
         motor.optimizeBusUtilization(0, 1.0);
     }
 
+    /**
+     * Initializes log entries
+     * @param canId drive motor's CAN ID
+     */
+    private void initLogs(int canId){
+        positionLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "position");
+        veloErrorLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "veloError"); 
+        veloLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "velo");
+        targetVeloEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "targetVelo");
+        appliedVoltsLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "appliedVolts");
+        supplyCurrLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "supplyCurrent");
+        statorCurrLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "statorCurrent");
+        temperatureLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature");
+    }
+
     /**
      * Set motor velo to target velo
      * @param metersPerSec target velo in m/s
@@ -162,4 +189,35 @@ public void publishStats(){
         statorCurrentPublisher.set(motor.getStatorCurrent().getValueAsDouble());
     }
 
-}
+    public void logStats(){
+        positionLogEntry.append(
+            motor.getPosition().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        veloErrorLogEntry.append(
+            motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        veloLogEntry.append(
+            motor.getVelocity().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        targetVeloEntry.append(targetRps, GRTUtil.getFPGATime());
+
+        appliedVoltsLogEntry.append(
+            motor.getMotorVoltage().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        supplyCurrLogEntry.append(
+            motor.getSupplyCurrent().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        statorCurrLogEntry.append(
+            motor.getStatorCurrent().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+
+        temperatureLogEntry.append(
+            motor.getDeviceTemp().getValueAsDouble(), GRTUtil.getFPGATime()
+        );
+    }
+}