"""Module *metered_motors*
Contains the `MeteredMotors` class which adds encoder feedback to the
original :py:class:`botcore.Motors` class. This is a building block towards being able
to tell how fast you are going, or how far you have gone.
"""
import botcore
#import devbotcore as botcore
from pyb import Timer, Pin, disable_irq, enable_irq
[docs]class MeteredMotors:
"""A wrapper class for the existing Motor class that adds support
for making use of the two optical encoders too. Notice that it
mimics the API of the :py:class:`botcore.Motors` class, and then extends it.
"""
[docs] def left_encoder_handler(self, timer_is_unused):
"""Interrupt handler for the left encoder wheel. You don't call this."""
self.tick[botcore.LEFT] += self.dir[botcore.LEFT]
[docs] def right_encoder_handler(self, timer_is_unused):
"""Interrupt handler for the right encoder wheel. You don't call this."""
self.tick[botcore.RIGHT] += self.dir[botcore.RIGHT]
# You may be wondering why this class needs access to the LEDs
# The reason is that motor movement is detected using an optical
# system, and we have to "turn on the lights" (turn on the emitter LEDs)
def __init__(self, motors, leds):
self.motors = motors
self.leds = leds
self.enc_mask = 0x00
self.tick = [0,0]
# Design decision: I am keeping track of motor direction, and counting
# ticks UP or DOWN as appropriate (I was thinking of back-and-forth motion)
self.dir = [0,0]
# For future compatibility/swapout with TimedEncoders, we mimic it's hardware setup
# Encoders capture Timer5 counts. Run at 1MHz, with max ARR value this function seems to handle.
tim5 = Timer(5, prescaler=79, period=0x3FFFFFFF)
# Set up to capture optical pulses from the left encoder
enc_l_pin = Pin(Pin.cpu.A1, Pin.IN, af=Pin.AF2_TIM5)
enc_l_ch = tim5.channel(2, mode=Timer.IC, pin=enc_l_pin, polarity=Timer.RISING)
enc_l_ch.callback(self.left_encoder_handler)
# Set up to capture optical pulses from the right encoder
enc_r_pin = Pin(Pin.cpu.A2, Pin.IN, af=Pin.AF2_TIM5)
enc_r_ch = tim5.channel(3, mode=Timer.IC, pin=enc_r_pin, polarity=Timer.RISING)
enc_r_ch.callback(self.right_encoder_handler)
enable_irq(True)
[docs] def enable(self, do_enable):
"""Enabling the motors allows them to move, disabling them stops them from spinning.
Args:
do_enable (bool): true to enable, false to disable
"""
self.motors.enable(do_enable)
[docs] def run(self, num, pwr):
"""Run the specified motor at the specified power.
Args:
num (int): which motor (0 = LEFT, 1 = RIGHT)
pwr (int): power level as a percentage, -100 to 0 to +100
Power levels are negative for reverse (clockwise) motion.
Power levels are positive for forward (counter-clockwise) motion.
A power level of 0 means stop.
"""
if pwr != 0:
self.enc_mask |= (1 << num)
else:
self.enc_mask &= ~(1 << num)
if pwr > 0:
self.dir[num] = 1
elif pwr < 0:
self.dir[num] = -1
else:
self.dir[num] = 0
self.leds.enc_emit(self.enc_mask)
self.motors.run(num, pwr)
#
# Above routines were in Motor class too. Below are the extensions.
#
[docs] def get_ticks(self):
"""Get the current tick counts as a (left, right) tuple.
Does not reset the tick counters.
"""
irq_state = disable_irq()
ticks = (self.tick[botcore.LEFT], self.tick[botcore.RIGHT])
enable_irq(irq_state)
return ticks
[docs] def get_and_reset_ticks(self):
"""Get the current tick counts as a (left, right) tuple, and restart the counters."""
irq_state = disable_irq()
ticks = (self.tick[botcore.LEFT], self.tick[botcore.RIGHT])
self.tick = [0,0]
enable_irq(irq_state)
#print(ticks)
return ticks