From d3e805f8bbca279d9173b81fe3183d1acfca2f51 Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 27 Jan 2025 19:59:18 -0800 Subject: [PATCH 01/27] Untested class --- .../frc/robot/util/Motors/LoggedSparkMax.java | 141 ++++++++++++++++++ .../util/Motors/LoggedSparkMaxConfig.java | 39 +++++ 2 files changed, 180 insertions(+) create mode 100644 src/main/java/frc/robot/util/Motors/LoggedSparkMax.java create mode 100644 src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java new file mode 100644 index 00000000..5caa7e74 --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -0,0 +1,141 @@ +package frc.robot.util.Motors; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +public class LoggedSparkMax { + + private final SparkMax motor; + private final SparkMaxConfig sparkMaxConfig; + private final ClosedLoopConfig closedLoopConfig; + private final RelativeEncoder encoder; + private SparkClosedLoopController closedLoopController; + private double targetPosition; + + private NetworkTableInstance ntInstance; + private NetworkTable motorStatsTable; + private DoublePublisher neoPositionPublisher; + private DoublePublisher neoSetPositionPublisher; + + private DoubleLogEntry positionLogEntry; + private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry busVoltageLogEntry; + private DoubleLogEntry outputCurrtLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry appliedOutputLogEntry; + + public LoggedSparkMax(LoggedSparkMaxConfig config) { + motor = new SparkMax(config.getCanId(), MotorType.kBrushless); + sparkMaxConfig = config.getSparkMaxConfig(); + closedLoopConfig = config.getClosedLoopConfig(); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + encoder = motor.getEncoder(); + + initLogs(config.getCanId()); + intiNT(config.getCanId()); + } + + public void configurePIDF(double p, double i, double d, double f) { + closedLoopConfig.pidf(p, i, d, f); + sparkMaxConfig.apply(closedLoopConfig); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + } + + public void setPosition(double position){ + targetPosition = position; + closedLoopController.setReference(position, ControlType.kPosition); + } + + public double getPosition(){ + return encoder.getPosition(); + } + + /** + * Initializes NetworkTables + * @param canId motor's CAN ID + */ + private void intiNT(int canId) { + ntInstance = NetworkTableInstance.getDefault(); + motorStatsTable = ntInstance.getTable("MotorStats"); + neoPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "neoPosition" + ).publish(); + neoSetPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "neoSetPosition" + ).publish(); + } + + /** + * Initializes logs + * @param canId motor's CAN ID + */ + private void initLogs(int canId){ + positionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); + + targetPositionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetPosition"); + + busVoltageLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "busVoltage"); + + outputCurrtLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "outputCurrent"); + + temperatureLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature"); + + appliedOutputLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "appliedOutput"); + } + + /** + * Publishes Neo stats to NT + */ + public void publishStats(){ + neoPositionPublisher.set(getPosition()); + neoSetPositionPublisher.set(targetPosition); + } + + /** + * Logs Neo stats into log file + */ + public void logStats(){ + positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); + targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); + + outputCurrtLogEntry.append( + motor.getOutputCurrent(), GRTUtil.getFPGATime() + ); + + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + + appliedOutputLogEntry.append( + motor.getAppliedOutput(), GRTUtil.getFPGATime() + ); + } +} diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java new file mode 100644 index 00000000..461638ee --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -0,0 +1,39 @@ +package frc.robot.util.Motors; + +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class LoggedSparkMaxConfig { + private final int canId; + private final ClosedLoopConfig closedLoopConfig; + private final SparkMaxConfig sparkMaxConfig; + private final EncoderConfig encoderConfig; + + public LoggedSparkMaxConfig( + int canId, ClosedLoopConfig closedLoopConfig, EncoderConfig encoderConfig){ + this.canId = canId; + this.closedLoopConfig = closedLoopConfig; + this.encoderConfig = encoderConfig; + this.sparkMaxConfig = new SparkMaxConfig(); + this.sparkMaxConfig.apply(closedLoopConfig); + this.sparkMaxConfig.apply(encoderConfig); + } + + public int getCanId() { + return canId; + } + + public ClosedLoopConfig getClosedLoopConfig() { + return closedLoopConfig; + } + + public SparkMaxConfig getSparkMaxConfig() { + return sparkMaxConfig; + } + + public EncoderConfig getEncoderConfig() { + return encoderConfig; + } + +} From 479507d753be1ebea9c7615115ebc15867ee21a5 Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 27 Jan 2025 20:28:38 -0800 Subject: [PATCH 02/27] Untested Talon Class --- src/main/java/frc/robot/Constants.java | 2 + .../subsystems/swerve/KrakenDriveMotor.java | 1 + .../frc/robot/util/Motors/LoggedTalon.java | 170 ++++++++++++++++++ 3 files changed, 173 insertions(+) create mode 100644 src/main/java/frc/robot/util/Motors/LoggedTalon.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2d6a95d1..e4e0d75f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,6 +80,8 @@ public static class SwerveConstants { 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 VisionConstants{ diff --git a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java index d86a220c..b748adf0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java @@ -138,6 +138,7 @@ private void initLogs(int canId){ 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 diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java new file mode 100644 index 00000000..8be8020a --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -0,0 +1,170 @@ +package frc.robot.util.Motors; + +import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; + +public class LoggedTalon{ + + private final TalonFX motor; + + private NetworkTableInstance ntInstance; + private NetworkTable swerveStatsTable; + private DoublePublisher veloErrorPublisher; + private DoublePublisher veloPublisher; + private DoublePublisher appliedVlotsPublisher; + private DoublePublisher supplyCurrentPublisher; + private DoublePublisher statorCurrentPublisher; + + private DoubleLogEntry positionLogEntry; + private DoubleLogEntry veloErrorLogEntry; + private DoubleLogEntry veloLogEntry; + private DoubleLogEntry appliedVoltsLogEntry; + private DoubleLogEntry supplyCurrLogEntry; + private DoubleLogEntry statorCurrLogEntry; + private DoubleLogEntry temperatureLogEntry; + + public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ + motor = new TalonFX(canId, "can"); + for (int i = 0; i < 4; i++) { + boolean error = motor.getConfigurator().apply(talonConfig, 0.1) == StatusCode.OK; + if (!error) break; + } + initNT(canId); + initLogs(canId); + } + + /** + * initializes network table and entries + * @param canId motor's CAN ID + */ + private void initNT(int canId){ + ntInstance = NetworkTableInstance.getDefault(); + swerveStatsTable = ntInstance.getTable(CTRE_TABLE); + veloErrorPublisher = swerveStatsTable.getDoubleTopic( + canId + "veloError" + ).publish(); + veloPublisher = swerveStatsTable.getDoubleTopic(canId + "velo").publish(); + appliedVlotsPublisher = swerveStatsTable.getDoubleTopic( + canId + "appliedVolts" + ).publish(); + supplyCurrentPublisher = swerveStatsTable.getDoubleTopic( + canId + "supplyCurrent" + ).publish(); + statorCurrentPublisher = swerveStatsTable.getDoubleTopic( + canId + "statorCurrent" + ).publish(); + } + + /** + * 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" + ); + 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" + ); + } + + /** + * Configures drive motor's PIDSV + * @param p kP + * @param i kI + * @param d kD + * @param s kS for static friction + * @param v kV Voltage feed forward + */ + public void configPID(double p, double i, double d, double s, double v) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kP = p; + slot0Configs.kI = i; + slot0Configs.kD = d; + slot0Configs.kS = s; + slot0Configs.kV = v; + + motor.getConfigurator().apply(slot0Configs); + } + + public double getPosition(){ + return motor.getPosition().getValueAsDouble(); + } + + public double getVelocity(){ + return motor.getVelocity().getValueAsDouble(); + } + + /** + * Publishes motor stats to NT for logging + */ + public void publishStats(){ + veloErrorPublisher.set(motor.getClosedLoopError().getValueAsDouble()); + veloPublisher.set(motor.getVelocity().getValueAsDouble()); + appliedVlotsPublisher.set(motor.getMotorVoltage().getValueAsDouble()); + supplyCurrentPublisher.set(motor.getSupplyCurrent().getValueAsDouble()); + 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() + ); + + 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() + ); + } + + public void setPosition(double position){ + motor.setControl(new PositionVoltage(position)); + } +} From 367353f784be0cf347e645af65cdc7c4d9e8cf26 Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 28 Jan 2025 19:15:57 -0800 Subject: [PATCH 03/27] Added methods to accomodate usages in existing branches --- .../frc/robot/util/Motors/LoggedSparkMax.java | 59 +++++++- .../frc/robot/util/Motors/LoggedTalon.java | 130 ++++++++++++------ 2 files changed, 142 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index 5caa7e74..4119a4f6 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -12,11 +12,13 @@ import frc.robot.util.GRTUtil; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; public class LoggedSparkMax { @@ -25,7 +27,7 @@ public class LoggedSparkMax { private final ClosedLoopConfig closedLoopConfig; private final RelativeEncoder encoder; private SparkClosedLoopController closedLoopController; - private double targetPosition; + private double targetReference; private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; @@ -53,6 +55,13 @@ public LoggedSparkMax(LoggedSparkMaxConfig config) { intiNT(config.getCanId()); } + /** + * Configures the PIDF values for the motor + * @param p kP value + * @param i kI value + * @param d kD value + * @param f kF value + */ public void configurePIDF(double p, double i, double d, double f) { closedLoopConfig.pidf(p, i, d, f); sparkMaxConfig.apply(closedLoopConfig); @@ -62,11 +71,47 @@ public void configurePIDF(double p, double i, double d, double f) { closedLoopController = motor.getClosedLoopController(); } - public void setPosition(double position){ - targetPosition = position; - closedLoopController.setReference(position, ControlType.kPosition); + /** + * Sets the target reference for the motor + * @param value value of the reference + * @param controlType type of control (position, velocity, etc.) + */ + public void setReference(double value, ControlType controlType){ + targetReference = value; + closedLoopController.setReference(value, controlType); } + /** + * Sets the target reference for the motor + * @param value value of the reference + * @param controlType type of control (position, velocity, etc.) + * @param closedLoopSlot closed loop slot + * @param arbFF arbitrary feed forward + * @param arbFFUnits arbitrary feed forward units + */ + public void setReference(double value, ControlType controlType, + ClosedLoopSlot closedLoopSlot, double arbFF, + ArbFFUnits arbFFUnits + ){ + targetReference = value; + closedLoopController.setReference(value, ControlType.kPosition, + closedLoopSlot, arbFF, arbFFUnits + ); + } + + /** + * Sets the motor's speed to a specific value + * @param value value of the speed from -1.0 to 1.0 + */ + public void set(double value){ + motor.set(value); + } + + /** + * Gets the position of the motor in rotations after taking the position + * conversion factor into account + * @return position of the motor in rotations + */ public double getPosition(){ return encoder.getPosition(); } @@ -115,7 +160,7 @@ private void initLogs(int canId){ */ public void publishStats(){ neoPositionPublisher.set(getPosition()); - neoSetPositionPublisher.set(targetPosition); + neoSetPositionPublisher.set(targetReference); } /** @@ -123,7 +168,7 @@ public void publishStats(){ */ public void logStats(){ positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); - targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + targetPositionLogEntry.append(targetReference, GRTUtil.getFPGATime()); busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); outputCurrtLogEntry.append( diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 8be8020a..166f5009 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.networktables.DoublePublisher; @@ -20,20 +21,28 @@ public class LoggedTalon{ private final TalonFX motor; private NetworkTableInstance ntInstance; - private NetworkTable swerveStatsTable; - private DoublePublisher veloErrorPublisher; + private NetworkTable motorStatsTable; + + private DoublePublisher positionPublisher; private DoublePublisher veloPublisher; private DoublePublisher appliedVlotsPublisher; private DoublePublisher supplyCurrentPublisher; private DoublePublisher statorCurrentPublisher; + private DoublePublisher temperaturePublisher; + private DoublePublisher targetPositionPublisher; + private DoublePublisher targetVelocityPublisher; private DoubleLogEntry positionLogEntry; - private DoubleLogEntry veloErrorLogEntry; private DoubleLogEntry veloLogEntry; private DoubleLogEntry appliedVoltsLogEntry; private DoubleLogEntry supplyCurrLogEntry; private DoubleLogEntry statorCurrLogEntry; private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry targetVelocityLogEntry; + + private double targetPosition; + private double targetVelocity; public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ motor = new TalonFX(canId, "can"); @@ -45,26 +54,73 @@ public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ initLogs(canId); } + /** + * Using PIDF to set the motor's position + * @param position position reference + */ + public void setPosition(double position){ + targetPosition = position; + motor.setControl(new PositionVoltage(position)); + } + + /** + * Using PIDF to set the motor's velocity + * @param velocity$ velocity reference + */ + public void setVelocity(double velocity){ + targetVelocity = velocity; + motor.setControl(new VelocityVoltage(velocity)); + } + + /** + * Configures drive motor's PIDSV + * @param p kP + * @param i kI + * @param d kD + * @param s kS for static friction + * @param v kV Voltage feed forward + */ + public void configPID(double p, double i, double d, double s, double v) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kP = p; + slot0Configs.kI = i; + slot0Configs.kD = d; + slot0Configs.kS = s; + slot0Configs.kV = v; + + motor.getConfigurator().apply(slot0Configs); + } + /** * initializes network table and entries * @param canId motor's CAN ID */ private void initNT(int canId){ ntInstance = NetworkTableInstance.getDefault(); - swerveStatsTable = ntInstance.getTable(CTRE_TABLE); - veloErrorPublisher = swerveStatsTable.getDoubleTopic( - canId + "veloError" + motorStatsTable = ntInstance.getTable(CTRE_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "position" ).publish(); - veloPublisher = swerveStatsTable.getDoubleTopic(canId + "velo").publish(); - appliedVlotsPublisher = swerveStatsTable.getDoubleTopic( + veloPublisher = motorStatsTable.getDoubleTopic(canId + "velo").publish(); + appliedVlotsPublisher = motorStatsTable.getDoubleTopic( canId + "appliedVolts" ).publish(); - supplyCurrentPublisher = swerveStatsTable.getDoubleTopic( + supplyCurrentPublisher = motorStatsTable.getDoubleTopic( canId + "supplyCurrent" ).publish(); - statorCurrentPublisher = swerveStatsTable.getDoubleTopic( + statorCurrentPublisher = motorStatsTable.getDoubleTopic( canId + "statorCurrent" ).publish(); + temperaturePublisher = motorStatsTable.getDoubleTopic( + canId + "temperature" + ).publish(); + targetPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "targetPosition" + ).publish(); + targetVelocityPublisher = motorStatsTable.getDoubleTopic( + canId + "targetVelocity" + ).publish(); } /** @@ -75,9 +131,6 @@ 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" ); @@ -93,32 +146,28 @@ private void initLogs(int canId){ temperatureLogEntry = new DoubleLogEntry( DataLogManager.getLog(), canId + "temperature" ); + targetPositionLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetPosition" + ); + targetVelocityLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetVelocity" + ); } /** - * Configures drive motor's PIDSV - * @param p kP - * @param i kI - * @param d kD - * @param s kS for static friction - * @param v kV Voltage feed forward + * Gets motor's position in rotations after taking the + * SensorToMechanismRatio into account + * @return position of the motor in rotations */ - public void configPID(double p, double i, double d, double s, double v) { - Slot0Configs slot0Configs = new Slot0Configs(); - - slot0Configs.kP = p; - slot0Configs.kI = i; - slot0Configs.kD = d; - slot0Configs.kS = s; - slot0Configs.kV = v; - - motor.getConfigurator().apply(slot0Configs); - } - public double getPosition(){ return motor.getPosition().getValueAsDouble(); } + /** + * Gets motor's velocity in RPS after taking the SensorToMechanismRatio into + * account + * @return velocity of the motor in RPS + */ public double getVelocity(){ return motor.getVelocity().getValueAsDouble(); } @@ -127,22 +176,24 @@ public double getVelocity(){ * Publishes motor stats to NT for logging */ public void publishStats(){ - veloErrorPublisher.set(motor.getClosedLoopError().getValueAsDouble()); + positionPublisher.set(motor.getPosition().getValueAsDouble()); veloPublisher.set(motor.getVelocity().getValueAsDouble()); appliedVlotsPublisher.set(motor.getMotorVoltage().getValueAsDouble()); supplyCurrentPublisher.set(motor.getSupplyCurrent().getValueAsDouble()); statorCurrentPublisher.set(motor.getStatorCurrent().getValueAsDouble()); + temperaturePublisher.set(motor.getDeviceTemp().getValueAsDouble()); + targetPositionPublisher.set(targetPosition); + targetVelocityPublisher.set(targetVelocity); } + /** + * Loggs motor stats into log file + */ public void logStats(){ positionLogEntry.append( motor.getPosition().getValueAsDouble(), GRTUtil.getFPGATime() ); - veloErrorLogEntry.append( - motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime() - ); - veloLogEntry.append( motor.getVelocity().getValueAsDouble(), GRTUtil.getFPGATime() ); @@ -162,9 +213,8 @@ public void logStats(){ temperatureLogEntry.append( motor.getDeviceTemp().getValueAsDouble(), GRTUtil.getFPGATime() ); - } - - public void setPosition(double position){ - motor.setControl(new PositionVoltage(position)); + + targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + targetVelocityLogEntry.append(targetVelocity, GRTUtil.getFPGATime()); } } From aa9bf854477bf55ad9c63eed0b8fd5f289125cad Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 28 Jan 2025 20:47:56 -0800 Subject: [PATCH 04/27] Enable PIDF tuning through NT + Logging more things for spark max. --- .../subsystems/swerve/SwerveSubsystem.java | 1 + .../frc/robot/util/Motors/LoggedSparkMax.java | 117 ++++++++++++++---- 2 files changed, 91 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 81fb4434..c06aedd0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -21,6 +21,7 @@ import static frc.robot.Constants.SwerveConstants.*; import static frc.robot.Constants.LoggingConstants.*; +import static frc.robot.Constants.DebugConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index 4119a4f6..218b428c 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -6,11 +6,17 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import frc.robot.util.GRTUtil; +import static frc.robot.Constants.LoggingConstants.REV_TABLE; +import static frc.robot.Constants.DebugConstants.REV_DEBUG; + +import java.util.EnumSet; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.ClosedLoopSlot; @@ -22,6 +28,7 @@ public class LoggedSparkMax { + private final int canId; private final SparkMax motor; private final SparkMaxConfig sparkMaxConfig; private final ClosedLoopConfig closedLoopConfig; @@ -31,18 +38,25 @@ public class LoggedSparkMax { private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; - private DoublePublisher neoPositionPublisher; - private DoublePublisher neoSetPositionPublisher; + private DoublePublisher positionPublisher; + private DoublePublisher velocityPublisher; + private DoublePublisher busVoltagePublisher; + private DoublePublisher outputCurrentPublisher; + private DoublePublisher appliedOutputPublisher; + private DoublePublisher temperaturePublisher; + private DoublePublisher targetReferencePublisher; private DoubleLogEntry positionLogEntry; - private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry velocityLogEntry; private DoubleLogEntry busVoltageLogEntry; private DoubleLogEntry outputCurrtLogEntry; - private DoubleLogEntry temperatureLogEntry; private DoubleLogEntry appliedOutputLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry targetReferenceLogEntry; public LoggedSparkMax(LoggedSparkMaxConfig config) { - motor = new SparkMax(config.getCanId(), MotorType.kBrushless); + canId = config.getCanId(); + motor = new SparkMax(canId, MotorType.kBrushless); sparkMaxConfig = config.getSparkMaxConfig(); closedLoopConfig = config.getClosedLoopConfig(); motor.configure(sparkMaxConfig, @@ -51,8 +65,11 @@ public LoggedSparkMax(LoggedSparkMaxConfig config) { closedLoopController = motor.getClosedLoopController(); encoder = motor.getEncoder(); - initLogs(config.getCanId()); - intiNT(config.getCanId()); + initLogs(); + intiNT(); + if(REV_DEBUG){ + enableDebug(); + } } /** @@ -116,18 +133,41 @@ public double getPosition(){ return encoder.getPosition(); } + /** + * Gets the velocity of the motor in RPM after taking the velocity + * conversion factor into account + */ + public double getVelocity(){ + return encoder.getVelocity(); + } + /** * Initializes NetworkTables * @param canId motor's CAN ID */ - private void intiNT(int canId) { + private void intiNT() { ntInstance = NetworkTableInstance.getDefault(); - motorStatsTable = ntInstance.getTable("MotorStats"); - neoPositionPublisher = motorStatsTable.getDoubleTopic( - canId + "neoPosition" + motorStatsTable = ntInstance.getTable(REV_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "Position" ).publish(); - neoSetPositionPublisher = motorStatsTable.getDoubleTopic( - canId + "neoSetPosition" + velocityPublisher = motorStatsTable.getDoubleTopic( + canId + "Velocity" + ).publish(); + busVoltagePublisher = motorStatsTable.getDoubleTopic( + canId + "BusVoltage" + ).publish(); + outputCurrentPublisher = motorStatsTable.getDoubleTopic( + canId + "OutputCurrent" + ).publish(); + appliedOutputPublisher = motorStatsTable.getDoubleTopic( + canId + "AppliedOutput" + ).publish(); + temperaturePublisher = motorStatsTable.getDoubleTopic( + canId + "Temperature" + ).publish(); + targetReferencePublisher = motorStatsTable.getDoubleTopic( + canId + "TargetReference" ).publish(); } @@ -135,12 +175,12 @@ private void intiNT(int canId) { * Initializes logs * @param canId motor's CAN ID */ - private void initLogs(int canId){ + private void initLogs(){ positionLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); - targetPositionLogEntry = - new DoubleLogEntry(DataLogManager.getLog(), canId + "targetPosition"); + targetReferenceLogEntry= + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetReference"); busVoltageLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "busVoltage"); @@ -156,31 +196,54 @@ private void initLogs(int canId){ } /** - * Publishes Neo stats to NT + * Allows for PIDF tuning in NT + */ + private void enableDebug(){ + motorStatsTable.getDoubleArrayTopic(canId + "PIDF").publish().set( + new double[] {0., 0., 0., 0.} + ); + motorStatsTable.addListener(canId + "PIDF", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + configurePIDF( + event.valueData.value.getDoubleArray()[0], + event.valueData.value.getDoubleArray()[1], + event.valueData.value.getDoubleArray()[2], + event.valueData.value.getDoubleArray()[3] + ); + } + ); + } + + /** + * Publishes stats to NT */ public void publishStats(){ - neoPositionPublisher.set(getPosition()); - neoSetPositionPublisher.set(targetReference); + positionPublisher.set(getPosition()); + velocityPublisher.set(getVelocity()); + busVoltagePublisher.set(motor.getBusVoltage()); + outputCurrentPublisher.set(motor.getOutputCurrent()); + appliedOutputPublisher.set(motor.getAppliedOutput()); + temperaturePublisher.set(motor.getMotorTemperature()); + targetReferencePublisher.set(targetReference); } /** - * Logs Neo stats into log file + * Logs stats into log file */ public void logStats(){ positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); - targetPositionLogEntry.append(targetReference, GRTUtil.getFPGATime()); + velocityLogEntry.append(getVelocity(), GRTUtil.getFPGATime()); busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); - outputCurrtLogEntry.append( motor.getOutputCurrent(), GRTUtil.getFPGATime() ); - - temperatureLogEntry.append( - motor.getMotorTemperature(), GRTUtil.getFPGATime() - ); - appliedOutputLogEntry.append( motor.getAppliedOutput(), GRTUtil.getFPGATime() ); + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + targetReferenceLogEntry.append(targetReference, GRTUtil.getFPGATime()); } } From 010458f2690ebeaf318b9a3e6f37b0e1194b8bd8 Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 10:22:16 -0800 Subject: [PATCH 05/27] Allows changing talon pid through NT --- .../frc/robot/util/Motors/LoggedTalon.java | 45 ++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 166f5009..a816b15a 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -1,6 +1,9 @@ package frc.robot.util.Motors; import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; +import static frc.robot.Constants.DebugConstants.CTRE_DEBUG; + +import java.util.EnumSet; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.Slot0Configs; @@ -11,6 +14,7 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; @@ -20,6 +24,9 @@ public class LoggedTalon{ private final TalonFX motor; + private final int canId; + private double[] pidsvg = new double[6]; + private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; @@ -50,8 +57,20 @@ public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ boolean error = motor.getConfigurator().apply(talonConfig, 0.1) == StatusCode.OK; if (!error) break; } + this.canId = canId; + pidsvg = new double[] { + talonConfig.Slot0.kP, + talonConfig.Slot0.kI, + talonConfig.Slot0.kD, + talonConfig.Slot0.kS, + talonConfig.Slot0.kV, + talonConfig.Slot0.kG + }; initNT(canId); initLogs(canId); + if(CTRE_DEBUG){ + enableDebug(); + } } /** @@ -80,7 +99,9 @@ public void setVelocity(double velocity){ * @param s kS for static friction * @param v kV Voltage feed forward */ - public void configPID(double p, double i, double d, double s, double v) { + public void configurePIDSVG(double p, double i, double d, double s, double v, + double g + ) { Slot0Configs slot0Configs = new Slot0Configs(); slot0Configs.kP = p; @@ -88,6 +109,7 @@ public void configPID(double p, double i, double d, double s, double v) { slot0Configs.kD = d; slot0Configs.kS = s; slot0Configs.kV = v; + slot0Configs.kG = g; motor.getConfigurator().apply(slot0Configs); } @@ -154,6 +176,27 @@ private void initLogs(int canId){ ); } + /** + * Allows changing PID through NT + */ + private void enableDebug(){ + motorStatsTable.getDoubleArrayTopic(canId + "PIDSVG").publish().set( + pidsvg + ); + motorStatsTable.addListener(canId + "PIDSVG", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + configurePIDSVG( + event.valueData.value.getDoubleArray()[0], + event.valueData.value.getDoubleArray()[1], + event.valueData.value.getDoubleArray()[2], + event.valueData.value.getDoubleArray()[3], + event.valueData.value.getDoubleArray()[4], + event.valueData.value.getDoubleArray()[5] + ); + } + ); + } /** * Gets motor's position in rotations after taking the * SensorToMechanismRatio into account From 92c20a34199f68c0f065be64be8b9b1559d399ae Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 15:59:11 -0800 Subject: [PATCH 06/27] fixed typo --- src/main/java/frc/robot/util/Motors/LoggedTalon.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index a816b15a..b81d3530 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -84,7 +84,7 @@ public void setPosition(double position){ /** * Using PIDF to set the motor's velocity - * @param velocity$ velocity reference + * @param velocity velocity reference */ public void setVelocity(double velocity){ targetVelocity = velocity; From ac86d8b4600784507dc982ab6d0a5d8147fc57a9 Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 17:21:19 -0800 Subject: [PATCH 07/27] Added comments & allowed one spark max to follow another --- .../util/Motors/LoggedSparkMaxConfig.java | 34 ++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java index 461638ee..7d0cd21e 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -20,20 +20,52 @@ public LoggedSparkMaxConfig( this.sparkMaxConfig.apply(encoderConfig); } + /** + * Follow another motor + * @param canId the CAN ID of the motor to follow + */ + public void follow(int canId){ + this.sparkMaxConfig.follow(canId); + } + + /** + * Follow another motor + * @param canId the CAN ID of the motor to follow + * @param invert whether to invert the motor + */ + public void follow(int canId, boolean invert){ + this.sparkMaxConfig.follow(canId, invert); + } + + /** + * Get the CAN ID of the motor + * @return the CAN ID of the motor + */ public int getCanId() { return canId; } + /** + * Get the closed loop configuration + * @return the closed loop configuration + */ public ClosedLoopConfig getClosedLoopConfig() { return closedLoopConfig; } + /** + * Get the Spark Max configuration + * @return the Spark Max configuration + */ public SparkMaxConfig getSparkMaxConfig() { return sparkMaxConfig; } + /** + * Get the encoder configuration + * @return the encoder configuration + */ public EncoderConfig getEncoderConfig() { return encoderConfig; } - } From 8190463fda8e3cbeb8f5a82ad421d28e28e89bfc Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 17:28:17 -0800 Subject: [PATCH 08/27] Using optional int to represent the can ID to follow --- .../util/Motors/LoggedSparkMaxConfig.java | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java index 7d0cd21e..fab826b2 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -1,5 +1,7 @@ package frc.robot.util.Motors; +import java.util.OptionalInt; + import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -11,30 +13,19 @@ public class LoggedSparkMaxConfig { private final EncoderConfig encoderConfig; public LoggedSparkMaxConfig( - int canId, ClosedLoopConfig closedLoopConfig, EncoderConfig encoderConfig){ + int canId, ClosedLoopConfig closedLoopConfig, + EncoderConfig encoderConfig, OptionalInt followCanId + ){ this.canId = canId; this.closedLoopConfig = closedLoopConfig; this.encoderConfig = encoderConfig; this.sparkMaxConfig = new SparkMaxConfig(); this.sparkMaxConfig.apply(closedLoopConfig); this.sparkMaxConfig.apply(encoderConfig); - } - - /** - * Follow another motor - * @param canId the CAN ID of the motor to follow - */ - public void follow(int canId){ - this.sparkMaxConfig.follow(canId); - } - /** - * Follow another motor - * @param canId the CAN ID of the motor to follow - * @param invert whether to invert the motor - */ - public void follow(int canId, boolean invert){ - this.sparkMaxConfig.follow(canId, invert); + if(followCanId.isPresent()){ + this.sparkMaxConfig.follow(followCanId.getAsInt()); + } } /** From 1b00441798ff24020b2c8e163e51edd17e58da38 Mon Sep 17 00:00:00 2001 From: Jesse Date: Thu, 30 Jan 2025 16:03:27 -0800 Subject: [PATCH 09/27] Allows to set the motor's position & differentiated setPosition and setPositionReference --- .../frc/robot/util/Motors/LoggedTalon.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index b81d3530..1fa6b825 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -74,23 +74,35 @@ public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ } /** - * Using PIDF to set the motor's position + * Sets the motor's position reference * @param position position reference */ - public void setPosition(double position){ + public void setPositionReference(double position){ targetPosition = position; motor.setControl(new PositionVoltage(position)); } /** - * Using PIDF to set the motor's velocity + * Set the motor's velocity reference * @param velocity velocity reference */ - public void setVelocity(double velocity){ + public void setVelocityREference(double velocity){ targetVelocity = velocity; motor.setControl(new VelocityVoltage(velocity)); } + /** + * Sets the motor's position to a certain value + * @param position mechanisam position after taking the position conversion + * into account + */ + public void setPosition(double position){ + for (int i = 0; i < 4; i++) { + boolean error = motor.setPosition(position) == StatusCode.OK; + if (!error) break; + } + } + /** * Configures drive motor's PIDSV * @param p kP From dff12b37473021204a6199a1e421656b9ad17e1f Mon Sep 17 00:00:00 2001 From: Jesse Date: Thu, 30 Jan 2025 16:22:50 -0800 Subject: [PATCH 10/27] allows to set a position reference with feedforward --- src/main/java/frc/robot/util/Motors/LoggedTalon.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 1fa6b825..4472f829 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -82,11 +82,20 @@ public void setPositionReference(double position){ motor.setControl(new PositionVoltage(position)); } + /** + * Sets the motor's position reference with feedforward + * @param position position reference + * @param arbFF arbitrary feedforward + */ + public void setPositionReferenceWithArbFF(double position, double arbFF){ + targetPosition = position; + motor.setControl(new PositionVoltage(position).withFeedForward(arbFF)); + } /** * Set the motor's velocity reference * @param velocity velocity reference */ - public void setVelocityREference(double velocity){ + public void setVelocityReference(double velocity){ targetVelocity = velocity; motor.setControl(new VelocityVoltage(velocity)); } From 2e3aaf5bfb81f45c60e3f3bfb1871c5ff00800e4 Mon Sep 17 00:00:00 2001 From: Jesse Date: Thu, 30 Jan 2025 20:07:29 -0800 Subject: [PATCH 11/27] Allow softlimit config --- .../java/frc/robot/util/Motors/LoggedSparkMaxConfig.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java index fab826b2..efdcd32f 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -1,9 +1,11 @@ package frc.robot.util.Motors; +import java.util.Optional; import java.util.OptionalInt; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; public class LoggedSparkMaxConfig { @@ -14,7 +16,8 @@ public class LoggedSparkMaxConfig { public LoggedSparkMaxConfig( int canId, ClosedLoopConfig closedLoopConfig, - EncoderConfig encoderConfig, OptionalInt followCanId + EncoderConfig encoderConfig, OptionalInt followCanId, + Optional softLimitConfig ){ this.canId = canId; this.closedLoopConfig = closedLoopConfig; @@ -26,6 +29,9 @@ public LoggedSparkMaxConfig( if(followCanId.isPresent()){ this.sparkMaxConfig.follow(followCanId.getAsInt()); } + if(softLimitConfig.isPresent()){ + this.sparkMaxConfig.apply(softLimitConfig.get()); + } } /** From 923216569b44b93dc53339c0c7f1b64f69aa40e1 Mon Sep 17 00:00:00 2001 From: Jesse Date: Thu, 30 Jan 2025 20:20:17 -0800 Subject: [PATCH 12/27] spark max now tracks speeds set through set() --- src/main/java/frc/robot/util/Motors/LoggedSparkMax.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index 218b428c..ded98285 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -122,6 +122,7 @@ public void setReference(double value, ControlType controlType, */ public void set(double value){ motor.set(value); + targetReference = value; } /** From 69c869f0571f2b358a7af6304578b261e05254eb Mon Sep 17 00:00:00 2001 From: eerinn Date: Wed, 15 Jan 2025 16:31:44 -0800 Subject: [PATCH 13/27] setangle --- .../frc/robot/subsystems/PivotSubsytem.java | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/PivotSubsytem.java diff --git a/src/main/java/frc/robot/subsystems/PivotSubsytem.java b/src/main/java/frc/robot/subsystems/PivotSubsytem.java new file mode 100644 index 00000000..239ba5d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PivotSubsytem.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; + +import static frc.robot.Constants.PivotConstants.pivotMotorID; + + +public class PivotSubsytem extends SubsystemBase{ + + private SparkMax pivotMotor; + private RelativeEncoder pivotEncoder; + private SparkClosedLoopController pivotPID; + + private double PivotP = 1; + private double PivotI = 0; + private double PivotD = 0; + + private double angle; + + public void PivotSubsystem(){ + + pivotMotor = new SparkMax (pivotMotorID, MotorType.kBrushless); + pivotEncoder = pivotMotor.getEncoder(); + pivotPID = pivotMotor.getClosedLoopController(); + + } + + public void setAngle(double angle){ + pivotPID.setReference(angle, ControlType.kPosition); + } + + + } + From ae49536e18f9d3b076dbbdc83002c7285e8c12b2 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Thu, 16 Jan 2025 17:49:47 -0800 Subject: [PATCH 14/27] setup basic pivot with pid no soft limits --- src/main/java/frc/robot/Constants.java | 5 +++ .../frc/robot/subsystems/PivotSubsytem.java | 41 +++++++++++++++---- 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4e0d75f..63d66d80 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,4 +152,9 @@ public static class VisionConstants{ public static final PolynomialRegression oStdDevModel = new PolynomialRegression( VisionConstants.STD_DEV_DIST,VisionConstants.O_STD_DEV,1); } + + public static class IntakeConstants { + public static final int PIVOT_ID = 0; + public static final double PIVOT_CONVERSION_FACTOR = 1 / 30.; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PivotSubsytem.java b/src/main/java/frc/robot/subsystems/PivotSubsytem.java index 239ba5d6..d0bc96f8 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsytem.java +++ b/src/main/java/frc/robot/subsystems/PivotSubsytem.java @@ -4,11 +4,17 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; -import static frc.robot.Constants.PivotConstants.pivotMotorID; +import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR; +import static frc.robot.Constants.IntakeConstants.PIVOT_ID; public class PivotSubsytem extends SubsystemBase{ @@ -17,24 +23,43 @@ public class PivotSubsytem extends SubsystemBase{ private RelativeEncoder pivotEncoder; private SparkClosedLoopController pivotPID; - private double PivotP = 1; - private double PivotI = 0; - private double PivotD = 0; + private EncoderConfig encoderConfig; + private ClosedLoopConfig closedLoopConfig; + private SparkMaxConfig sparkMaxConfig; - private double angle; + private double p = 1; + private double i = 0; + private double d = 0; public void PivotSubsystem(){ - - pivotMotor = new SparkMax (pivotMotorID, MotorType.kBrushless); + + pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); pivotEncoder = pivotMotor.getEncoder(); + + encoderConfig = new EncoderConfig(); + encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR); + + closedLoopConfig = new ClosedLoopConfig(); + closedLoopConfig.pid(p, i, d); + + sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig.apply(closedLoopConfig) + .apply(encoderConfig); + + pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + pivotPID = pivotMotor.getClosedLoopController(); - + } public void setAngle(double angle){ pivotPID.setReference(angle, ControlType.kPosition); } + public double getPosition() { + return pivotEncoder.getPosition(); + } + } From b4029d4b2a1300ec8993d22bd34323f37953347f Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Thu, 16 Jan 2025 20:24:47 -0800 Subject: [PATCH 15/27] pivot ready for r1 --- src/main/java/frc/robot/Constants.java | 4 ++++ .../robot/subsystems/pivot/PivotState.java | 20 +++++++++++++++++ .../subsystems/{ => pivot}/PivotSubsytem.java | 22 +++++++++++++------ 3 files changed, 39 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotState.java rename src/main/java/frc/robot/subsystems/{ => pivot}/PivotSubsytem.java (73%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 63d66d80..6fcda61b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,5 +156,9 @@ public static class VisionConstants{ public static class IntakeConstants { public static final int PIVOT_ID = 0; public static final double PIVOT_CONVERSION_FACTOR = 1 / 30.; + public static final double ZERO_POSITION = 0; + public static final double SOURCE_POSITION = 0; //TODO: change + public static final double OUTTAKE_POSITION = 0; //TODO: change + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/pivot/PivotState.java new file mode 100644 index 00000000..80dd8a60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotState.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.pivot; + +import frc.robot.Constants; + +public enum PivotState { + ZERO(Constants.IntakeConstants.ZERO_POSITION), + OUTTAKE(Constants.IntakeConstants.OUTTAKE_POSITION), + SOURCE(Constants.IntakeConstants.SOURCE_POSITION); + + private final double targetAngle; + + private PivotState(double targetAngle) { + this.targetAngle = targetAngle; + } + + public double getTargetAngle() { + return targetAngle; + } + +} diff --git a/src/main/java/frc/robot/subsystems/PivotSubsytem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java similarity index 73% rename from src/main/java/frc/robot/subsystems/PivotSubsytem.java rename to src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java index d0bc96f8..9f78eab8 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsytem.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java @@ -1,11 +1,13 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.pivot; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; @@ -26,6 +28,7 @@ public class PivotSubsytem extends SubsystemBase{ private EncoderConfig encoderConfig; private ClosedLoopConfig closedLoopConfig; private SparkMaxConfig sparkMaxConfig; + private SoftLimitConfig softLimitConfig; private double p = 1; private double i = 0; @@ -39,12 +42,19 @@ public void PivotSubsystem(){ encoderConfig = new EncoderConfig(); encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR); + softLimitConfig = new SoftLimitConfig(); + softLimitConfig.forwardSoftLimitEnabled(true) + .forwardSoftLimit(Units.degreesToRadians(90)) + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(0); + closedLoopConfig = new ClosedLoopConfig(); closedLoopConfig.pid(p, i, d); sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(closedLoopConfig) - .apply(encoderConfig); + .apply(encoderConfig) + .apply(softLimitConfig); pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -52,14 +62,12 @@ public void PivotSubsystem(){ } - public void setAngle(double angle){ - pivotPID.setReference(angle, ControlType.kPosition); - } - public double getPosition() { return pivotEncoder.getPosition(); } - + public void setState(PivotState state) { + pivotPID.setReference(state.getTargetAngle(), ControlType.kPosition); + } } From f1cec0a5f750fe361cf3843d7d7980ee34b3df56 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 17 Jan 2025 11:56:42 -0800 Subject: [PATCH 16/27] update conversion factor --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6fcda61b..b280debc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,7 +155,7 @@ public static class VisionConstants{ public static class IntakeConstants { public static final int PIVOT_ID = 0; - public static final double PIVOT_CONVERSION_FACTOR = 1 / 30.; + public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change public static final double OUTTAKE_POSITION = 0; //TODO: change From c721ce07a1f95992523a6656f5520116cc203586 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 17 Jan 2025 12:19:39 -0800 Subject: [PATCH 17/27] update syntax in pivot, begin comands --- .../pivot/SetIntakePositionCommand.java | 24 +++++++++++++++++++ ...PivotSubsytem.java => PivotSubsystem.java} | 4 ++-- 2 files changed, 26 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java rename src/main/java/frc/robot/subsystems/pivot/{PivotSubsytem.java => PivotSubsystem.java} (96%) diff --git a/src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java b/src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java new file mode 100644 index 00000000..89353554 --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.subsystems.pivot.PivotSubsystem; + +public class SetIntakePositionCommand extends Command{ + private PivotSubsystem pivotSubsystem; + + public SetIntakePositionCommand(PivotSubsystem pivotSubsystem) { + this.addRequirements(pivotSubsystem); + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void execute() { + this.pivotSubsystem.setState(PivotState.SOURCE); + } + + // @Override + // public boolean isFinished() { + + // } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java similarity index 96% rename from src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java rename to src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java index 9f78eab8..d4343990 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotSubsytem.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -19,7 +19,7 @@ import static frc.robot.Constants.IntakeConstants.PIVOT_ID; -public class PivotSubsytem extends SubsystemBase{ +public class PivotSubsystem extends SubsystemBase{ private SparkMax pivotMotor; private RelativeEncoder pivotEncoder; @@ -34,7 +34,7 @@ public class PivotSubsytem extends SubsystemBase{ private double i = 0; private double d = 0; - public void PivotSubsystem(){ + public PivotSubsystem(){ pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); pivotEncoder = pivotMotor.getEncoder(); From 09081462cc76bbf3fa390e59373ee5c2ad5ea781 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 17 Jan 2025 18:00:35 -0800 Subject: [PATCH 18/27] pivot commands --- src/main/java/frc/robot/Constants.java | 2 ++ .../pivot/SetPivotOuttakeCommand.java | 24 +++++++++++++++++++ ...ommand.java => SetPivotSourceCommand.java} | 12 +++++----- .../pivot/SetPivotVerticalCommand.java | 5 ++++ .../commands/pivot/SetPivotZeroCommand.java | 24 +++++++++++++++++++ .../robot/subsystems/pivot/PivotState.java | 3 ++- .../subsystems/pivot/PivotSubsystem.java | 20 +++++++++++++--- 7 files changed, 80 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java rename src/main/java/frc/robot/commands/pivot/{SetIntakePositionCommand.java => SetPivotSourceCommand.java} (65%) create mode 100644 src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java create mode 100644 src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b280debc..0574e115 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -159,6 +159,8 @@ public static class IntakeConstants { public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change public static final double OUTTAKE_POSITION = 0; //TODO: change + public static final double VERTICAL_POSITION = 0; //TODO: change + public static final double PIVOT_TOLERANCE = 0; //TODO: change } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java new file mode 100644 index 00000000..cf8b904a --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.subsystems.pivot.PivotSubsystem; + +public class SetPivotOuttakeCommand extends Command{ + private PivotSubsystem pivotSubsystem; + + public SetPivotOuttakeCommand(PivotSubsystem pivotSubsystem) { + this.addRequirements(pivotSubsystem); + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void execute() { + this.pivotSubsystem.setState(PivotState.OUTTAKE); + } + + @Override + public boolean isFinished() { + return pivotSubsystem.atState(PivotState.OUTTAKE); + } +} diff --git a/src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java similarity index 65% rename from src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java rename to src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java index 89353554..70ce8845 100644 --- a/src/main/java/frc/robot/commands/pivot/SetIntakePositionCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -4,10 +4,10 @@ import frc.robot.subsystems.pivot.PivotState; import frc.robot.subsystems.pivot.PivotSubsystem; -public class SetIntakePositionCommand extends Command{ +public class SetPivotSourceCommand extends Command{ private PivotSubsystem pivotSubsystem; - public SetIntakePositionCommand(PivotSubsystem pivotSubsystem) { + public SetPivotSourceCommand(PivotSubsystem pivotSubsystem) { this.addRequirements(pivotSubsystem); this.pivotSubsystem = pivotSubsystem; } @@ -17,8 +17,8 @@ public void execute() { this.pivotSubsystem.setState(PivotState.SOURCE); } - // @Override - // public boolean isFinished() { - - // } + @Override + public boolean isFinished() { + return pivotSubsystem.atState(PivotState.SOURCE); + } } diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java new file mode 100644 index 00000000..6401bba6 --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -0,0 +1,5 @@ +package frc.robot.commands.pivot; + +public class SetPivotVerticalCommand { + +} diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java new file mode 100644 index 00000000..9f160232 --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.subsystems.pivot.PivotSubsystem; + +public class SetPivotZeroCommand extends Command{ + private PivotSubsystem pivotSubsystem; + + public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) { + this.addRequirements(pivotSubsystem); + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void execute() { + this.pivotSubsystem.setState(PivotState.ZERO); + } + + @Override + public boolean isFinished() { + return pivotSubsystem.atState(PivotState.ZERO); + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/pivot/PivotState.java index 80dd8a60..ef65df23 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotState.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotState.java @@ -5,7 +5,8 @@ public enum PivotState { ZERO(Constants.IntakeConstants.ZERO_POSITION), OUTTAKE(Constants.IntakeConstants.OUTTAKE_POSITION), - SOURCE(Constants.IntakeConstants.SOURCE_POSITION); + SOURCE(Constants.IntakeConstants.SOURCE_POSITION), + VERTICAL(Constants.IntakeConstants.VERTICAL_POSITION); private final double targetAngle; diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java index d4343990..f132d7f8 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -17,6 +17,7 @@ import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR; import static frc.robot.Constants.IntakeConstants.PIVOT_ID; +import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE; public class PivotSubsystem extends SubsystemBase{ @@ -34,6 +35,8 @@ public class PivotSubsystem extends SubsystemBase{ private double i = 0; private double d = 0; + private PivotState targetState; + public PivotSubsystem(){ pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); @@ -60,14 +63,25 @@ public PivotSubsystem(){ pivotPID = pivotMotor.getClosedLoopController(); + targetState = PivotState.ZERO; + } - public double getPosition() { + public double getCurrentAngle() { return pivotEncoder.getPosition(); } - public void setState(PivotState state) { - pivotPID.setReference(state.getTargetAngle(), ControlType.kPosition); + public PivotState getTargetState() { + return targetState; + } + + public void setState(PivotState targetState) { + pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition); + this.targetState = targetState; + } + + public boolean atState(PivotState state) { + return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; } } From 58086993a77fb9ce827e277f3533c34f9ce4ef71 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 17 Jan 2025 20:11:28 -0800 Subject: [PATCH 19/27] init rollers --- .../subsystems/intake/pivot/PivotState.java | 21 +++++ .../intake/pivot/PivotSubsystem.java | 87 +++++++++++++++++++ .../intake/rollers/RollerSubsystem.java | 7 ++ 3 files changed, 115 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java new file mode 100644 index 00000000..7c72b25c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.intake.pivot; + +import frc.robot.Constants; + +public enum PivotState { + ZERO(Constants.IntakeConstants.ZERO_POSITION), + OUTTAKE(Constants.IntakeConstants.OUTTAKE_POSITION), + SOURCE(Constants.IntakeConstants.SOURCE_POSITION), + VERTICAL(Constants.IntakeConstants.VERTICAL_POSITION); + + private final double targetAngle; + + private PivotState(double targetAngle) { + this.targetAngle = targetAngle; + } + + public double getTargetAngle() { + return targetAngle; + } + +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java new file mode 100644 index 00000000..fc30dea3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.intake.pivot; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; + +import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR; +import static frc.robot.Constants.IntakeConstants.PIVOT_ID; +import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE; + + +public class PivotSubsystem extends SubsystemBase{ + + private SparkMax pivotMotor; + private RelativeEncoder pivotEncoder; + private SparkClosedLoopController pivotPID; + + private EncoderConfig encoderConfig; + private ClosedLoopConfig closedLoopConfig; + private SparkMaxConfig sparkMaxConfig; + private SoftLimitConfig softLimitConfig; + + private double p = 1; + private double i = 0; + private double d = 0; + + private PivotState targetState; + + public PivotSubsystem(){ + + pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); + pivotEncoder = pivotMotor.getEncoder(); + + encoderConfig = new EncoderConfig(); + encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR); + + softLimitConfig = new SoftLimitConfig(); + softLimitConfig.forwardSoftLimitEnabled(true) + .forwardSoftLimit(Units.degreesToRadians(90)) + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(0); + + closedLoopConfig = new ClosedLoopConfig(); + closedLoopConfig.pid(p, i, d); + + sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig.apply(closedLoopConfig) + .apply(encoderConfig) + .apply(softLimitConfig); + + pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotPID = pivotMotor.getClosedLoopController(); + + targetState = PivotState.ZERO; + + } + + public double getCurrentAngle() { + return pivotEncoder.getPosition(); + } + + public PivotState getTargetState() { + return targetState; + } + + public void setState(PivotState targetState) { + pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition); + this.targetState = targetState; + } + + public boolean atState(PivotState state) { + return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; + } + } + diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java new file mode 100644 index 00000000..0df2b6a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.intake.rollers; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class RollerSubsystem extends SubsystemBase{ + // private final +} From b310c21dae153460c4c8ed93c2f97709aaa05c19 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Sat, 18 Jan 2025 21:58:15 -0800 Subject: [PATCH 20/27] commands and basic test bindings in robocoantiner --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 28 ++++++ .../pivot/SetPivotOuttakeCommand.java | 4 +- .../commands/pivot/SetPivotSourceCommand.java | 4 +- .../pivot/SetPivotVerticalCommand.java | 23 ++++- .../commands/pivot/SetPivotZeroCommand.java | 4 +- .../intake/rollers/RollerSubsystem.java | 24 ++++- .../robot/subsystems/pivot/PivotState.java | 21 ----- .../subsystems/pivot/PivotSubsystem.java | 87 ------------------- 9 files changed, 80 insertions(+), 117 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotState.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0574e115..e5c1f1e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,6 +155,8 @@ public static class VisionConstants{ public static class IntakeConstants { public static final int PIVOT_ID = 0; + public static final int ROLLER_ID = 0; + public static final int INTAKE_SENSOR_ID = 0; public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c6d5ffe..12eb720a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import frc.robot.commands.pivot.SetPivotVerticalCommand; +import frc.robot.commands.pivot.SetPivotZeroCommand; import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.PS5DriveController; @@ -13,11 +15,18 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PS5Controller; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +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.Vision.VisionSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.rollers.RollerSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.Constants.VisionConstants; /** @@ -31,6 +40,8 @@ public class RobotContainer { private BaseDriveController driveController; private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); + private final PivotSubsystem pivotSubsystem = new PivotSubsystem(); + private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); private final VisionSubsystem visionSubsystem2 = new VisionSubsystem( VisionConstants.cameraConfigs[1] ); @@ -41,8 +52,13 @@ public class RobotContainer { VisionConstants.cameraConfigs[3] ); + private CommandPS5Controller mechController; + private Trigger aButton = new Trigger(mechController.button(0)); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + mechController = new CommandPS5Controller(1); + constructDriveController(); // startLog(); setVisionDataInterface(); @@ -75,6 +91,18 @@ private void configureBindings() { ) ); + aButton.onTrue( + new ConditionalCommand( + new SetPivotZeroCommand(pivotSubsystem), + new SetPivotVerticalCommand(pivotSubsystem), + () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL) + ) + ); + + rollerSubsystem.setDefaultCommand(new InstantCommand( () -> { + rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis()); + }, rollerSubsystem)); + /* Pressing the button resets the field axes to the current robot axes. */ driveController.bindDriverHeadingReset( () ->{ diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java index cf8b904a..b8b4ce30 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivot.PivotState; -import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotOuttakeCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java index 70ce8845..7e72148a 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivot.PivotState; -import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotSourceCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index 6401bba6..931d47f4 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -1,5 +1,24 @@ package frc.robot.commands.pivot; -public class SetPivotVerticalCommand { - +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; + +public class SetPivotVerticalCommand extends Command{ + private PivotSubsystem pivotSubsystem; + + public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) { + this.addRequirements(pivotSubsystem); + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void execute() { + this.pivotSubsystem.setState(PivotState.VERTICAL); + } + + @Override + public boolean isFinished() { + return pivotSubsystem.atState(PivotState.VERTICAL); + } } diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 9f160232..5157e2bb 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pivot.PivotState; -import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotZeroCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java index 0df2b6a3..a97725bc 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -1,7 +1,29 @@ package frc.robot.subsystems.intake.rollers; +import static frc.robot.Constants.IntakeConstants.INTAKE_SENSOR_ID; +import static frc.robot.Constants.IntakeConstants.ROLLER_ID; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class RollerSubsystem extends SubsystemBase{ - // private final + private final SparkMax rollerMotor; + private final DigitalInput intakeSensor; + + public RollerSubsystem() { + rollerMotor = new SparkMax(ROLLER_ID, MotorType.kBrushless); + intakeSensor = new DigitalInput(INTAKE_SENSOR_ID); + } + + public void setRollerPower(double speed) { + rollerMotor.set(speed); + } + + public boolean getIntakeSensor() { + return intakeSensor.get(); + } + } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/pivot/PivotState.java deleted file mode 100644 index ef65df23..00000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotState.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.subsystems.pivot; - -import frc.robot.Constants; - -public enum PivotState { - ZERO(Constants.IntakeConstants.ZERO_POSITION), - OUTTAKE(Constants.IntakeConstants.OUTTAKE_POSITION), - SOURCE(Constants.IntakeConstants.SOURCE_POSITION), - VERTICAL(Constants.IntakeConstants.VERTICAL_POSITION); - - private final double targetAngle; - - private PivotState(double targetAngle) { - this.targetAngle = targetAngle; - } - - public double getTargetAngle() { - return targetAngle; - } - -} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java deleted file mode 100644 index f132d7f8..00000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java +++ /dev/null @@ -1,87 +0,0 @@ -package frc.robot.subsystems.pivot; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.SoftLimitConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; - -import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR; -import static frc.robot.Constants.IntakeConstants.PIVOT_ID; -import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE; - - -public class PivotSubsystem extends SubsystemBase{ - - private SparkMax pivotMotor; - private RelativeEncoder pivotEncoder; - private SparkClosedLoopController pivotPID; - - private EncoderConfig encoderConfig; - private ClosedLoopConfig closedLoopConfig; - private SparkMaxConfig sparkMaxConfig; - private SoftLimitConfig softLimitConfig; - - private double p = 1; - private double i = 0; - private double d = 0; - - private PivotState targetState; - - public PivotSubsystem(){ - - pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); - - encoderConfig = new EncoderConfig(); - encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR); - - softLimitConfig = new SoftLimitConfig(); - softLimitConfig.forwardSoftLimitEnabled(true) - .forwardSoftLimit(Units.degreesToRadians(90)) - .reverseSoftLimitEnabled(true) - .reverseSoftLimit(0); - - closedLoopConfig = new ClosedLoopConfig(); - closedLoopConfig.pid(p, i, d); - - sparkMaxConfig = new SparkMaxConfig(); - sparkMaxConfig.apply(closedLoopConfig) - .apply(encoderConfig) - .apply(softLimitConfig); - - pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - pivotPID = pivotMotor.getClosedLoopController(); - - targetState = PivotState.ZERO; - - } - - public double getCurrentAngle() { - return pivotEncoder.getPosition(); - } - - public PivotState getTargetState() { - return targetState; - } - - public void setState(PivotState targetState) { - pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition); - this.targetState = targetState; - } - - public boolean atState(PivotState state) { - return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; - } - } - From b54c6029837df08d31e804d4461b27312135e779 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Sun, 19 Jan 2025 17:14:36 -0800 Subject: [PATCH 21/27] update conversion factor, works intakeless --- .vscode/settings.json | 2 +- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 55 ++++++++++--------- .../pivot/SetPivotVerticalCommand.java | 6 ++ .../commands/pivot/SetPivotZeroCommand.java | 6 ++ .../intake/pivot/PivotSubsystem.java | 12 +++- 6 files changed, 55 insertions(+), 32 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 6adedc37..64b765cf 100644 --- a/.vscode/settings.json +++ b/.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 -Xmx8G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e5c1f1e9..42e3b047 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,15 +154,15 @@ public static class VisionConstants{ } public static class IntakeConstants { - public static final int PIVOT_ID = 0; + public static final int PIVOT_ID = 1; public static final int ROLLER_ID = 0; public static final int INTAKE_SENSOR_ID = 0; public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change public static final double OUTTAKE_POSITION = 0; //TODO: change - public static final double VERTICAL_POSITION = 0; //TODO: change - public static final double PIVOT_TOLERANCE = 0; //TODO: change + public static final double VERTICAL_POSITION = Units.degreesToRadians(90); //TODO: change + public static final double PIVOT_TOLERANCE = .05; //TODO: change } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12eb720a..33a046e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,9 +39,9 @@ public class RobotContainer { private BaseDriveController driveController; - private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); + // private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final PivotSubsystem pivotSubsystem = new PivotSubsystem(); - private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); + // private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); private final VisionSubsystem visionSubsystem2 = new VisionSubsystem( VisionConstants.cameraConfigs[1] ); @@ -53,11 +53,12 @@ public class RobotContainer { ); private CommandPS5Controller mechController; - private Trigger aButton = new Trigger(mechController.button(0)); + private Trigger aButton; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { mechController = new CommandPS5Controller(1); + aButton = new Trigger(mechController.cross()); constructDriveController(); // startLog(); @@ -79,17 +80,17 @@ 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. */ - swerveSubsystem.setDefaultCommand( - new RunCommand(() -> { - swerveSubsystem.setDrivePowers( - driveController.getForwardPower(), - driveController.getLeftPower(), - driveController.getRotatePower() - ); - }, - swerveSubsystem - ) - ); + // swerveSubsystem.setDefaultCommand( + // new RunCommand(() -> { + // swerveSubsystem.setDrivePowers( + // driveController.getForwardPower(), + // driveController.getLeftPower(), + // driveController.getRotatePower() + // ); + // }, + // swerveSubsystem + // ) + // ); aButton.onTrue( new ConditionalCommand( @@ -99,17 +100,21 @@ private void configureBindings() { ) ); - rollerSubsystem.setDefaultCommand(new InstantCommand( () -> { - rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis()); - }, rollerSubsystem)); - - /* Pressing the button resets the field axes to the current robot axes. */ - driveController.bindDriverHeadingReset( - () ->{ - swerveSubsystem.resetDriverHeading(); - }, - swerveSubsystem - ); + // aButton.onTrue( + // new SetPivotVerticalCommand(pivotSubsystem) + // ); + + // rollerSubsystem.setDefaultCommand(new InstantCommand( () -> { + // rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis()); + // }, rollerSubsystem)); + + // /* Pressing the button resets the field axes to the current robot axes. */ + // driveController.bindDriverHeadingReset( + // () ->{ + // swerveSubsystem.resetDriverHeading(); + // }, + // swerveSubsystem + // ); } /** diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index 931d47f4..a342ce87 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -15,10 +15,16 @@ public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.VERTICAL); + System.out.println("setting vertical"); } @Override public boolean isFinished() { return pivotSubsystem.atState(PivotState.VERTICAL); } + + @Override + public void end(boolean interrupted) { + pivotSubsystem.setSpeed(0); + } } diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 5157e2bb..6374b4a8 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -15,10 +15,16 @@ public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.ZERO); + System.out.println("setting horizaontal"); } @Override public boolean isFinished() { return pivotSubsystem.atState(PivotState.ZERO); } + + @Override + public void end(boolean interrupted) { + pivotSubsystem.setSpeed(0); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index fc30dea3..366dd863 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -41,9 +41,10 @@ public PivotSubsystem(){ pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); pivotEncoder = pivotMotor.getEncoder(); + pivotEncoder.setPosition(0); encoderConfig = new EncoderConfig(); - encoderConfig.positionConversionFactor(PIVOT_CONVERSION_FACTOR); + encoderConfig.positionConversionFactor(1./ PIVOT_CONVERSION_FACTOR); softLimitConfig = new SoftLimitConfig(); softLimitConfig.forwardSoftLimitEnabled(true) @@ -56,8 +57,8 @@ public PivotSubsystem(){ sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(closedLoopConfig) - .apply(encoderConfig) - .apply(softLimitConfig); + .apply(encoderConfig); + // .apply(softLimitConfig); pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -77,9 +78,14 @@ public PivotState getTargetState() { public void setState(PivotState targetState) { pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition); + // System.out.println("state set to : " + targetState.getTargetAngle()); this.targetState = targetState; } + public void setSpeed(double speed) { + pivotMotor.set(speed); + } + public boolean atState(PivotState state) { return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; } From 3fa069ba20820993547b782592cc214d80afa1f1 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Mon, 20 Jan 2025 18:52:39 -0800 Subject: [PATCH 22/27] tuned Kg in terms of giant metal block --- src/main/java/frc/robot/Constants.java | 13 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../pivot/SetPivotVerticalCommand.java | 2 +- .../commands/pivot/SetPivotZeroCommand.java | 2 +- .../intake/pivot/PivotSubsystem.java | 26 +++++++++++++++---- 5 files changed, 37 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 42e3b047..8574019a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -157,12 +157,25 @@ public static class IntakeConstants { public static final int PIVOT_ID = 1; public static final int ROLLER_ID = 0; public static final int INTAKE_SENSOR_ID = 0; + public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); + public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change public static final double OUTTAKE_POSITION = 0; //TODO: change public static final double VERTICAL_POSITION = Units.degreesToRadians(90); //TODO: change + public static final double PIVOT_TOLERANCE = .05; //TODO: change + public static final double PIVOT_KG = 1.2; + public static final double PIVOT_KS = 0; + public static final double PIVOT_KV = 0; + + public static final double PIVOT_P = 2.1; + public static final double PIVOT_I = 0; + public static final double PIVOT_D = 0; + + + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 33a046e4..9b6ad03b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,7 +97,7 @@ private void configureBindings() { new SetPivotZeroCommand(pivotSubsystem), new SetPivotVerticalCommand(pivotSubsystem), () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL) - ) + ).withTimeout(3) ); // aButton.onTrue( diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index a342ce87..c0b37f25 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -15,7 +15,7 @@ public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.VERTICAL); - System.out.println("setting vertical"); + System.out.println("vertical" + pivotSubsystem.getPositionError()); } @Override diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 6374b4a8..4fb0ac12 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -15,7 +15,7 @@ public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.ZERO); - System.out.println("setting horizaontal"); + System.out.println("horizontal" + pivotSubsystem.getPositionError()); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index 366dd863..b7da9271 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.intake.pivot; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -13,10 +14,18 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR; +import static frc.robot.Constants.IntakeConstants.PIVOT_D; +import static frc.robot.Constants.IntakeConstants.PIVOT_I; import static frc.robot.Constants.IntakeConstants.PIVOT_ID; +import static frc.robot.Constants.IntakeConstants.PIVOT_KG; +import static frc.robot.Constants.IntakeConstants.PIVOT_KS; +import static frc.robot.Constants.IntakeConstants.PIVOT_KV; +import static frc.robot.Constants.IntakeConstants.PIVOT_P; import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE; @@ -31,11 +40,10 @@ public class PivotSubsystem extends SubsystemBase{ private SparkMaxConfig sparkMaxConfig; private SoftLimitConfig softLimitConfig; - private double p = 1; - private double i = 0; - private double d = 0; + private ArmFeedforward armFeedforward; private PivotState targetState; + private double gravityFF; public PivotSubsystem(){ @@ -53,7 +61,9 @@ public PivotSubsystem(){ .reverseSoftLimit(0); closedLoopConfig = new ClosedLoopConfig(); - closedLoopConfig.pid(p, i, d); + closedLoopConfig.pid(PIVOT_P, PIVOT_I, PIVOT_D, ClosedLoopSlot.kSlot0); + + armFeedforward = new ArmFeedforward(PIVOT_KS, PIVOT_KG, PIVOT_KV); sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(closedLoopConfig) @@ -65,6 +75,7 @@ public PivotSubsystem(){ pivotPID = pivotMotor.getClosedLoopController(); targetState = PivotState.ZERO; + gravityFF = 0; } @@ -77,7 +88,8 @@ public PivotState getTargetState() { } public void setState(PivotState targetState) { - pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition); + gravityFF = armFeedforward.calculate(targetState.getTargetAngle(), 2); + pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition, ClosedLoopSlot.kSlot0, gravityFF, ArbFFUnits.kVoltage); // System.out.println("state set to : " + targetState.getTargetAngle()); this.targetState = targetState; } @@ -89,5 +101,9 @@ public void setSpeed(double speed) { public boolean atState(PivotState state) { return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; } + + public double getPositionError() { + return Math.abs(getCurrentAngle() - targetState.getTargetAngle()); + } } From 93440e77377cbf6518c56aca8f99abe365ff71d6 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Tue, 28 Jan 2025 19:11:28 -0800 Subject: [PATCH 23/27] added bindings --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 28 +++++++++++-------- .../intake/rollers/RollerSubsystem.java | 2 +- 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8574019a..fc5614f1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,7 +155,7 @@ public static class VisionConstants{ public static class IntakeConstants { public static final int PIVOT_ID = 1; - public static final int ROLLER_ID = 0; + public static final int ROLLER_ID = 5; public static final int INTAKE_SENSOR_ID = 0; public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9b6ad03b..4d5ffdb7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,8 @@ package frc.robot; -import frc.robot.commands.pivot.SetPivotVerticalCommand; -import frc.robot.commands.pivot.SetPivotZeroCommand; +// import frc.robot.commands.pivot.SetPivotVerticalCommand; +// import frc.robot.commands.pivot.SetPivotZeroCommand; import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.PS5DriveController; @@ -41,7 +41,7 @@ public class RobotContainer { // private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final PivotSubsystem pivotSubsystem = new PivotSubsystem(); - // private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); + private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); private final VisionSubsystem visionSubsystem2 = new VisionSubsystem( VisionConstants.cameraConfigs[1] ); @@ -53,12 +53,14 @@ public class RobotContainer { ); private CommandPS5Controller mechController; - private Trigger aButton; + private Trigger aButton, lTrigger, rTrigger; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { mechController = new CommandPS5Controller(1); aButton = new Trigger(mechController.cross()); + lTrigger = new Trigger(mechController.L2()); + rTrigger = new Trigger(mechController.R2()); constructDriveController(); // startLog(); @@ -92,13 +94,17 @@ private void configureBindings() { // ) // ); - aButton.onTrue( - new ConditionalCommand( - new SetPivotZeroCommand(pivotSubsystem), - new SetPivotVerticalCommand(pivotSubsystem), - () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL) - ).withTimeout(3) - ); + // aButton.onTrue( + // new ConditionalCommand( + // new SetPivotZeroCommand(pivotSubsystem), + // new SetPivotVerticalCommand(pivotSubsystem), + // () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL) + // ).withTimeout(3) + // ); + + rollerSubsystem.setDefaultCommand(new InstantCommand( () -> { + rollerSubsystem.setRollerPower(.25 * (mechController.getL2Axis() - mechController.getR2Axis())); + }, rollerSubsystem)); // aButton.onTrue( // new SetPivotVerticalCommand(pivotSubsystem) diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java index a97725bc..ee3aeec3 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -23,7 +23,7 @@ public void setRollerPower(double speed) { } public boolean getIntakeSensor() { - return intakeSensor.get(); + return !intakeSensor.get(); } } From 93d56b9ee72c53a5ddaf2cfd54ad92ad7c6f094c Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Sun, 2 Feb 2025 13:33:57 -0800 Subject: [PATCH 24/27] renaming folders for conv --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 5 +++++ .../frc/robot/commands/pivot/SetPivotOuttakeCommand.java | 4 ++-- .../java/frc/robot/commands/pivot/SetPivotSourceCommand.java | 4 ++-- .../frc/robot/commands/pivot/SetPivotVerticalCommand.java | 4 ++-- .../java/frc/robot/commands/pivot/SetPivotZeroCommand.java | 4 ++-- .../java/frc/robot/subsystems/intake/pivot/PivotState.java | 2 +- .../frc/robot/subsystems/intake/pivot/PivotSubsystem.java | 2 +- .../frc/robot/subsystems/intake/rollers/RollerSubsystem.java | 2 +- 9 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc5614f1..e49422f9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,8 +154,8 @@ public static class VisionConstants{ } public static class IntakeConstants { - public static final int PIVOT_ID = 1; - public static final int ROLLER_ID = 5; + public static final int PIVOT_ID = 14; + public static final int ROLLER_ID = 16; public static final int INTAKE_SENSOR_ID = 0; public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4d5ffdb7..5a4ed92d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,11 @@ 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.Intake.pivot.PivotState; +import frc.robot.subsystems.Intake.pivot.PivotSubsystem; +import frc.robot.subsystems.Intake.rollers.RollerSubsystem; +import frc.robot.subsystems.PhoenixLoggingSubsystem.PhoenixLoggingSubsystem; import frc.robot.subsystems.Vision.VisionSubsystem; import frc.robot.subsystems.intake.pivot.PivotState; import frc.robot.subsystems.intake.pivot.PivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java index b8b4ce30..b732060a 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intake.pivot.PivotState; -import frc.robot.subsystems.intake.pivot.PivotSubsystem; +import frc.robot.subsystems.Intake.pivot.PivotState; +import frc.robot.subsystems.Intake.pivot.PivotSubsystem; public class SetPivotOuttakeCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java index 7e72148a..ba9a5d7b 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intake.pivot.PivotState; -import frc.robot.subsystems.intake.pivot.PivotSubsystem; +import frc.robot.subsystems.Intake.pivot.PivotState; +import frc.robot.subsystems.Intake.pivot.PivotSubsystem; public class SetPivotSourceCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index c0b37f25..ae625a8b 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intake.pivot.PivotState; -import frc.robot.subsystems.intake.pivot.PivotSubsystem; +import frc.robot.subsystems.Intake.pivot.PivotState; +import frc.robot.subsystems.Intake.pivot.PivotSubsystem; public class SetPivotVerticalCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 4fb0ac12..3686c9db 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intake.pivot.PivotState; -import frc.robot.subsystems.intake.pivot.PivotSubsystem; +import frc.robot.subsystems.Intake.pivot.PivotState; +import frc.robot.subsystems.Intake.pivot.PivotSubsystem; public class SetPivotZeroCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java index 7c72b25c..a2b8c427 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake.pivot; +package frc.robot.subsystems.Intake.pivot; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index b7da9271..96ff644c 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake.pivot; +package frc.robot.subsystems.Intake.pivot; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java index ee3aeec3..c6b6f438 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake.rollers; +package frc.robot.subsystems.Intake.rollers; import static frc.robot.Constants.IntakeConstants.INTAKE_SENSOR_ID; import static frc.robot.Constants.IntakeConstants.ROLLER_ID; From dd33702aa069428019afd4bc9f9401f35d842a52 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Mon, 3 Feb 2025 12:13:03 -0800 Subject: [PATCH 25/27] move config to constants --- src/main/java/frc/robot/Constants.java | 32 +++++++++++++- .../intake/pivot/PivotSubsystem.java | 44 ++++--------------- 2 files changed, 39 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e49422f9..32f1a06b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,21 @@ package frc.robot; +import java.util.OptionalInt; +import java.util.Optional; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; + +import java.util.OptionalInt; +import java.util.Optional; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; + import org.photonvision.PhotonPoseEstimator.PoseStrategy; import edu.wpi.first.math.geometry.Pose3d; @@ -13,6 +28,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.subsystems.Vision.CameraConfig; import frc.robot.util.PolynomialRegression; +import frc.robot.util.Motors.LoggedSparkMaxConfig; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -158,7 +174,7 @@ public static class IntakeConstants { public static final int ROLLER_ID = 16; public static final int INTAKE_SENSOR_ID = 0; - public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI); + public static final double PIVOT_CONVERSION_FACTOR = (2 * Math.PI) / 30.; public static final double ZERO_POSITION = 0; public static final double SOURCE_POSITION = 0; //TODO: change @@ -175,7 +191,19 @@ public static class IntakeConstants { public static final double PIVOT_I = 0; public static final double PIVOT_D = 0; - + public static final LoggedSparkMaxConfig PivotMotorLoggedSparkMaxConfig = new LoggedSparkMaxConfig( + PIVOT_ID, + new ClosedLoopConfig().pid(PIVOT_P, PIVOT_I, PIVOT_D, ClosedLoopSlot.kSlot0), + new EncoderConfig().positionConversionFactor(PIVOT_CONVERSION_FACTOR), + OptionalInt.empty(), + Optional.of( + new SoftLimitConfig() + .forwardSoftLimitEnabled(true) + .forwardSoftLimit(Units.degreesToRadians(90)) + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(0) + ) + ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index 96ff644c..f10e7882 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -3,6 +3,9 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; +import frc.robot.util.Motors.LoggedSparkMax; +import frc.robot.util.Motors.LoggedSparkMaxConfig; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -28,55 +31,26 @@ import static frc.robot.Constants.IntakeConstants.PIVOT_P; import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE; +import java.util.Optional; +import java.util.OptionalLong; + public class PivotSubsystem extends SubsystemBase{ - private SparkMax pivotMotor; + private LoggedSparkMax pivotMotor; private RelativeEncoder pivotEncoder; private SparkClosedLoopController pivotPID; - private EncoderConfig encoderConfig; - private ClosedLoopConfig closedLoopConfig; - private SparkMaxConfig sparkMaxConfig; - private SoftLimitConfig softLimitConfig; - private ArmFeedforward armFeedforward; private PivotState targetState; private double gravityFF; public PivotSubsystem(){ - - pivotMotor = new SparkMax(PIVOT_ID, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); - pivotEncoder.setPosition(0); - - encoderConfig = new EncoderConfig(); - encoderConfig.positionConversionFactor(1./ PIVOT_CONVERSION_FACTOR); - - softLimitConfig = new SoftLimitConfig(); - softLimitConfig.forwardSoftLimitEnabled(true) - .forwardSoftLimit(Units.degreesToRadians(90)) - .reverseSoftLimitEnabled(true) - .reverseSoftLimit(0); - - closedLoopConfig = new ClosedLoopConfig(); - closedLoopConfig.pid(PIVOT_P, PIVOT_I, PIVOT_D, ClosedLoopSlot.kSlot0); - - armFeedforward = new ArmFeedforward(PIVOT_KS, PIVOT_KG, PIVOT_KV); - - sparkMaxConfig = new SparkMaxConfig(); - sparkMaxConfig.apply(closedLoopConfig) - .apply(encoderConfig); - // .apply(softLimitConfig); - - pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - pivotPID = pivotMotor.getClosedLoopController(); - + pivotMotor = new LoggedSparkMax(IntakeConstants.PivotMotorLoggedSparkMaxConfig); targetState = PivotState.ZERO; + armFeedforward = new ArmFeedforward(PIVOT_KS, PIVOT_KG, PIVOT_KV); gravityFF = 0; - } public double getCurrentAngle() { From dafd3b6b0f860665c2a5aeb918264f9b9578ef39 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Wed, 12 Feb 2025 15:39:22 -0800 Subject: [PATCH 26/27] tune pvito r1 --- src/main/java/frc/robot/Constants.java | 23 +++++----- src/main/java/frc/robot/RobotContainer.java | 44 ++++++++++++++----- .../pivot/SetPivotOuttakeCommand.java | 2 +- .../commands/pivot/SetPivotSourceCommand.java | 2 +- .../pivot/SetPivotVerticalCommand.java | 4 +- .../commands/pivot/SetPivotZeroCommand.java | 4 +- .../intake/pivot/PivotSubsystem.java | 19 ++++++-- .../frc/robot/util/Motors/LoggedSparkMax.java | 8 ++++ 8 files changed, 74 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 32f1a06b..ff53c1a0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,7 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.MAXMotionConfig; import com.revrobotics.spark.config.SoftLimitConfig; import java.util.OptionalInt; @@ -17,10 +18,10 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.MAXMotionConfig; import com.revrobotics.spark.config.SoftLimitConfig; import org.photonvision.PhotonPoseEstimator.PoseStrategy; - import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -172,24 +173,24 @@ public static class VisionConstants{ public static class IntakeConstants { public static final int PIVOT_ID = 14; public static final int ROLLER_ID = 16; - public static final int INTAKE_SENSOR_ID = 0; + public static final int INTAKE_SENSOR_ID = 1; public static final double PIVOT_CONVERSION_FACTOR = (2 * Math.PI) / 30.; public static final double ZERO_POSITION = 0; - public static final double SOURCE_POSITION = 0; //TODO: change - public static final double OUTTAKE_POSITION = 0; //TODO: change - public static final double VERTICAL_POSITION = Units.degreesToRadians(90); //TODO: change + public static final double SOURCE_POSITION = Units.degreesToRadians(45); //TODO: change + public static final double OUTTAKE_POSITION = Units.degreesToRadians(-45); //TODO: change + public static final double VERTICAL_POSITION = Units.degreesToRadians(82); //TODO: change - public static final double PIVOT_TOLERANCE = .05; //TODO: change + public static final double PIVOT_TOLERANCE = .08; //TODO: change - public static final double PIVOT_KG = 1.2; + public static final double PIVOT_KG = 3; public static final double PIVOT_KS = 0; public static final double PIVOT_KV = 0; - public static final double PIVOT_P = 2.1; + public static final double PIVOT_P = .3; public static final double PIVOT_I = 0; - public static final double PIVOT_D = 0; + public static final double PIVOT_D = 0.3; public static final LoggedSparkMaxConfig PivotMotorLoggedSparkMaxConfig = new LoggedSparkMaxConfig( PIVOT_ID, @@ -199,9 +200,9 @@ public static class IntakeConstants { Optional.of( new SoftLimitConfig() .forwardSoftLimitEnabled(true) - .forwardSoftLimit(Units.degreesToRadians(90)) + .forwardSoftLimit(Units.degreesToRadians(85)) .reverseSoftLimitEnabled(true) - .reverseSoftLimit(0) + .reverseSoftLimit(Units.degreesToRadians(-50)) ) ); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5a4ed92d..e44f8e19 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,10 @@ package frc.robot; +import frc.robot.commands.Pivot.SetPivotOuttakeCommand; +import frc.robot.commands.Pivot.SetPivotSourceCommand; +import frc.robot.commands.Pivot.SetPivotVerticalCommand; +import frc.robot.commands.Pivot.SetPivotZeroCommand; // import frc.robot.commands.pivot.SetPivotVerticalCommand; // import frc.robot.commands.pivot.SetPivotZeroCommand; import frc.robot.controllers.BaseDriveController; @@ -58,7 +62,7 @@ public class RobotContainer { ); private CommandPS5Controller mechController; - private Trigger aButton, lTrigger, rTrigger; + private Trigger aButton, lTrigger, rTrigger, lBumper, rBumper; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -67,6 +71,9 @@ public RobotContainer() { lTrigger = new Trigger(mechController.L2()); rTrigger = new Trigger(mechController.R2()); + lBumper = new Trigger(mechController.L1()); + rBumper = new Trigger(mechController.R1()); + constructDriveController(); // startLog(); setVisionDataInterface(); @@ -99,17 +106,30 @@ private void configureBindings() { // ) // ); - // aButton.onTrue( - // new ConditionalCommand( - // new SetPivotZeroCommand(pivotSubsystem), - // new SetPivotVerticalCommand(pivotSubsystem), - // () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL) - // ).withTimeout(3) - // ); - - rollerSubsystem.setDefaultCommand(new InstantCommand( () -> { - rollerSubsystem.setRollerPower(.25 * (mechController.getL2Axis() - mechController.getR2Axis())); - }, rollerSubsystem)); + rBumper.onTrue( + new ConditionalCommand( + new SetPivotOuttakeCommand(pivotSubsystem), + new ConditionalCommand( + new SetPivotZeroCommand(pivotSubsystem).andThen(new SetPivotSourceCommand(pivotSubsystem)), + new SetPivotSourceCommand(pivotSubsystem), + () -> (pivotSubsystem.getCurrentAngle() < PivotState.ZERO.getTargetAngle())), + () -> (pivotSubsystem.getTargetState() == PivotState.SOURCE) + ) + ); + + lBumper.onTrue( + new SetPivotVerticalCommand(pivotSubsystem).withTimeout(2.5) + ); + + rollerSubsystem.setDefaultCommand(new ConditionalCommand( + new InstantCommand( () -> { + //ps5 trigger's range is -1 to 1, with non-input position being -1. This maps the range -1 to 1 to 0 to 1. + rollerSubsystem.setRollerPower(.25 * (mechController.getR2Axis() + 1.) / 2.); + }, rollerSubsystem), + new InstantCommand( () -> { + rollerSubsystem.setRollerPower(.25 * (mechController.getR2Axis() - mechController.getL2Axis())); + }, rollerSubsystem), + () -> rollerSubsystem.getIntakeSensor())); // aButton.onTrue( // new SetPivotVerticalCommand(pivotSubsystem) diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java index b732060a..4a2c2cf6 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.pivot; +package frc.robot.commands.Pivot; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.pivot.PivotState; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java index ba9a5d7b..4409dabb 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.pivot; +package frc.robot.commands.Pivot; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.pivot.PivotState; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index ae625a8b..930ac9a1 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.pivot; +package frc.robot.commands.Pivot; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.pivot.PivotState; @@ -15,7 +15,7 @@ public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.VERTICAL); - System.out.println("vertical" + pivotSubsystem.getPositionError()); + // System.out.println("vertical" + pivotSubsystem.getPositionError()); } @Override diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 3686c9db..28dad6f3 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.pivot; +package frc.robot.commands.Pivot; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.pivot.PivotState; @@ -15,7 +15,7 @@ public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) { @Override public void execute() { this.pivotSubsystem.setState(PivotState.ZERO); - System.out.println("horizontal" + pivotSubsystem.getPositionError()); + // System.out.println("horizontal" + pivotSubsystem.getPositionError()); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index f10e7882..5947949b 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -49,12 +49,13 @@ public class PivotSubsystem extends SubsystemBase{ public PivotSubsystem(){ pivotMotor = new LoggedSparkMax(IntakeConstants.PivotMotorLoggedSparkMaxConfig); targetState = PivotState.ZERO; + pivotMotor.setPosition(Units.degreesToRadians(82)); armFeedforward = new ArmFeedforward(PIVOT_KS, PIVOT_KG, PIVOT_KV); gravityFF = 0; } public double getCurrentAngle() { - return pivotEncoder.getPosition(); + return pivotMotor.getPosition(); } public PivotState getTargetState() { @@ -62,8 +63,14 @@ public PivotState getTargetState() { } public void setState(PivotState targetState) { - gravityFF = armFeedforward.calculate(targetState.getTargetAngle(), 2); - pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition, ClosedLoopSlot.kSlot0, gravityFF, ArbFFUnits.kVoltage); + if (getCurrentAngle() < targetState.getTargetAngle()) { + gravityFF = armFeedforward.calculate((targetState.getTargetAngle()), 1.5); + // System.out.println(targetState.getTargetAngle() + Units.degreesToRadians(90)); + } + else { + gravityFF = 0; + } + pivotMotor.setReference(targetState.getTargetAngle(), ControlType.kPosition, ClosedLoopSlot.kSlot0, gravityFF, ArbFFUnits.kVoltage); // System.out.println("state set to : " + targetState.getTargetAngle()); this.targetState = targetState; } @@ -79,5 +86,11 @@ public boolean atState(PivotState state) { public double getPositionError() { return Math.abs(getCurrentAngle() - targetState.getTargetAngle()); } + + @Override + public void periodic() { + pivotMotor.publishStats(); + } + } diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index ded98285..df1808a5 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -134,6 +134,14 @@ public double getPosition(){ return encoder.getPosition(); } + /** + * Sets the position of the motor in rotations after taking the position + * conversion factor into account + */ + public void setPosition(double value){ + encoder.setPosition(value); + } + /** * Gets the velocity of the motor in RPM after taking the velocity * conversion factor into account From 75243b36e73ed28a1c911543227401b7ba5c1f14 Mon Sep 17 00:00:00 2001 From: eerinn Date: Wed, 19 Feb 2025 16:45:49 -0800 Subject: [PATCH 27/27] actual pivot branch this time --- .../java/frc/robot/Commands/AutoAlignCommand.java | 2 +- src/main/java/frc/robot/Constants.java | 5 +++++ src/main/java/frc/robot/RobotContainer.java | 14 +++++--------- .../commands/pivot/SetPivotOuttakeCommand.java | 6 +++--- .../commands/pivot/SetPivotSourceCommand.java | 6 +++--- .../commands/pivot/SetPivotVerticalCommand.java | 6 +++--- .../robot/commands/pivot/SetPivotZeroCommand.java | 6 +++--- .../robot/subsystems/intake/pivot/PivotState.java | 2 +- .../subsystems/intake/pivot/PivotSubsystem.java | 2 +- .../subsystems/intake/rollers/RollerSubsystem.java | 2 +- .../robot/subsystems/swerve/SwerveSubsystem.java | 1 - .../java/frc/robot/util/Motors/LoggedTalon.java | 2 +- 12 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Commands/AutoAlignCommand.java b/src/main/java/frc/robot/Commands/AutoAlignCommand.java index b13b897c..7df51db2 100644 --- a/src/main/java/frc/robot/Commands/AutoAlignCommand.java +++ b/src/main/java/frc/robot/Commands/AutoAlignCommand.java @@ -1,4 +1,4 @@ -package frc.robot.Commands; +package frc.robot.commands; import frc.robot.subsystems.swerve.SwerveSubsystem; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ff53c1a0..46996f7f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,6 +95,11 @@ public static class SwerveConstants { public static final boolean STATE_DEBUG = false; } + public static class DebugConstants{ + public static final Boolean REV_DEBUG = false; + public static final Boolean CTRE_DEBUG = false; + } + public static class LoggingConstants{ public static final String SWERVE_TABLE = "SwerveStats"; public static final String REV_TABLE = "MotorStats"; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e44f8e19..b02a6bf1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,6 @@ package frc.robot; -import frc.robot.commands.Pivot.SetPivotOuttakeCommand; -import frc.robot.commands.Pivot.SetPivotSourceCommand; -import frc.robot.commands.Pivot.SetPivotVerticalCommand; -import frc.robot.commands.Pivot.SetPivotZeroCommand; // import frc.robot.commands.pivot.SetPivotVerticalCommand; // import frc.robot.commands.pivot.SetPivotZeroCommand; import frc.robot.controllers.BaseDriveController; @@ -28,16 +24,16 @@ 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.Intake.pivot.PivotState; -import frc.robot.subsystems.Intake.pivot.PivotSubsystem; -import frc.robot.subsystems.Intake.rollers.RollerSubsystem; -import frc.robot.subsystems.PhoenixLoggingSubsystem.PhoenixLoggingSubsystem; import frc.robot.subsystems.Vision.VisionSubsystem; import frc.robot.subsystems.intake.pivot.PivotState; import frc.robot.subsystems.intake.pivot.PivotSubsystem; import frc.robot.subsystems.intake.rollers.RollerSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.Constants.VisionConstants; +import frc.robot.commands.pivot.SetPivotOuttakeCommand; +import frc.robot.commands.pivot.SetPivotSourceCommand; +import frc.robot.commands.pivot.SetPivotVerticalCommand; +import frc.robot.commands.pivot.SetPivotZeroCommand; /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -48,7 +44,7 @@ public class RobotContainer { private BaseDriveController driveController; - // private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final PivotSubsystem pivotSubsystem = new PivotSubsystem(); private final RollerSubsystem rollerSubsystem = new RollerSubsystem(); private final VisionSubsystem visionSubsystem2 = new VisionSubsystem( diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java index 4a2c2cf6..b8b4ce30 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java @@ -1,8 +1,8 @@ -package frc.robot.commands.Pivot; +package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.pivot.PivotState; -import frc.robot.subsystems.Intake.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotOuttakeCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java index 4409dabb..7e72148a 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -1,8 +1,8 @@ -package frc.robot.commands.Pivot; +package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.pivot.PivotState; -import frc.robot.subsystems.Intake.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotSourceCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java index 930ac9a1..5e8ff432 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -1,8 +1,8 @@ -package frc.robot.commands.Pivot; +package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.pivot.PivotState; -import frc.robot.subsystems.Intake.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotVerticalCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java index 28dad6f3..5ff5b0cb 100644 --- a/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -1,8 +1,8 @@ -package frc.robot.commands.Pivot; +package frc.robot.commands.pivot; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.pivot.PivotState; -import frc.robot.subsystems.Intake.pivot.PivotSubsystem; +import frc.robot.subsystems.intake.pivot.PivotState; +import frc.robot.subsystems.intake.pivot.PivotSubsystem; public class SetPivotZeroCommand extends Command{ private PivotSubsystem pivotSubsystem; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java index a2b8c427..7c72b25c 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Intake.pivot; +package frc.robot.subsystems.intake.pivot; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index 5947949b..77146a59 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Intake.pivot; +package frc.robot.subsystems.intake.pivot; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java index c6b6f438..ee3aeec3 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Intake.rollers; +package frc.robot.subsystems.intake.rollers; import static frc.robot.Constants.IntakeConstants.INTAKE_SENSOR_ID; import static frc.robot.Constants.IntakeConstants.ROLLER_ID; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index c06aedd0..81fb4434 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -21,7 +21,6 @@ import static frc.robot.Constants.SwerveConstants.*; import static frc.robot.Constants.LoggingConstants.*; -import static frc.robot.Constants.DebugConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 4472f829..cd44b90f 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -1,7 +1,7 @@ package frc.robot.util.Motors; -import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; import static frc.robot.Constants.DebugConstants.CTRE_DEBUG; +import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; import java.util.EnumSet;