diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 012787f8..98ae7051 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.BuildConstants; import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -17,6 +21,31 @@ public Robot() { m_robotContainer = new RobotContainer(); } + @Override + public void robotInit() { + if (isReal()) { + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All Changes Committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncommitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs")); + Logger.addDataReceiver(new NT4Publisher()); + } + Logger.start(); + } + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e97e4b45..245e9951 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,8 +8,14 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.coral.EnableEjectBeamCommand; +import frc.robot.commands.coral.OpenLoopCoralCommand; import frc.robot.commands.drive.DriveTeleopCommand; import frc.robot.controllers.FlyskyJoystick; +import frc.robot.subsystems.coral.CoralIO; +import frc.robot.subsystems.coral.CoralIOFX; +import frc.robot.subsystems.coral.CoralSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.drive.Swerve; import org.strykeforce.telemetry.TelemetryController; @@ -21,18 +27,30 @@ public class RobotContainer { private final XboxController xboxController = new XboxController(1); private final Joystick driveJoystick = new Joystick(0); + private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick); private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new); + private final CoralSubsystem coralSubsystem; + private final CoralIO coralIO; + public RobotContainer() { swerve = new Swerve(); driveSubsystem = new DriveSubsystem(swerve); - configureBindings(); + coralIO = new CoralIOFX(); + coralSubsystem = new CoralSubsystem(coralIO); + + configureTelemetry(); configureDriverBindings(); + configureOperatorBindings(); } - private void configureBindings() {} + private void configureTelemetry() { + telemetryService.register(driveSubsystem); + telemetryService.register(coralSubsystem); + telemetryService.start(); + } private void configureDriverBindings() { driveSubsystem.setDefaultCommand( @@ -40,6 +58,22 @@ private void configureDriverBindings() { () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem)); } + private void configureOperatorBindings() { + // Stop Coral + new JoystickButton(xboxController, XboxController.Button.kB.value) + .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0)); + + // Intake Coral + new JoystickButton(xboxController, XboxController.Button.kY.value) + .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5)) + .onTrue(new EnableEjectBeamCommand(false, coralSubsystem)); + + // Eject Coral + new JoystickButton(xboxController, XboxController.Button.kA.value) + .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5)) + .onTrue(new EnableEjectBeamCommand(true, coralSubsystem)); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java b/src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java new file mode 100644 index 00000000..97bb8ab4 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.coral.CoralSubsystem; + +public class EnableEjectBeamCommand extends InstantCommand { + private CoralSubsystem coralSubsystem; + private Boolean enable; + + public EnableEjectBeamCommand(Boolean enable, CoralSubsystem coralSubsystem) { + this.coralSubsystem = coralSubsystem; + this.enable = enable; + } + + @Override + public void initialize() { + coralSubsystem.enableEjectBeam(enable); + } +} diff --git a/src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java b/src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java new file mode 100644 index 00000000..a736540c --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.coral.CoralSubsystem; + +public class OpenLoopCoralCommand extends InstantCommand { + private CoralSubsystem coralSubsystem; + private double pct; + + public OpenLoopCoralCommand(CoralSubsystem coralSubsystem, double pct) { + this.coralSubsystem = coralSubsystem; + this.pct = pct; + } + + @Override + public void initialize() { + coralSubsystem.setPct(pct); + } +} diff --git a/src/main/java/frc/robot/constants/CoralConstants.java b/src/main/java/frc/robot/constants/CoralConstants.java index 3ac015bb..c1ffe4ad 100644 --- a/src/main/java/frc/robot/constants/CoralConstants.java +++ b/src/main/java/frc/robot/constants/CoralConstants.java @@ -1,3 +1,82 @@ package frc.robot.constants; -public class CoralConstants {} +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.AngularVelocity; + +public class CoralConstants { + public static int kCoralFxId = 0; + + public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1); + + public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1); + public static final AngularVelocity kEjectingSpeed = RotationsPerSecond.of(1); + + // Coral Talon FX Config + public static TalonFXSConfiguration getFXConfig() { + TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(10) + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + fxsConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(true) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxsConfig.HardwareLimitSwitch = hwLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0) + .withKA(0); + fxsConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration(0) + .withMotionMagicCruiseVelocity(0) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(0); + fxsConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Coast); + fxsConfig.MotorOutput = motorOut; + + return fxsConfig; + } +} diff --git a/src/main/java/frc/robot/subsystems/algae/algaeIO.java b/src/main/java/frc/robot/subsystems/algae/algaeIO.java index a7712178..3fb9d43f 100644 --- a/src/main/java/frc/robot/subsystems/algae/algaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/algaeIO.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.algae; +import com.ctre.phoenix6.signals.ReverseLimitValue; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -8,6 +10,8 @@ public interface algaeIO { @AutoLog public static class IOInputs { public AngularVelocity velocity; + public Angle location; + public ReverseLimitValue reverseLimitSwitch; } public default void updateInputs(IOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/algae/algaeIOFX.java b/src/main/java/frc/robot/subsystems/algae/algaeIOFX.java index 38550683..3f28d28a 100644 --- a/src/main/java/frc/robot/subsystems/algae/algaeIOFX.java +++ b/src/main/java/frc/robot/subsystems/algae/algaeIOFX.java @@ -1,3 +1,62 @@ package frc.robot.subsystems.algae; -public class algaeIOFX {} +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ReverseLimitValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.AlgaeConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; + +/** Implementation of the algaeIO interface using TalonFX motors. */ +public class algaeIOFX implements algaeIO { + private Logger logger; + private final TalonFX talonFX; + private StatusSignal currVelocity; + private StatusSignal reverseLimitSwitch; + private StatusSignal currAngle; + + // FX Access objects + TalonFXConfigurator configurator; + private MotionMagicDutyCycle positionRequest = + new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); + + // Constructor to initialize the TalonFX motor controller + public algaeIOFX() { + logger = LoggerFactory.getLogger(this.getClass()); + talonFX = new TalonFX(AlgaeConstants.kFxId); + reverseLimitSwitch = talonFX.getReverseLimit(); + currVelocity = talonFX.getVelocity(); + currAngle = talonFX.getPosition(); + } + + @Override + public void updateInputs(IOInputs inputs) { + // inputs.reverseLimitSwitch = reverseLimitSwitch.refresh().getValue().value == 1; + inputs.velocity = currVelocity.refresh().getValue(); + inputs.location = currAngle.refresh().getValue(); + } + + @Override + public void setPosition(double position) { + talonFX.setPosition(position); + } + + public Angle getPosition() { + return currAngle.getValue(); + } + + @Override + public void zero() { + // not doing one for now + } + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFX, true); + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIO.java b/src/main/java/frc/robot/subsystems/coral/CoralIO.java index ebd16b79..0f5a36fc 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralIO.java +++ b/src/main/java/frc/robot/subsystems/coral/CoralIO.java @@ -1,3 +1,29 @@ package frc.robot.subsystems.coral; -public class CoralIO {} +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.units.measure.AngularVelocity; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface CoralIO { + + @AutoLog + public static class CoralIOInputs { + public AngularVelocity velocity = RotationsPerSecond.of(0.0); + public boolean isFwdBeamBroken = false; + public boolean isRevBeamBroken = false; + } + + public default void setVelocity(AngularVelocity velocity) {} + + public default void setPct(double percentOutput) {} + + public default void enableFwdLimitSwitch(boolean enabled) {} + + public default void enableRevLimitSwitch(boolean enabled) {} + + public default void updateInputs(CoralIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIOFX.java b/src/main/java/frc/robot/subsystems/coral/CoralIOFX.java new file mode 100644 index 00000000..ae9ef0e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralIOFX.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.coral; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFXS; +import com.ctre.phoenix6.signals.ForwardLimitValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.CoralConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; + +public class CoralIOFX implements CoralIO { + // private objects + private Logger logger; + private TalonFXS talonFx; + + // FX Access objects + private TalonFXSConfigurator configurator; + private MotionMagicVelocityDutyCycle velocityRequest = + new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0); + private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0).withEnableFOC(false); + private StatusSignal curVelocity; + private StatusSignal fwdLimitSwitch; + private StatusSignal revLimitSwitch; + + public CoralIOFX() { + logger = LoggerFactory.getLogger(this.getClass()); + talonFx = new TalonFXS(CoralConstants.kCoralFxId); + + // Config controller + configurator = talonFx.getConfigurator(); + configurator.apply(new TalonFXSConfiguration()); // Factory default motor controller + configurator.apply(CoralConstants.getFXConfig()); + + // Attach status signals + curVelocity = talonFx.getVelocity(); + fwdLimitSwitch = talonFx.getForwardLimit(); + revLimitSwitch = talonFx.getReverseLimit(); + } + + @Override + public void setVelocity(AngularVelocity velocity) { + logger.info("Setting velocity to {} rots per second", velocity.in(RotationsPerSecond)); + + talonFx.setControl(velocityRequest.withVelocity(velocity)); + } + + @Override + public void setPct(double percentOutput) { + talonFx.setControl(dutyCycleRequest.withOutput(percentOutput)); + } + + @Override + public void enableFwdLimitSwitch(boolean enabled) { + talonFx + .getConfigurator() + .apply(CoralConstants.getFXConfig().HardwareLimitSwitch.withForwardLimitEnable(enabled)); + } + + @Override + public void enableRevLimitSwitch(boolean enabled) { + talonFx + .getConfigurator() + .apply(CoralConstants.getFXConfig().HardwareLimitSwitch.withReverseLimitEnable(enabled)); + } + + @Override + public void updateInputs(CoralIOInputs inputs) { + BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); + inputs.velocity = curVelocity.getValue(); + inputs.isFwdBeamBroken = fwdLimitSwitch.getValue().value == 0; // FIXME check right value + inputs.isRevBeamBroken = revLimitSwitch.getValue().value == 0; // FIXME check right value + } + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFx, true); + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java index f87d1e21..af24f3d3 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java +++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java @@ -1,3 +1,134 @@ package frc.robot.subsystems.coral; -public class CoralSubsystem {} +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.CoralConstants; +import frc.robot.standards.ClosedLoopSpeedSubsystem; +import java.util.Set; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class CoralSubsystem extends MeasurableSubsystem implements ClosedLoopSpeedSubsystem { + private final CoralIO io; + private final CoralIOInputsAutoLogged inputs = new CoralIOInputsAutoLogged(); + private AngularVelocity setpoint = RotationsPerSecond.of(0.0); + private CoralState curState = CoralState.IDLE; + private org.slf4j.Logger logger = LoggerFactory.getLogger(CoralSubsystem.class); + + public CoralSubsystem(CoralIO io) { + this.io = io; + } + + public CoralState getState() { + return curState; + } + + @Override + public AngularVelocity getSpeed() { + return inputs.velocity; + } + + public void setState(CoralState state) { + logger.info("{} -> {}", curState, state); + this.curState = state; + } + + @Override + public void setSpeed(AngularVelocity speed) { + setpoint = speed; + io.setVelocity(speed); + } + + public void setPct(double percentOutput) { + io.setPct(percentOutput); + } + + public void enableEjectBeam(boolean enable) { + io.enableRevLimitSwitch(enable); + } + + @Override + public boolean atSpeed() { + return setpoint.minus(inputs.velocity).abs(RotationsPerSecond) + <= CoralConstants.kCloseEnough.in(RotationsPerSecond); + } + + public boolean isEnterBeamBroken() { + return inputs.isFwdBeamBroken; + } + + public boolean isExitBeamBroken() { + return inputs.isRevBeamBroken; + } + + public void intake() { + io.enableRevLimitSwitch(true); + setSpeed(CoralConstants.kIntakingSpeed); + setState(CoralState.INTAKING); + } + + public void eject() { + io.enableRevLimitSwitch(false); + setSpeed(CoralConstants.kEjectingSpeed); + setState(CoralState.EJECTING); + } + + // Periodic Function + @Override + public void periodic() { + // Read Inputs + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + + // State Machine + switch (curState) { + case IDLE -> {} + case INTAKING -> { + if (isEnterBeamBroken()) { + setState(CoralState.CORAL_LOADING); + } + } + case CORAL_LOADING -> { + if (isExitBeamBroken()) { + setState(CoralState.HAS_CORAL); + } + } + case HAS_CORAL -> {} + case EJECTING -> { + if (!isExitBeamBroken()) { + setState(CoralState.EMPTY); + } + } + case EMPTY -> {} + default -> {} + } + + // Log Outputs + Logger.recordOutput("Coral/curState", curState); + Logger.recordOutput("Coral/setpoint", setpoint.in(RotationsPerSecond)); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); + io.registerWith(telemetryService); + } + + @Override + public Set getMeasures() { + return Set.of(new Measure("State", () -> curState.ordinal())); + } + + public enum CoralState { + IDLE, + HAS_CORAL, + CORAL_LOADING, + INTAKING, + EJECTING, + EMPTY + } +}