Encoder Driver

# Encoder driver 
# by Kai De La Cruz & Kenzie Goldman 1/30/25

from time import ticks_ms, ticks_diff, ticks_add 
from pyb import Pin, Timer 
import math

class Encoder: 
    '''A quadrature encoder decoding interface encapsulated in a python 
    class'''

    def __init__(self, tim_num, chA_pin, chB_pin):
        '''Initializes an Encoder object'''
        self.prev_time = ticks_ms() #time of the most recent update
        self.position = 0.0 #total accumulated position of the encoder
        self.prev_count = 0.0 #counter value from the most recent update
        self.delta = 0.0 #change in count between last two updates
        self.velocity_buffer = []
        self.window_size = 5  # Number of samples to average over
        # copy params to attrbutes
        self.chA_pin = Pin(chA_pin)
        self.chB_pin = Pin(chB_pin)

        # Configure timer channels for encoder interface
        self.tim = Timer(tim_num, prescaler=0, period=0xFFFF)
        self.encd_CHA = self.tim.channel(1, pin=self.chA_pin, mode=Timer.ENC_AB)
        self.encd_CHB = self.tim.channel(2, pin=self.chB_pin, mode=Timer.ENC_AB)

    def update(self):
        '''Runs one update step on the encoder's timer counter to keep track of 
        the change in count and check for counter reload'''
        current_time = ticks_ms()
        self.dt = ticks_diff(current_time, self.prev_time)/1000
        self.prev_time = current_time

        current_count = self.tim.counter()
        self.delta = current_count - self.prev_count
        self.prev_count = current_count

        # Handle overflow/underflow
        if self.delta >= 32768:  # AR+1/2 
            self.delta -= 65536
        elif self.delta <= -32768:
            self.delta += 65536

        self.position += self.delta

    def get_position(self):
        '''Returns the most recently updated value of position as determined 
        within the update() method'''
        self.theta = (self.position/1440*2*math.pi)
        return float(self.theta)
        
    def get_velocity(self):
        '''Returns a smoothed measure of velocity using a moving average of 
        the most recent updates'''
        velocity = (self.delta / 1440) * (2 * math.pi) / self.dt 
        self.velocity_buffer.append(velocity)

        # Keep only the last `window_size` samples
        if len(self.velocity_buffer) > self.window_size:
            self.velocity_buffer.pop(0)

        # Return the average velocity
        return sum(self.velocity_buffer) / len(self.velocity_buffer)

    def zero(self):
        '''Sets the present encoder position to zero and causes future cupdates 
        to measure with respect to the new zero position'''
        self.position = 0
        self.delta = 0
        self.prev_count = 0