Skip to content

intake: Plug arm absolute encoder into motor controller #480

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
auscompgeek wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

intake: Plug arm absolute encoder into motor controller #480

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 8 additions & 12 deletions components/intake.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
SparkMax,
SparkMaxConfig,
)
from wpilib import DutyCycleEncoder
from wpimath.controller import (
LinearQuadraticRegulator_2_1,
)
Expand All @@ -17,8 +16,7 @@
from wpimath.system.plant import DCMotor, LinearSystemId
from wpimath.trajectory import TrapezoidProfile

from ids import DioChannel, SparkId, TalonId
from utilities.rev import configure_through_bore_encoder
from ids import SparkId, TalonId


class IntakeComponent:
Expand All @@ -27,7 +25,7 @@ class IntakeComponent:

# Offset is measured in the vertical position
VERTICAL_ENCODER_VALUE = 4.610450
ARM_ENCODER_OFFSET = VERTICAL_ENCODER_VALUE - math.pi / 2.0
ARM_ENCODER_OFFSET = -(VERTICAL_ENCODER_VALUE - math.pi / 2.0) / math.tau
DEPLOYED_ANGLE_LOWER = 3.392366 - ARM_ENCODER_OFFSET
DEPLOYED_ANGLE_UPPER = 3.892366 - ARM_ENCODER_OFFSET
RETRACTED_ANGLE = 4.610450 - ARM_ENCODER_OFFSET
Expand All @@ -44,9 +42,6 @@ def __init__(self, intake_mech_root: wpilib.MechanismRoot2d) -> None:
self.intake_motor.setInverted(True)

self.arm_motor = SparkMax(SparkId.INTAKE_ARM, SparkMax.MotorType.kBrushless)
self.encoder = DutyCycleEncoder(DioChannel.INTAKE_ENCODER, math.tau, 0)
configure_through_bore_encoder(self.encoder)
self.encoder.setInverted(True)

spark_config = SparkMaxConfig()
spark_config.inverted(False)
Expand All @@ -56,6 +51,10 @@ def __init__(self, intake_mech_root: wpilib.MechanismRoot2d) -> None:
spark_config.encoder.velocityConversionFactor(
(1 / 60) * math.tau * (1 / self.gear_ratio)
)
spark_config.absoluteEncoder.positionConversionFactor(math.tau)
spark_config.absoluteEncoder.velocityConversionFactor(math.tau * (1 / 60))
spark_config.absoluteEncoder.zeroOffset(IntakeComponent.ARM_ENCODER_OFFSET)
spark_config.absoluteEncoder.inverted(True)

self.arm_motor.configure(
spark_config,
Expand All @@ -64,6 +63,7 @@ def __init__(self, intake_mech_root: wpilib.MechanismRoot2d) -> None:
)

self.motor_encoder = self.arm_motor.getEncoder()
self.encoder = self.arm_motor.getAbsoluteEncoder()

plant = LinearSystemId.singleJointedArmSystem(
DCMotor.NEO(1), self.ARM_MOI, self.gear_ratio
Expand Down Expand Up @@ -129,10 +129,6 @@ def retract(self):
):
self._force_retract()

@feedback
def raw_encoder(self) -> float:
return self.encoder.get()

def position_degrees(self) -> float:
return math.degrees(self.position())

Expand All @@ -141,7 +137,7 @@ def position(self) -> float:
return self.loop.xhat(0)

def position_observation(self) -> float:
return self.encoder.get() - IntakeComponent.ARM_ENCODER_OFFSET
return self.encoder.getPosition()

@feedback
def position_observation_degrees(self) -> float:
Expand Down
8 changes: 3 additions & 5 deletions physics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,8 +176,8 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
self.intake_arm_motor = rev.SparkMaxSim(
robot.intake_component.arm_motor, intake_arm_gearbox
)
self.intake_arm_encoder_sim = DutyCycleEncoderSim(
robot.intake_component.encoder
self.intake_arm_encoder_sim = rev.SparkAbsoluteEncoderSim(
robot.intake_component.arm_motor
)
self.intake_arm = SparkArmSim(
SingleJointedArmSim(
Expand Down Expand Up @@ -323,9 +323,7 @@ def update_sim(self, now: float, tm_diff: float) -> None:

# Update intake arm simulation
self.intake_arm.update(tm_diff)
self.intake_arm_encoder_sim.set(
self.intake_arm.mech_sim.getAngle() + IntakeComponent.ARM_ENCODER_OFFSET
)
self.intake_arm_encoder_sim.setPosition(self.intake_arm.mech_sim.getAngle())

# Update wrist simulation
self.wrist.update(tm_diff)
Expand Down