diff --git a/components/wrist.py b/components/wrist.py index ad0c2cfa..1b68d03c 100644 --- a/components/wrist.py +++ b/components/wrist.py @@ -7,21 +7,19 @@ SparkMax, SparkMaxConfig, ) -from wpilib import DutyCycleEncoder from wpimath.controller import ArmFeedforward, PIDController from wpimath.trajectory import TrapezoidProfile -from ids import DioChannel, SparkId +from ids import SparkId from utilities.functions import clamp from utilities.rev import ( configure_spark_ephemeral, configure_spark_reset_and_persist, - configure_through_bore_encoder, ) class WristComponent: - ENCODER_ZERO_OFFSET = 3.46463832679 + ENCODER_ZERO_OFFSET = 3.46463832679 / math.tau COM_DIFFERENCE = 0.54722467321 MAXIMUM_DEPRESSION = math.radians(-112.0) + COM_DIFFERENCE MAXIMUM_ELEVATION = math.radians(0) + COM_DIFFERENCE @@ -42,10 +40,6 @@ def __init__(self, mech_root: wpilib.MechanismRoot2d): "wrist_COM", length=0.5, angle=0, color=wpilib.Color8Bit(wpilib.Color.kBlue) ) - self.wrist_encoder = DutyCycleEncoder(DioChannel.WRIST_ENCODER, math.tau, 0) - configure_through_bore_encoder(self.wrist_encoder) - self.wrist_encoder.setInverted(False) - self.motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless) self.idle_mode = SparkMaxConfig.IdleMode.kBrake @@ -72,10 +66,15 @@ def __init__(self, mech_root: wpilib.MechanismRoot2d): wrist_config.encoder.velocityConversionFactor( (1 / 60) * math.tau * (1 / self.wrist_gear_ratio) ) + wrist_config.absoluteEncoder.positionConversionFactor(math.tau) + wrist_config.absoluteEncoder.velocityConversionFactor(math.tau * (1 / 60)) + wrist_config.absoluteEncoder.zeroOffset(self.ENCODER_ZERO_OFFSET) + wrist_config.absoluteEncoder.inverted(False) configure_spark_reset_and_persist(self.motor, wrist_config) self.motor_encoder = self.motor.getEncoder() + self.wrist_encoder = self.motor.getAbsoluteEncoder() self.desired_state = TrapezoidProfile.State(WristComponent.NEUTRAL_ANGLE, 0.0) self.tracked_state = self.desired_state @@ -111,13 +110,9 @@ def toggle_neutral_mode(self) -> None: configure_spark_ephemeral(self.motor, wrist_config) - @feedback - def raw_encoder(self) -> float: - return self.wrist_encoder.get() - @feedback def inclination(self) -> float: - return self.wrist_encoder.get() - self.ENCODER_ZERO_OFFSET + return self.wrist_encoder.getPosition() @feedback def inclination_deg(self) -> float: diff --git a/physics.py b/physics.py index 3f860794..6ff9c764 100644 --- a/physics.py +++ b/physics.py @@ -195,7 +195,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): wrist_gearbox = DCMotor.NEO(1) wrist_motor = rev.SparkMaxSim(robot.wrist.motor, wrist_gearbox) - self.wrist_encoder_sim = DutyCycleEncoderSim(robot.wrist.wrist_encoder) + self.wrist_encoder_sim = rev.SparkAbsoluteEncoderSim(robot.wrist.motor) wrist_sim = SingleJointedArmSim( wrist_gearbox, @@ -329,9 +329,7 @@ def update_sim(self, now: float, tm_diff: float) -> None: # Update wrist simulation self.wrist.update(tm_diff) - self.wrist_encoder_sim.set( - self.wrist.mech_sim.getAngle() + WristComponent.ENCODER_ZERO_OFFSET - ) + self.wrist_encoder_sim.setPosition(self.wrist.mech_sim.getAngle()) # Simulate algae pick up if self.floor_intake.current_state == "intaking":