Merge pull request #131 from thedropbears/add_shaft_encoder

components/wrist.py

@@ -1,52 +1,50 @@
 import math
 import time
 
-from magicbot import feedback, tunable
-from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig
-from wpilib import DigitalInput
-from wpimath.controller import ArmFeedforward
+from magicbot import feedback
+from rev import (
+    SparkMax,
+    SparkMaxConfig,
+)
+from wpilib import AnalogEncoder, AnalogInput
+from wpimath.controller import ArmFeedforward, PIDController
 from wpimath.trajectory import TrapezoidProfile
 
-from ids import DioChannel, SparkId
+from ids import AnalogChannel, SparkId
 from utilities.functions import clamp
 
 
 class WristComponent:
+    ENCODER_ZERO_OFFSET = 4.427696
     MAXIMUM_DEPRESSION = math.radians(-113.0)
-    MAXIMUM_ELEVATION = math.radians(-10.0)
+    MAXIMUM_ELEVATION = math.radians(0)
     NEUTRAL_ANGLE = math.radians(-90.0)
 
     WRIST_MAX_VEL = math.radians(30.0)
     WRIST_MAX_ACC = math.radians(15.0)
-
-    angle_change_rate_while_zeroing = tunable(math.radians(0.1))
-    wrist_gear_ratio = 432.0
+    wrist_gear_ratio = 350.628
     TOLERANCE = math.radians(3.0)
 
-    zeroing_voltage = tunable(-1.0)
-
-    has_indexed = tunable(False)
-
     def __init__(self):
-        self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH)
+        self.wrist_encoder_raw = AnalogInput(AnalogChannel.WRIST_ENCODER)
 
-        self.wrist = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless)
+        self.wrist_encoder = AnalogEncoder(self.wrist_encoder_raw, math.tau, 0)
+        self.wrist_encoder.setInverted(False)
+        self.wrist_encoder.setVoltagePercentageRange(0.2 / 5, 4.8 / 5)
 
-        self.wrist_controller = self.wrist.getClosedLoopController()
+        self.motor = SparkMax(SparkId.WRIST, SparkMax.MotorType.kBrushless)
 
         wrist_config = SparkMaxConfig()
         wrist_config.inverted(False)
         wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
-        wrist_config.closedLoop.P(
-            7.6813,
-            ClosedLoopSlot.kSlot0,
-        )
-        wrist_config.closedLoop.D(69.887, ClosedLoopSlot.kSlot0)
+
         self.wrist_profile = TrapezoidProfile(
             TrapezoidProfile.Constraints(self.WRIST_MAX_VEL, self.WRIST_MAX_ACC)
         )
+        # Values need to be modified with new gear ratio
+        self.pid = PIDController(Kp=13.387, Ki=0, Kd=0.0078403)
 
-        self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.09, kV=8.42, kA=0.0)
+        self.wrist_ff = ArmFeedforward(kS=0.42619, kG=0.13, kV=6.83, kA=0.0)
 
         wrist_config.encoder.positionConversionFactor(
             math.tau * (1 / self.wrist_gear_ratio)
@@ -55,21 +53,21 @@ def __init__(self):
             (1 / 60) * math.tau * (1 / self.wrist_gear_ratio)
         )
 
-        self.wrist.configure(
+        self.motor.configure(
             wrist_config,
             SparkMax.ResetMode.kResetSafeParameters,
             SparkMax.PersistMode.kPersistParameters,
         )
 
-        self.encoder = self.wrist.getEncoder()
+        self.motor_encoder = self.motor.getEncoder()
 
         self.desired_angle = WristComponent.NEUTRAL_ANGLE
 
     def on_enable(self):
         self.tilt_to(WristComponent.NEUTRAL_ANGLE)
         wrist_config = SparkMaxConfig()
         wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
-        self.wrist.configure(
+        self.motor.configure(
             wrist_config,
             SparkMax.ResetMode.kNoResetSafeParameters,
             SparkMax.PersistMode.kNoPersistParameters,
@@ -78,34 +76,31 @@ def on_enable(self):
     def on_disable(self):
         wrist_config = SparkMaxConfig()
         wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast)
-        self.wrist.configure(
+        self.motor.configure(
             wrist_config,
             SparkMax.ResetMode.kNoResetSafeParameters,
             SparkMax.PersistMode.kNoPersistParameters,
         )
 
-    def zero_wrist(self) -> None:
-        self.has_indexed = False
-
     @feedback
-    def wrist_at_bottom_limit(self) -> bool:
-        return not self.switch.get()
+    def encoder_raw_volts(self) -> float:
+        return self.wrist_encoder_raw.getVoltage()
 
     @feedback
     def inclination(self) -> float:
-        return self.encoder.getPosition()
+        return self.wrist_encoder.get() - self.ENCODER_ZERO_OFFSET
 
     @feedback
     def inclination_deg(self) -> float:
-        return math.degrees(self.encoder.getPosition())
+        return math.degrees(self.inclination())
 
     @feedback
     def shoot_angle_deg(self) -> float:
         return self.inclination_deg() + 90
 
     @feedback
     def current_velocity(self) -> float:
-        return self.encoder.getVelocity()
+        return self.motor_encoder.getVelocity()
 
     @feedback
     def at_setpoint(self) -> bool:
@@ -122,23 +117,13 @@ def reset_windup(self) -> None:
         self.tilt_to(self.inclination())
 
     def execute(self) -> None:
-        if self.wrist_at_bottom_limit():
-            self.encoder.setPosition(self.MAXIMUM_DEPRESSION)
-            self.has_indexed = True
-
-        if not self.has_indexed:
-            self.wrist.setVoltage(self.zeroing_voltage)
-            return
-
         desired_state = self.wrist_profile.calculate(
             time.monotonic() - self.last_setpoint_update_time,
             TrapezoidProfile.State(self.inclination(), self.current_velocity()),
             TrapezoidProfile.State(self.desired_angle, 0.0),
         )
         ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity)
-        self.wrist_controller.setReference(
-            desired_state.position,
-            SparkMax.ControlType.kPosition,
-            slot=ClosedLoopSlot.kSlot0,
-            arbFeedforward=ff,
+
+        self.motor.setVoltage(
+            self.pid.calculate(self.inclination(), desired_state.position) + ff
         )

ids.py

@@ -77,3 +77,10 @@ class RioSerialNumber(enum.StrEnum):
 
     TEST_BOT = "0305cc42"
     COMP_BOT = "03062898"
+
+
+@enum.unique
+class AnalogChannel(enum.IntEnum):
+    """roboRIO Analog input channel number"""
+
+    WRIST_ENCODER = 0

physics.py

@@ -120,7 +120,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
         ]
 
         self.wrist_motor = rev.SparkMaxSim(
-            sparkMax=robot.wrist.wrist, motor=DCMotor.NEO(1)
+            sparkMax=robot.wrist.motor, motor=DCMotor.NEO(1)
         )
 
         self.imu = robot.chassis.imu.sim_state

robot.py

@@ -261,8 +261,6 @@ def testPeriodic(self) -> None:
 
         self.algae_manipulator_component.execute()
 
-        if self.gamepad.getLeftBumper():
-            self.wrist.zero_wrist()
         if self.gamepad.getLeftTriggerAxis() > 0.3:
             self.wrist.tilt_to(self.inclination_angle)
         self.wrist.execute()