diff --git a/build.gradle b/build.gradle index 8620dfd..bf72faa 100644 --- a/build.gradle +++ b/build.gradle @@ -155,6 +155,8 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' + // Ensure Java compiler treats source files as UTF-8 so Unicode comments/characters compile + options.encoding = 'UTF-8' } // Create version file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a1..3a232e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,8 +72,8 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; -import frc.robot.subsystems.intake.RollerIOSimTalonFX; -import frc.robot.subsystems.intake.RollerIOTalonFX; +import frc.robot.subsystems.intake.RollerIOSimSpark; +import frc.robot.subsystems.intake.RollerIOSpark; import frc.robot.subsystems.launcher.FlywheelIO; import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIOTalonFX; @@ -198,8 +198,8 @@ public Robot() { if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOReal()); intake = new Intake( - new RollerIOTalonFX(RollerConstants.upperRollerConfig), - new RollerIOTalonFX(RollerConstants.lowerRollerConfig), + new RollerIOSpark(RollerConstants.upperRollerConfig), + new RollerIOSpark(RollerConstants.lowerRollerConfig), new IntakeArmIOReal()); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); @@ -245,8 +245,8 @@ public Robot() { var intakeArmIOSim = new IntakeArmIOSim(); intake = new Intake( - new RollerIOSimTalonFX(RollerConstants.upperRollerConfig), - new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig), + new RollerIOSimSpark(RollerConstants.upperRollerConfig), + new RollerIOSimSpark(RollerConstants.lowerRollerConfig), intakeArmIOSim); pneumaticsSimulator = new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1)); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f8e53da..8b89b3e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -3,13 +3,8 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; public class IntakeConstants { /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ @@ -20,18 +15,13 @@ public static class RollerConstants { // motor controller public static final double motorReduction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReduction); - public static final Slot0Configs velocityVoltageGains = - new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); - public static final Slot1Configs velocityTorqueCurrentGains = - new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5); - + public static final int numMotors = 1; public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; - // simulation - public static final DCMotor gearbox = DCMotor.getKrakenX60(2); + // roller constants + public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters + public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec // configs public static final RollerConfig upperRollerConfig = diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java new file mode 100644 index 0000000..ef4e044 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -0,0 +1,103 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; +import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; + +public class RollerIOSimSpark implements RollerIO { + private static final double KICKER_MOI_KG_M2 = 0.00052; + private static final double KP = 0.11; + private static final double KD = 0.0; + private static final LinearVelocity maxTangentialVelocity = + MetersPerSecond.of( + NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + * rollerRadius.in(Meters) + / motorReduction); + private static final DCMotor gearbox = DCMotor.getNeoVortex(numMotors); + + private final DCMotorSim rollerSim; + + private final SparkFlex flex; + private final SparkClosedLoopController controller; + private final SparkFlexSim flexSim; + + public RollerIOSimSpark(RollerConfig rollerConfig) { + flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); + controller = flex.getClosedLoopController(); + + var config = new SparkFlexConfig(); + config + .inverted(rollerConfig.inverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + config + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); + + flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + flexSim = new SparkFlexSim(flex, gearbox); + + rollerSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + // Update simulation state + double busVoltage = RoboRioSim.getVInVoltage(); + rollerSim.setInput(flexSim.getAppliedOutput() * busVoltage); + rollerSim.update(Robot.defaultPeriodSecs); + flexSim.iterate(rollerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + + // Update inputs + inputs.connected = true; + inputs.velocityMetersPerSec = flexSim.getVelocity() * rollerRadius.in(Meters); + inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); + inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); + } + + @Override + public void setOpenLoop(Voltage volts) { + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + double feedforwardVolts = + RobotConstants.kNominalVoltage + * tangentialVelocity.in(MetersPerSecond) + / maxTangentialVelocity.in(MetersPerSecond); + controller.setSetpoint( + tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index 1739699..87096d0 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; @@ -14,6 +16,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -26,6 +29,13 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOSimTalonFX implements RollerIO { + private static final double kP = 0.11; + private static final double kD = 0.0; + private static final Slot0Configs velocityVoltageGains = + new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); + private static final Slot1Configs velocityTorqueCurrentGains = + new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); + private static final DCMotor gearbox = DCMotor.getKrakenX60(numMotors); private final DCMotorSim rollerSim; diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java new file mode 100644 index 0000000..48357e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -0,0 +1,96 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; + +public class RollerIOSpark implements RollerIO { + private static final double KP = 0.001; + private static final double KD = 0.0; + private static final LinearVelocity maxTangentialVelocity = + MetersPerSecond.of( + NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + * rollerRadius.in(Meters) + / motorReduction); + + private final SparkFlex flex; + private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + private final SparkInputs sparkInputs; + + public RollerIOSpark(RollerConfig rollerConfig) { + flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); + encoder = flex.getEncoder(); + controller = flex.getClosedLoopController(); + + var config = new SparkFlexConfig(); + config + .inverted(rollerConfig.inverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + config + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor) + .uvwAverageDepth(2) + .uvwMeasurementPeriod(8); + + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); + + tryUntilOk( + flex, + 5, + () -> + flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + inputs.connected = sparkInputs.isConnected(); + inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters); + inputs.appliedVolts = sparkInputs.getAppliedVolts(); + inputs.currentAmps = sparkInputs.getOutputCurrent(); + } + + @Override + public void setOpenLoop(Voltage volts) { + flex.setVoltage(volts); + ; + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + double feedforwardVolts = + RobotConstants.kNominalVoltage + * tangentialVelocity.in(MetersPerSecond) + / maxTangentialVelocity.in(MetersPerSecond); + controller.setSetpoint( + tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 7619a05..05cc765 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; @@ -23,6 +25,13 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOTalonFX implements RollerIO { + private static final double kP = 0.11; + private static final double kD = 0.0; + private static final Slot0Configs velocityVoltageGains = + new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); + private static final Slot1Configs velocityTorqueCurrentGains = + new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); + private final TalonFX motor; private final TalonFXConfiguration config; private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling);