diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java new file mode 100644 index 0000000..690ce8b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems.feeder; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*; + +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 frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; + +public class KickerIOSpark implements KickerIO { + + private final SparkFlex flex; + private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + private final SparkInputs sparkInputs; + + public KickerIOSpark() { + flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + encoder = flex.getEncoder(); + controller = flex.getClosedLoopController(); + + var config = new SparkFlexConfig(); + config + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + config + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0); + + flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); + } + + @Override + public void updateInputs(KickerIOInputs inputs) { + // Update inputs + inputs.connected = true; + inputs.velocityMetersPerSec = sparkInputs.getVelocity(); + inputs.appliedVolts = sparkInputs.getAppliedVolts(); + inputs.currentAmps = sparkInputs.getOutputCurrent(); + } + + @Override + public void setOpenLoop(double output) { + flex.setVoltage(output); + ; + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + double feedforwardVolts = + RobotConstants.kNominalVoltage + * tangentialVelocity.in(MetersPerSecond) + / maxTangentialVelocity.in(MetersPerSecond); + controller.setSetpoint( + tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java new file mode 100644 index 0000000..acdff43 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems.feeder; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static frc.robot.subsystems.feeder.FeederConstants.SpindexerConstants.*; + +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 frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; + +public class SpindexerIOSpark implements SpindexerIO { + + private final SparkFlex flex; + private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + private final SparkInputs sparkInputs; + + public SpindexerIOSpark() { + flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless); + encoder = flex.getEncoder(); + controller = flex.getClosedLoopController(); + + var config = new SparkFlexConfig(); + config + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + config + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0); + + flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); + } + + @Override + public void updateInputs(SpindexerIOInputs inputs) { + + // Update inputs + inputs.connected = true; + inputs.velocityMetersPerSec = sparkInputs.getVelocity(); + inputs.appliedVolts = sparkInputs.getAppliedVolts(); + inputs.currentAmps = sparkInputs.getOutputCurrent(); + } + + @Override + public void setOpenLoop(double output) { + flex.setVoltage(output); + ; + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + double feedforwardVolts = + RobotConstants.kNominalVoltage + * tangentialVelocity.in(MetersPerSecond) + / maxTangentialVelocity.in(MetersPerSecond); + controller.setSetpoint( + tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +}