diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index dd215b6..cbaceb3 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -22,7 +22,7 @@ public interface CAN { int ELEVATOR_MASTER_SPARK = -1; int ELEVATOR_SLAVE_SPARK = -1; int ALGAE_JOINT_SPARK = -1; - int CORAL_JOINT_SPARK = -1; + int CORAL_JOINT_TALON = -1; int GRIPPER_TALON = -1; int STORAGE_SPARK = -1; } diff --git a/src/main/java/frc/robot/subsystems/CoralJoint.java b/src/main/java/frc/robot/subsystems/CoralJoint.java index 77e476f..d24020c 100644 --- a/src/main/java/frc/robot/subsystems/CoralJoint.java +++ b/src/main/java/frc/robot/subsystems/CoralJoint.java @@ -1,8 +1,10 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.spark.SparkLowLevel; import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem; import com.spikes2212.util.smartmotorcontrollers.SparkWrapper; +import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.RobotMap; @@ -10,7 +12,7 @@ public class CoralJoint extends SmartMotorControllerGenericSubsystem { public enum StoragePose { - INTAKE(-1), PLACEMENT(-1), RESTING(-1); + INTAKE(-1), L1(-1), L2(-1), L3(-1), RESTING(-1); public final double neededPitch; @@ -28,7 +30,7 @@ public enum StoragePose { private static final double DISTANCE_PER_PULSE = GEAR_RATIO * DEGREES_IN_ROTATIONS; private static final double SECONDS_IN_MINUTE = 60; - private final SparkWrapper spark; + private final TalonFXWrapper talonFX; private final DigitalInput topLimit; private final DigitalInput bottomLimit; @@ -37,20 +39,19 @@ public enum StoragePose { public static CoralJoint getInstance() { if (instance == null) { instance = new CoralJoint(NAMESPACE_NAME, - SparkWrapper.createSparkMax(RobotMap.CAN.CORAL_JOINT_SPARK, SparkLowLevel.MotorType.kBrushless), + new TalonFXWrapper(RobotMap.CAN.CORAL_JOINT_TALON), new DigitalInput(RobotMap.DIO.CORAL_JOINT_TOP_LIMIT), new DigitalInput(RobotMap.DIO.CORAL_JOINT_BOTTOM_LIMIT)); } return instance; } - private CoralJoint(String namespaceName, SparkWrapper spark, DigitalInput topLimit, DigitalInput bottomLimit) { - super(namespaceName, spark); - this.spark = spark; + private CoralJoint(String namespaceName, TalonFXWrapper talonFX, DigitalInput topLimit, DigitalInput bottomLimit) { + super(namespaceName, talonFX); + this.talonFX = talonFX; this.topLimit = topLimit; this.bottomLimit = bottomLimit; - spark.setPositionConversionFactor(DISTANCE_PER_PULSE); - spark.setVelocityConversionFactor(DISTANCE_PER_PULSE / SECONDS_IN_MINUTE); + talonFX.setEncoderConversionFactor(DISTANCE_PER_PULSE); configureDashboard(); } @@ -61,15 +62,15 @@ public boolean canMove(double speed) { public void calibrateEncoderPosition() { if (topLimit.get()) { - spark.setPosition(StoragePose.RESTING.neededPitch); + talonFX.setPosition(STORAGE_POSE.L3.neededPitch); } else if (bottomLimit.get()) { - spark.setPosition(StoragePose.PLACEMENT.neededPitch); + talonFX.setPosition(STORAGE_POSE.RESTING.neededPitch); } } public void configureDashboard() { namespace.putBoolean("top limit", topLimit::get); namespace.putBoolean("bottom limit", bottomLimit::get); - namespace.putNumber("storage pose", spark::getPosition); + namespace.putNumber("storage pose", talonFX::getPosition); } }