How to set an encoder's position in a TrapezoidalProfileSubsystem?

LHACK4142 · March 2, 2024, 3:48am

Our chain, which is after our encoder, sometimes slips which causes the encoder’s position to be misaligned with the arm’s actual position. I’m trying to write a command that will let the drivers tell the encoder where it should be (ie set its position, ie gaslight it into thinking it’s where it should be), but when I sim the code and use GaslightCrankEncoders, I see the simulated arm instantaneously snap to where I gaslighted it to be, then snap back after maybe 1/50 seconds(?) then motion profile to where I gaslighted it to be.

I suspect this probably related to the UseState method’s self.sim_encoder.setPosition(setpoint.position), but I’m not sure.

Any help would be very appreciated.

The relevant code:

github.com

aesatchien/FRC2429_2024/blob/main/robot/commands/gaslight_crank_encoders.py

import math
import commands2
from wpilib import SmartDashboard

import constants
from subsystems.lower_crank_trapezoid import LowerCrankArmTrapezoidal

class GaslightCrankEncoders(commands2.CommandBase):  # change the name for your command
    # Gaslights the crank encoder.  Cycles the position to gaslight between the minimum position and our shoot position.
    def __init__(self, container, lower_crank: LowerCrankArmTrapezoidal) -> None:
        super().__init__()
        self.setName('Gaslight crank encoders')  # change this to something appropriate for this command
        self.container = container
        self.lower_crank = lower_crank
        self.positions_to_gaslight_to = [constants.k_crank_arm_dict['min_angle'], constants.k_crank_presets['shoot']['lower']]
        self.counter = 0
        # self.addRequirements(self.container.)  # commandsv2 version of requirements

    def initialize(self) -> None:
        """Called just before this Command runs the first time."""

This file has been truncated. show original

viggy96 · March 2, 2024, 5:42am

Honestly solving this in software isn’t a real solution. Fixing the slack in the mechanism to prevent chain slip is. There are many approaches to doing this that are viable.

A human can’t effectively predict how far off the encoder has slipped for the driver to input that.