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/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 2d6a95d1..46996f7f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,8 +4,24 @@ package frc.robot; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; +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.MAXMotionConfig; +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.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; @@ -13,6 +29,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 @@ -78,8 +95,15 @@ 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"; + public static final String CTRE_TABLE = "MotorStats"; } public static class VisionConstants{ @@ -150,4 +174,42 @@ 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 = 14; + public static final int ROLLER_ID = 16; + 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 = 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 = .08; //TODO: change + + 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 = .3; + public static final double PIVOT_I = 0; + public static final double PIVOT_D = 0.3; + + 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(85)) + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(Units.degreesToRadians(-50)) + ) + ); + + } } \ 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 6c6d5ffe..b02a6bf1 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,13 +15,25 @@ 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.FieldManagementSubsystem.FieldManagementSubsystem; 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} @@ -31,6 +45,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 +57,19 @@ public class RobotContainer { VisionConstants.cameraConfigs[3] ); + private CommandPS5Controller mechController; + private Trigger aButton, lTrigger, rTrigger, lBumper, rBumper; + /** 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()); + + lBumper = new Trigger(mechController.L1()); + rBumper = new Trigger(mechController.R1()); + constructDriveController(); // startLog(); setVisionDataInterface(); @@ -63,25 +90,58 @@ 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 + // ) + // ); + + 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) + ) ); - /* Pressing the button resets the field axes to the current robot axes. */ - driveController.bindDriverHeadingReset( - () ->{ - swerveSubsystem.resetDriverHeading(); - }, - swerveSubsystem + 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) + // ); + + // 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/SetPivotOuttakeCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java new file mode 100644 index 00000000..b8b4ce30 --- /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.intake.pivot.PivotState; +import frc.robot.subsystems.intake.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/SetPivotSourceCommand.java b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java new file mode 100644 index 00000000..7e72148a --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java @@ -0,0 +1,24 @@ +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; + +public class SetPivotSourceCommand extends Command{ + private PivotSubsystem pivotSubsystem; + + public SetPivotSourceCommand(PivotSubsystem pivotSubsystem) { + this.addRequirements(pivotSubsystem); + this.pivotSubsystem = pivotSubsystem; + } + + @Override + public void execute() { + this.pivotSubsystem.setState(PivotState.SOURCE); + } + + @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..5e8ff432 --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java @@ -0,0 +1,30 @@ +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; + +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); + // System.out.println("vertical" + pivotSubsystem.getPositionError()); + } + + @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 new file mode 100644 index 00000000..5ff5b0cb --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java @@ -0,0 +1,30 @@ +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; + +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); + // System.out.println("horizontal" + pivotSubsystem.getPositionError()); + } + + @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/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..77146a59 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -0,0 +1,96 @@ +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; +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; +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.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; + +import java.util.Optional; +import java.util.OptionalLong; + + +public class PivotSubsystem extends SubsystemBase{ + + private LoggedSparkMax pivotMotor; + private RelativeEncoder pivotEncoder; + private SparkClosedLoopController pivotPID; + + private ArmFeedforward armFeedforward; + + private PivotState targetState; + private double gravityFF; + + 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 pivotMotor.getPosition(); + } + + public PivotState getTargetState() { + return targetState; + } + + public void setState(PivotState targetState) { + 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; + } + + public void setSpeed(double speed) { + pivotMotor.set(speed); + } + + public boolean atState(PivotState state) { + return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE; + } + + public double getPositionError() { + return Math.abs(getCurrentAngle() - targetState.getTargetAngle()); + } + + @Override + public void periodic() { + pivotMotor.publishStats(); + } + + } + 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..ee3aeec3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/RollerSubsystem.java @@ -0,0 +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 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/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/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java new file mode 100644 index 00000000..df1808a5 --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -0,0 +1,258 @@ +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.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; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; + +public class LoggedSparkMax { + + private final int canId; + private final SparkMax motor; + private final SparkMaxConfig sparkMaxConfig; + private final ClosedLoopConfig closedLoopConfig; + private final RelativeEncoder encoder; + private SparkClosedLoopController closedLoopController; + private double targetReference; + + private NetworkTableInstance ntInstance; + private NetworkTable motorStatsTable; + 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 velocityLogEntry; + private DoubleLogEntry busVoltageLogEntry; + private DoubleLogEntry outputCurrtLogEntry; + private DoubleLogEntry appliedOutputLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry targetReferenceLogEntry; + + public LoggedSparkMax(LoggedSparkMaxConfig config) { + canId = config.getCanId(); + motor = new SparkMax(canId, MotorType.kBrushless); + sparkMaxConfig = config.getSparkMaxConfig(); + closedLoopConfig = config.getClosedLoopConfig(); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + encoder = motor.getEncoder(); + + initLogs(); + intiNT(); + if(REV_DEBUG){ + enableDebug(); + } + } + + /** + * 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); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + } + + /** + * 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); + targetReference = 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(); + } + + /** + * 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 + */ + public double getVelocity(){ + return encoder.getVelocity(); + } + + /** + * Initializes NetworkTables + * @param canId motor's CAN ID + */ + private void intiNT() { + ntInstance = NetworkTableInstance.getDefault(); + motorStatsTable = ntInstance.getTable(REV_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "Position" + ).publish(); + 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(); + } + + /** + * Initializes logs + * @param canId motor's CAN ID + */ + private void initLogs(){ + positionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); + + targetReferenceLogEntry= + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetReference"); + + 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"); + } + + /** + * 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(){ + 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 stats into log file + */ + public void logStats(){ + positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); + velocityLogEntry.append(getVelocity(), GRTUtil.getFPGATime()); + busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); + outputCurrtLogEntry.append( + motor.getOutputCurrent(), GRTUtil.getFPGATime() + ); + appliedOutputLogEntry.append( + motor.getAppliedOutput(), GRTUtil.getFPGATime() + ); + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + targetReferenceLogEntry.append(targetReference, 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..efdcd32f --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -0,0 +1,68 @@ +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 { + private final int canId; + private final ClosedLoopConfig closedLoopConfig; + private final SparkMaxConfig sparkMaxConfig; + private final EncoderConfig encoderConfig; + + public LoggedSparkMaxConfig( + int canId, ClosedLoopConfig closedLoopConfig, + EncoderConfig encoderConfig, OptionalInt followCanId, + Optional softLimitConfig + ){ + this.canId = canId; + this.closedLoopConfig = closedLoopConfig; + this.encoderConfig = encoderConfig; + this.sparkMaxConfig = new SparkMaxConfig(); + this.sparkMaxConfig.apply(closedLoopConfig); + this.sparkMaxConfig.apply(encoderConfig); + + if(followCanId.isPresent()){ + this.sparkMaxConfig.follow(followCanId.getAsInt()); + } + if(softLimitConfig.isPresent()){ + this.sparkMaxConfig.apply(softLimitConfig.get()); + } + } + + /** + * 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; + } +} 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..cd44b90f --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -0,0 +1,284 @@ +package frc.robot.util.Motors; + +import static frc.robot.Constants.DebugConstants.CTRE_DEBUG; +import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; + +import java.util.EnumSet; + +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.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +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; + +public class LoggedTalon{ + + private final TalonFX motor; + + private final int canId; + private double[] pidsvg = new double[6]; + + private NetworkTableInstance ntInstance; + 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 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"); + for (int i = 0; i < 4; i++) { + 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(); + } + } + + /** + * Sets the motor's position reference + * @param position position reference + */ + public void setPositionReference(double position){ + targetPosition = 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){ + 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 + * @param i kI + * @param d kD + * @param s kS for static friction + * @param v kV Voltage feed forward + */ + public void configurePIDSVG(double p, double i, double d, double s, double v, + double g + ) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kP = p; + slot0Configs.kI = i; + slot0Configs.kD = d; + slot0Configs.kS = s; + slot0Configs.kV = v; + slot0Configs.kG = g; + + motor.getConfigurator().apply(slot0Configs); + } + + /** + * initializes network table and entries + * @param canId motor's CAN ID + */ + private void initNT(int canId){ + ntInstance = NetworkTableInstance.getDefault(); + motorStatsTable = ntInstance.getTable(CTRE_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "position" + ).publish(); + veloPublisher = motorStatsTable.getDoubleTopic(canId + "velo").publish(); + appliedVlotsPublisher = motorStatsTable.getDoubleTopic( + canId + "appliedVolts" + ).publish(); + supplyCurrentPublisher = motorStatsTable.getDoubleTopic( + canId + "supplyCurrent" + ).publish(); + statorCurrentPublisher = motorStatsTable.getDoubleTopic( + canId + "statorCurrent" + ).publish(); + temperaturePublisher = motorStatsTable.getDoubleTopic( + canId + "temperature" + ).publish(); + targetPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "targetPosition" + ).publish(); + targetVelocityPublisher = motorStatsTable.getDoubleTopic( + canId + "targetVelocity" + ).publish(); + } + + /** + * Initializes log entries + * @param canId drive motor's CAN ID + */ + private void initLogs(int canId){ + positionLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "position" + ); + 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" + ); + targetPositionLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetPosition" + ); + targetVelocityLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetVelocity" + ); + } + + /** + * 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 + * @return position of the motor in rotations + */ + 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(); + } + + /** + * Publishes motor stats to NT for logging + */ + public void publishStats(){ + 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() + ); + + 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() + ); + + targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + targetVelocityLogEntry.append(targetVelocity, GRTUtil.getFPGATime()); + } +}