class UltraSonicMeter(object):
def __init__(self):
self.tmp = self.time = 0
self.cnt = 0
self.fr = 0
self.trig = Pin('X12', Pin.OUT_PP, Pin.PULL_NONE)
echoR = Pin('X1', Pin.IN, Pin.PULL_NONE)
echoF = Pin('X2', Pin.IN, Pin.PULL_NONE)
self.micros = pyb.Timer(5, prescaler=83, period=0x3fffffff)
self.timer = Timer(2, freq=1000)
self.timer.period(3600)
self.timer.prescaler(1375)
self.timer.callback(lambda e: self.run_trig())
extR = ExtInt(echoR, ExtInt.IRQ_RISING, Pin.PULL_NONE, self.start_count)
extF = ExtInt(echoF, ExtInt.IRQ_FALLING, Pin.PULL_NONE, self.read_dist)
def run_trig(self):
self.trig.high()
pyb.udelay(1)
self.trig.low()
def start_count(self, line):
self.micros.counter(0)
self.time = self.micros.counter()
self.timer.counter(0)
def read_dist(self, line):
end = self.micros.counter()
micros = end-self.time
distP1 = micros//5
distP2 = micros//6
distP3 = (distP1-distP2)//10*2
dist = distP2+distP3
if dist != 0:
self.cnt += 1
self.fr += dist
if self.cnt == 15:
tmp = self.tmp
dist = self.fr//self.cnt
if tmp != dist:
print(dist, 'mm')
self.tmp = dist
self.cnt = 0
self.fr = 0
class LedController():
def __init__(self):
self.red_led = pyb.LED(1)
self.green_led = pyb.LED(2)
self.blue_led = pyb.LED(3)
self.led_timer = Timer(LED_TIMER_ID, freq=50, callback=self.leds_on)
self.red_channel = self.led_timer.channel(1,
Timer.PWM,
callback=self.red_led_off,
pulse_width_percent=0)
self.green_channel = self.led_timer.channel(
2, Timer.PWM, callback=self.green_led_off, pulse_width_percent=0)
self.blue_channel = self.led_timer.channel(3,
Timer.PWM,
callback=self.blue_led_off,
pulse_width_percent=0)
def leds_on(self, timer):
self.red_led.on()
self.green_led.on()
self.blue_led.on()
def red_led_off(self, timer):
self.red_led.off()
def green_led_off(self, timer):
self.green_led.off()
def blue_led_off(self, timer):
self.blue_led.off()
def set_pw_colors(self, i, red_channel, green_channel, blue_channel):
red_on = max(0, math.sin(i) * RBG_MAX)
green_on = max(0, math.sin(i - DEG_120) * RBG_MAX)
blue_on = max(0, math.sin(i - DEG_240) * RBG_MAX)
debug("%f %f %f" % (red_on, blue_on, green_on))
self.red_channel.pulse_width_percent(red_on)
self.green_channel.pulse_width_percent(green_on)
self.blue_channel.pulse_width_percent(blue_on)
def pulse(self, duration_ms):
startTime = pyb.millis()
while (duration_ms < 0 or pyb.elapsed_millis(startTime) < duration_ms):
self.set_pw_colors(pyb.millis() / 1000, self.red_channel,
self.green_channel, self.blue_channel)
pyb.udelay(self.led_timer.period())
self.led_timer.deinit()
self.red_led_off(None)
self.green_led_off(None)
self.blue_led_off(None)
class LedController():
def __init__(self):
self.led_timer = Timer(LED_TIMER_ID, freq=50, callback=leds_on)
self.red_channel = self.led_timer.channel(1,
Timer.PWM,
callback=red_led_off,
pulse_width_percent=0)
self.green_channel = self.led_timer.channel(2,
Timer.PWM,
callback=green_led_off,
pulse_width_percent=0)
self.blue_channel = self.led_timer.channel(3,
Timer.PWM,
callback=blue_led_off,
pulse_width_percent=0)
def pulse(self):
startTime = pyb.millis()
while (pyb.elapsed_millis(startTime) < 12000):
set_pw_colors(pyb.millis() / 1000, self.red_channel,
self.green_channel, self.blue_channel)
pyb.udelay(self.led_timer.period())
# check basic functionality of the timer class
import pyb
from pyb import Timer
tim = Timer(4)
tim = Timer(4, prescaler=100, period=200)
print(tim.prescaler())
print(tim.period())
tim.prescaler(300)
print(tim.prescaler())
tim.period(400)
print(tim.period())
# Setting and printing frequency
tim = Timer(2, freq=100)
print(tim.freq())
tim.freq(0.001)
print("{:.3f}".format(tim.freq()))
class RomiMotor :
"""
We reuse the same timer for all instances of the class, so this is the callback
of the class, which calls the handlers in each instance.
"""
@classmethod
def class_rpm_handler(cls, tim) :
for h in cls.rpm_handlers :
h(tim)
# List of the rpm handlers of the instances, necessary because we share a single timer.
rpm_handlers = []
# The shared timer
rpmtimer = None
"""
Initialize a RomiMotor, connected either to the 'X' side or the 'Y' side of the Pyboard.
On either side (? is 'X' or 'Y'):
- PWM is on pin ?1
- DIR is on pin ?2
- SLEEP is on pin ?3
- ENCA is on pin ?4
- ENCB is on pin ?5
"""
def __init__(self, X=True) :
if X :
self.pwmpin = Pin('X6', Pin.OUT_PP)
self.pwmtim = Timer(2, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('X2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('X3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('X4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('X5', Pin.IN, Pin.PULL_UP)
else :
self.pwmpin = Pin('Y1', Pin.OUT_PP)
self.pwmtim = Timer(8, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('Y2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('Y3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP)
self.pwmscale = (self.pwmtim.period() + 1) // 10000 # scale factot for permyriad(10000 as it allows to get more distinct points and avoid divisions) power
self.count_a = 0 # counter for impulses on the A output of the encoder
self.target_a = 0 # target value for the A counter (for controlled rotation)
self.count_b = 0 # counter for impulses on the B output of the encoder
self.time_a = 0 # last time we got an impulse on the A output of the encoder
self.time_b = 0 # last time we got an impulse on the B output of the encoder
self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B
self.dirsensed = 0 # direction sensed through the phase of the A and B outputs
self.rpm = 0 # current speed in rotations per second
self.rpm_last_a = 0 # value of the A counter when we last computed the rpms
self.cruise_rpm = 0 # target value for the rpms
self.walking = False # Boolean that indicates if the robot is walking or stationary
self.desiredDir = True# Boolean that indecates the desired direction of the motor for move function
self._control = PID() # PID control for the rotation of the wheels
self._control.setTunings(KP, KI, KD);
self._control.setSampleTime(1);
self._control.setOutputLimits(-10000, 10000)
self._control_distance = PID() # PID control for the cascade of the distance
self._control_distance.setTunings(KP_DISTANCE, KI_DISTANCE, KD_DISTANCE);
self._control_distance.setSampleTime(1);
self._control_distance.setOutputLimits(-MAXIMUM_VELOCITY, MAXIMUM_VELOCITY)
ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler)
ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler)
if RomiMotor.rpmtimer is None : # create only one shared timer for all instances
RomiMotor.rpmtimer = Timer(4)
RomiMotor.rpmtimer.init(freq=100, callback=RomiMotor.class_rpm_handler)
RomiMotor.rpm_handlers.append(self.rpm_handler) # register the handler for this instance
"""
Handler for interrupts caused by impulses on the A output of the encoder.
This is where we sense the rotation direction and adjust the throttle to
reach a target number of rotations of the wheel.
"""
def enca_handler(self, pin) :
self.count_a += 1
if self.encb.value():
self.dirsensed = -1 # A occurs before B
else :
self.dirsensed = 1 # B occurs before A
if self.target_a > 0 : # If we have a target rotation
if self.count_a >= self.target_a :
self.cruise_rpm = 0
self.pwm.pulse_width(0) # If we reached of exceeded the rotation, stop the motor
self.target_a = 0 # remove the target
self.walking = False
else: # The cascade control for the distance
self._control_distance.setSetPoint(self.target_a)
self.cruise_rpm = self._control_distance.compute(self.count_a)
self.walking = True
"""
Handler for interrupts caused by impulses on the B output of the encoder.
"""
def encb_handler(self, pin) :
self.count_b += 1
"""
This is the handler of the timer interrupts to compute the rpm
"""
def rpm_handler(self, tim) :
self.rpm = 100*(self.count_a - self.rpm_last_a) # The timer is at 100Hz
self.rpm_last_a = self.count_a # Memorize the number of impulses on A
if self.cruise_rpm != 0 : # If we have an rpm target
self._control.setSetPoint(self.cruise_rpm)
output = self._control.compute(self.rpm)
if output < 0 or self.desiredDir == False: # Corrects the control output for the desired direction
if output < 0:
output = -output
self.dir.off()
else :
self.dir.on()
self.pwm.pulse_width(output * self.pwmscale)
self.walking = True
self.sleep.on()
else:
self.walking = False
"""
Set the power of the motor in percents.
Positive values go forward, negative values go backward.
"""
def throttle(self, pct) :
if pct is None :
return
if pct < 0 :
self.dir.off()
pct = -pct
else :
self.dir.on()
self.pwm.pulse_width(100 * pct * self.pwmscale)
self.walking = True
self.sleep.on()
"""
Get the current power as a percentage of the max power.
The result is positive if the motor runs forward, negative if it runs backward.
"""
def getThrottle(self) :
thr = self.pwm.pulse_width() // self.pwmscale
if self.dir.value() > 0 :
thr = -thr
return thr
"""
Release the motor to let it rotate freely.
"""
def release(self, release=True) :
if release :
self.sleep.off()
else :
self.sleep.on()
"""
Perform 'tics' 1/360 rotation of the wheel at 'power' percents of the max power.
If 'turns' is positive, the wheel turns forward, if it is negative, it turns backward.
"""
def rotatewheel(self, tics, power=20):
if tics < 0 :
self.desiredDir = False
tics = -tics
else :
self.desiredDir = True
self.count_a = 0
self.count_b = 0
self._control_distance.setOutputLimits(-power*100, power*100)
self.target_a = int(tics)
"""
Wait for the rotations requested by 'rotatewheel' to be done.
"""
def wait(self) :
while self.target_a !=0 :
pass
"""
Set a target rpms. The wheel turns in its current rotation direction,
'rpm' should be non negative.
"""
def cruise(self, rpm) :
if rpm < 0 :
self.desiredDir = False
rpm = -rpm
else :
self.desiredDir = True
self.cruise_rpm = int(rpm * 6)
"""
Get the current rpms. This is always non negative, regardless of the rotation direction.
"""
def get_rpms(self) :
return self.rpm / 60
"""
Cancel all targets of rotation and rpm
"""
def clear(self) :
self.target_a = 0
self.cruise_rpm = 0
self.desiredDir = True
self.rpm = 0
self.rpm_last_a = 0
self.count_a = 0
self.count_b = 0
self.pwm.pulse_width(0)
"""
Stop the motor.
"""
def stop(self) :
self.clear()
self.throttle(0)
# check basic functionality of the timer class import pyb from pyb import Timer tim = Timer(4) tim = Timer(4, prescaler=100, period=200) print(tim.prescaler()) print(tim.period()) tim.prescaler(300) print(tim.prescaler()) tim.period(400) print(tim.period())
class Encoder():
"""
Abstracts a quadrature encoder to give a tick count.
The count is a signed integer starting at zero. It wraps the count from the
attached timer to give a seamless and continuous tick count. Overflows of
the internal timer counter register should be adequatly handled. If
overflows or weird behaviour occurs around the overflow points, try
increasing Encoder.HYSTERESIS.
Note: Only works on pin pairs 'X1' & 'X2', 'X9' & 'X10', or 'Y1', 'Y2'. The
timer will be automatically selected. Both Timer 2 and 5 work for 'X1' &
'X2', but Timer 5 is preferred because Timer 2 is used for LED PWM but both
can be used by changing the values of Encoder.AF_MAP.
"""
# Constant for decoding in single line mode
SINGLE_MODE = Timer.ENC_A
# Constant for decoding in quad mode
DUAL_MODE = Timer.ENC_AB
# Maps alternate pin function descriptions to the required timer number
TIMER_MAP = {'AF1_TIM2': 2, 'AF2_TIM4': 4, 'AF2_TIM5': 5, 'AF3_TIM8': 8}
# Maps pin names to the alternate function to use
AF_MAP = {
'X1': 'AF2_TIM5',
'X2': 'AF2_TIM5',
'X9': 'AF2_TIM4',
'X10': 'AF2_TIM4',
'Y1': 'AF3_TIM8',
'Y2': 'AF3_TIM8'
}
# Defines the pin pairs that must be used
PIN_PAIRS = [['X1', 'X9', 'Y1'], ['X2', 'X10', 'Y2']]
# Hysteresis value to overflow detection
HYSTERESIS = 12 # One full rotation of encoder
def __init__(self, pinA, pinB, mode=DUAL_MODE):
"""
Instantiate an Encoder object.
Initalises the Pins, Timer and TimerChannel for use as a quadrature
decoder. Registers an overflow callback to elegantly handle counter
register overflows.
pinA: Any valid value taken by pyb.Pin constructor
pinB: Any valid value taken by pyb.Pin constructor
mode: Mode to use for decoding (Encoder.SINGLE_MODE or
Encoder.DUAL_MODE)
raises: Any exception thrown by pyb.Pin, pyb.Timer, pyb.Timer.channel
or Exception if pins are not compatible pairs
"""
self._chA = Pin(pinA)
self._chB = Pin(pinB)
self._ticks = 0
# Check pins are compatible
self._checkPins(pinA, pinB)
# init pins for alternate encoder function
af = self.AF_MAP[self._chA.names()[1]]
channel = self.TIMER_MAP[af]
af = getattr(Pin, af)
self._chA.init(Pin.AF_PP, pull=Pin.PULL_NONE, af=af)
self._chB.init(Pin.AF_PP, pull=Pin.PULL_NONE, af=af)
# init timer
self._timer = Timer(channel, prescaler=0, period=100000)
# init encoder mode
# self._channel = self._timer.channel(1, mode)
self._timer.channel(1, mode)
# setup overflow callback
self._timer.callback(self._overflow)
# init count register to middle of count
self._timer.counter(self._timer.period() // 2)
self._lastRead = self._timer.counter()
def _checkPins(self, pinA, pinB):
"""
Check that two pins can be used for a decoding and are on the same
timer.
"""
try:
if pinA in self.PIN_PAIRS[0]:
if self.PIN_PAIRS[0].index(pinA) != self.PIN_PAIRS[1].index(
pinB):
raise Exception()
elif pinA in self.PIN_PAIRS[1]:
if self.PIN_PAIRS[0].index(pinB) != self.PIN_PAIRS[1].index(
pinA):
raise Exception()
else:
raise Exception()
except:
raise Exception(pinA + ' & ' + pinB + ' are not on the same Timer')
def ticks(self, ticks=None):
"""
Get or set the current tick count.
Ticks is a signed integer.
"""
if ticks is not None: # set ticks to desired value
self._ticks = ticks
else: # retrieve latest count and update internals
count = self._timer.counter()
self._ticks = self._ticks + (count - self._lastRead)
self._lastRead = count
return self._ticks
def _overflow(self, timer):
"""
Timer overflow callback to gracefully handle overflow events. If
weird things are occurring, try increasing the HYSTERESIS value.
"""
count = timer.counter()
if 0 <= count <= self.HYSTERESIS: # overflow
self._ticks = self._ticks + (timer.period() - self._lastRead +
count)
self._lastRead = count
elif (timer.period() -
self.HYSTERESIS) <= count <= timer.period(): # underflow
self._ticks = self._ticks - (self._lastRead + timer.period() -
count)
self._lastRead = count
else: # hysteresis not big enough
#LED(1).on() # turn on inbuilt red led to show error
pass
class Encoder():
"""
Abstracts a quadrature encoder to give a tick count.
The count is a signed integer starting at zero. It wraps the count from the
attached timer to give a seamless and continuous tick count. Overflows of
the internal timer counter register should be adequatly handled. If
overflows or weird behaviour occurs around the overflow points, try
increasing Encoder.HYSTERESIS.
Note: Only works on pin pairs 'X1' & 'X2', 'X9' & 'X10', or 'Y1', 'Y2'. The
timer will be automatically selected. Both Timer 2 and 5 work for 'X1' &
'X2', but Timer 5 is preferred because Timer 2 is used for LED PWM but both
can be used by changing the values of Encoder.AF_MAP.
"""
# Constant for decoding in single line mode
SINGLE_MODE = Timer.ENC_A
# Constant for decoding in quad mode
DUAL_MODE = Timer.ENC_AB
# Maps alternate pin function descriptions to the required timer number
TIMER_MAP = {'AF1_TIM2': 2, 'AF2_TIM4': 4, 'AF2_TIM5': 5, 'AF3_TIM8': 8}
# Maps pin names to the alternate function to use
AF_MAP = {'X1' : 'AF2_TIM5', 'X2': 'AF2_TIM5', 'X9': 'AF2_TIM4',
'X10': 'AF2_TIM4', 'Y1': 'AF3_TIM8', 'Y2': 'AF3_TIM8'}
# Defines the pin pairs that must be used
PIN_PAIRS = [['X1','X9','Y1'],['X2','X10','Y2']]
# Hysteresis value to overflow detection
HYSTERESIS = 12 # One full rotation of encoder
def __init__(self, pinA, pinB, mode = DUAL_MODE):
"""
Instantiate an Encoder object.
Initalises the Pins, Timer and TimerChannel for use as a quadrature
decoder. Registers an overflow callback to elegantly handle counter
register overflows.
pinA: Any valid value taken by pyb.Pin constructor
pinB: Any valid value taken by pyb.Pin constructor
mode: Mode to use for decoding (Encoder.SINGLE_MODE or
Encoder.DUAL_MODE)
raises: Any exception thrown by pyb.Pin, pyb.Timer, pyb.Timer.channel
or Exception if pins are not compatible pairs
"""
self._chA = Pin(pinA)
self._chB = Pin(pinB)
self._ticks = 0
# Check pins are compatible
self._checkPins(pinA, pinB)
# init pins for alternate encoder function
af = self.AF_MAP[self._chA.names()[1]]
channel = self.TIMER_MAP[af]
af = getattr(Pin, af)
self._chA.init(Pin.AF_PP, pull = Pin.PULL_NONE, af = af)
self._chB.init(Pin.AF_PP, pull = Pin.PULL_NONE, af = af)
# init timer
self._timer = Timer(channel, prescaler = 0, period = 100000)
# init encoder mode
# self._channel = self._timer.channel(1, mode)
self._timer.channel(1, mode)
# setup overflow callback
self._timer.callback(self._overflow)
# init count register to middle of count
self._timer.counter(self._timer.period()//2)
self._lastRead = self._timer.counter()
def _checkPins(self, pinA, pinB):
"""
Check that two pins can be used for a decoding and are on the same
timer.
"""
try:
if pinA in self.PIN_PAIRS[0]:
if self.PIN_PAIRS[0].index(pinA) != self.PIN_PAIRS[1].index(pinB):
raise Exception()
elif pinA in self.PIN_PAIRS[1]:
if self.PIN_PAIRS[0].index(pinB) != self.PIN_PAIRS[1].index(pinA):
raise Exception()
else:
raise Exception()
except:
raise Exception(pinA + ' & ' + pinB + ' are not on the same Timer')
def ticks(self, ticks = None):
"""
Get or set the current tick count.
Ticks is a signed integer.
"""
if ticks is not None: # set ticks to desired value
self._ticks = ticks
else: # retrieve latest count and update internals
count = self._timer.counter()
self._ticks = self._ticks + (count - self._lastRead)
self._lastRead = count
return self._ticks
def _overflow(self, timer):
"""
Timer overflow callback to gracefully handle overflow events. If
weird things are occurring, try increasing the HYSTERESIS value.
"""
count = timer.counter()
if 0 <= count <= self.HYSTERESIS: # overflow
self._ticks = self._ticks + (timer.period() - self._lastRead + count)
self._lastRead = count
elif (timer.period() - self.HYSTERESIS) <= count <= timer.period(): # underflow
self._ticks = self._ticks - (self._lastRead + timer.period() - count)
self._lastRead = count
else: # hysteresis not big enough
#LED(1).on() # turn on inbuilt red led to show error
pass
class uPyBot:
"""This class represent uPyBot robot"""
#define various useful constants used by uPyBot programs
LOW_SPEED = const(85)
MID_SPEED = const(95)
HIGH_SPEED = const(100)
TURN_90 = const(6)
TURN_180 = const(17)
TURN_360 = const(34)
WHEEL_TURN_90 = const(17)
WHEEL_TURN_180 = const(35)
WHEEL_TURN_360 = const(70)
STEP_BACK_DISTANCE = const(7)
LOW_DISTANCE = const(10)
STALL_CHECK_TIME_MS = const(100)
MOVE_OK = const(0)
MOVE_OBSTACLE = const(1)
MOVE_STALL = const(2)
MOVE_MIN = const(10)
MOVE_STEP = const(10)
MOVE_MAX = const(100)
SYMMETRIC_TURN = const(1)
ASYMMETRIC_TURN = const(2)
LEFT_TURN = const(1)
RIGHT_TURN = const(2)
def __init__(self):
self._tmr2 = Timer(2, freq=20000,
mode=Timer.CENTER) #used for PWM for motor control
self._ch1 = self._tmr2.channel(1,
Timer.PWM,
pin=Pin.board.X1,
pulse_width=(self._tmr2.period() + 1) //
2)
self._ch4 = self._tmr2.channel(4,
Timer.PWM,
pin=Pin.board.X4,
pulse_width=(self._tmr2.period() + 1) //
2)
self._us_tmr = Timer(
3, prescaler=83,
period=0x3fffffff) #used to measure US sensor pulse duration
self._pin_in1 = Pin(Pin.board.X3, Pin.OUT_PP) #motor control pins
self._pin_in2 = Pin(Pin.board.X2, Pin.OUT_PP)
self._pin_in3 = Pin(Pin.board.X5, Pin.OUT_PP)
self._pin_in4 = Pin(Pin.board.X6, Pin.OUT_PP)
self._pin_int7 = Pin(Pin.board.X7, Pin.IN) #encoder interrupt pins
self._pin_int8 = Pin(Pin.board.X8, Pin.IN)
self._pin_trigger = Pin(Pin.board.X11, Pin.OUT_PP) #US trig pin
self._pin_echo = Pin(Pin.board.X12, Pin.IN) #US echo pin
self._pin_beep = Pin(Pin.board.X19, Pin.OUT_PP) #beeper ctrl pin
self._beeper = Beeper(self._pin_beep)
self._left_motor = Motor(self._pin_in1, self._pin_in2, self._ch1)
self._left_encoder = Encoder(self, self._pin_int7)
self._right_motor = Motor(self._pin_in4, self._pin_in3, self._ch4)
self._right_encoder = Encoder(self, self._pin_int8)
self._us = UltraSonicSensor(self._pin_trigger, self._pin_echo,
self._us_tmr)
def beep(self, duration=Beeper.BEEP_DEFAULT_DURATION_MS):
self._beeper.beep(duration)
def stop(self):
self._left_motor.stop()
self._right_motor.stop()
self._left_encoder.force_stop_cnt()
self._right_encoder.force_stop_cnt()
def forward(
self, pulses, speed
): #move forward specified number of the encoder pulses with specified speed
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(speed)
self._right_motor.set_speed(speed)
self._left_motor.set_forward()
self._right_motor.set_forward()
def backward(
self, pulses, speed
): #move backward specified number of the encoder pulses with specified speed
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(speed)
self._right_motor.set_speed(speed)
self._left_motor.set_backward()
self._right_motor.set_backward()
def turn_left(self, pulses): #turn specified number of pulses
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(uPyBot.HIGH_SPEED)
self._right_motor.set_speed(uPyBot.HIGH_SPEED)
self._left_motor.set_backward()
self._right_motor.set_forward()
def turn_right(self, pulses):
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(uPyBot.HIGH_SPEED)
self._right_motor.set_speed(uPyBot.HIGH_SPEED)
self._left_motor.set_forward()
self._right_motor.set_backward()
def left_wheel_forward(self, pulses):
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(0)
self._left_motor.set_speed(uPyBot.HIGH_SPEED)
self._right_motor.set_speed(0)
self._left_motor.set_forward()
self._right_motor.stop()
def left_wheel_backward(self, pulses):
self._left_encoder.set_stop_pulses(pulses)
self._right_encoder.set_stop_pulses(0)
self._left_motor.set_speed(uPyBot.HIGH_SPEED)
self._right_motor.set_speed(0)
self._left_motor.set_backward()
self._right_motor.stop()
def right_wheel_forward(self, pulses):
self._left_encoder.set_stop_pulses(0)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(0)
self._right_motor.set_speed(uPyBot.HIGH_SPEED)
self._left_motor.stop()
self._right_motor.set_forward()
def right_wheel_backward(self, pulses):
self._left_encoder.set_stop_pulses(0)
self._right_encoder.set_stop_pulses(pulses)
self._left_motor.set_speed(0)
self._right_motor.set_speed(uPyBot.HIGH_SPEED)
self._left_motor.stop()
self._right_motor.set_backward()
def is_stopped(self):
return self._left_encoder.is_stopped(
) and self._right_encoder.is_stopped()
def step_back(self):
self.backward(uPyBot.STEP_BACK_DISTANCE, uPyBot.MID_SPEED)
def is_stall(
self
): #check if any encoder is not moving since last check what means motor stall if run
if self._left_encoder.is_moving():
if not self._left_encoder.is_changing():
return True
if self._right_encoder.is_moving():
if not self._right_encoder.is_changing():
return True
return False
def wait_until_move_done(
self
): #when movement is triggered and ongoing check for obstacle and motor stall
start = time.ticks_ms()
while not self.is_stopped():
if self._us.distance_to_obstacle_in_cm(
) < uPyBot.LOW_DISTANCE: #detect obstacle
self.stop()
return uPyBot.MOVE_OBSTACLE
if time.ticks_diff(
time.ticks_ms(), start
) > uPyBot.STALL_CHECK_TIME_MS: #detect motor blockage (encoder counted pulses during STALL_CHECK_TIME_MS should change)
start = time.ticks_ms()
if self.is_stall():
return uPyBot.MOVE_STALL
return uPyBot.MOVE_OK
def rnd_move(self): #make rundom distance move forward
distance = random.randint(uPyBot.MOVE_MIN, uPyBot.MOVE_MAX)
self.forward(distance, uPyBot.HIGH_SPEED)
result = self.wait_until_move_done()
if result != uPyBot.MOVE_OK:
self.step_back()
return result
def symmetric_turn(self, direction, angle):
if direction == uPyBot.LEFT_TURN:
self.turn_left(angle)
else:
self.turn_right(angle)
result = self.wait_until_move_done()
if result != uPyBot.MOVE_OK:
self.step_back()
def asymmetric_turn(self, direction, angle):
#turn around one of the wheels
if direction == uPyBot.LEFT_TURN:
self.right_wheel_forward(angle)
else:
self.left_wheel_forward(angle)
result = self.wait_until_move_done()
if result != uPyBot.MOVE_OK:
self.step_back()
def rnd_turn(
self, move_result
): #make rundom turn of random type but only if not an obstacle detected for which 90 deg turn is executed always
turn_type = random.choice(
[uPyBot.SYMMETRIC_TURN, uPyBot.ASYMMETRIC_TURN])
turn_direction = random.choice([uPyBot.LEFT_TURN, uPyBot.RIGHT_TURN])
turn_angle = random.choice([uPyBot.TURN_90, uPyBot.TURN_180])
wheel_turn_angle = random.choice(
[uPyBot.WHEEL_TURN_90, uPyBot.WHEEL_TURN_180])
if move_result != uPyBot.MOVE_OK:
self.symmetric_turn(turn_direction, uPyBot.TURN_90)
else:
if turn_type == uPyBot.SYMMETRIC_TURN:
self.symmetric_turn(turn_direction, turn_angle)
else:
self.asymmetric_turn(turn_direction, wheel_turn_angle)
class RomiMotor:
"""
We reuse the same timer for all instances of the class, so this is the callback
of the class, which calls the handlers in each instance.
"""
@classmethod
def class_rpm_handler(cls, tim):
for h in cls.rpm_handlers:
h(tim)
# List of the rpm handlers of the instances, necessary because we share a single timer.
rpm_handlers = []
# The shared timer
rpmtimer = None
"""
Initialize a RomiMotor, connected either to the 'X' side or the 'Y' side of the Pyboard.
On either side (? is 'X' or 'Y'):
- PWM is on pin ?1
- DIR is on pin ?2
- SLEEP is on pin ?3
- ENCA is on pin ?4
- ENCB is on pin ?5
"""
def __init__(self, X=True):
if X:
self.pwmpin = Pin('X1', Pin.OUT_PP)
self.pwmtim = Timer(2, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('X2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('X3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('X4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('X5', Pin.IN, Pin.PULL_UP)
else:
self.pwmpin = Pin('Y1', Pin.OUT_PP)
self.pwmtim = Timer(8, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('Y2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('Y3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP)
self.pwmscale = (self.pwmtim.period() +
1) // 100 # scale factor for percent power
self.count_a = 0 # counter for impulses on the A output of the encoder
self.target_a = 0 # target value for the A counter (for controlled rotation)
self.count_b = 0 # counter for impulses on the B output of the encoder
self.time_a = 0 # last time we got an impulse on the A output of the encoder
self.time_b = 0 # last time we got an impulse on the B output of the encoder
self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B
self.dirsensed = 0 # direction sensed through the phase of the A and B outputs
self.rpm = 0 # current speed in rotations per second
self.rpm_last_a = 0 # value of the A counter when we last computed the rpms
self.cruise_rpm = 0 # target value for the rpms
ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler)
ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler)
if RomiMotor.rpmtimer is None: # create only one shared timer for all instances
RomiMotor.rpmtimer = Timer(4)
RomiMotor.rpmtimer.init(freq=4,
callback=RomiMotor.class_rpm_handler)
RomiMotor.rpm_handlers.append(
self.rpm_handler) # register the handler for this instance
"""
Handler for interrupts caused by impulses on the A output of the encoder.
This is where we sense the rotation direction and adjust the throttle to
reach a target number of rotations of the wheel.
"""
def enca_handler(self, pin):
self.count_a += 1
self.time_a = pyb.millis()
if pyb.elapsed_millis(self.time_b) > self.elapsed_a_b:
self.dirsensed = -1 # A occurs before B
else:
self.dirsensed = 1 # B occurs before A
if self.target_a > 0: # If we have a target rotation
if self.count_a >= self.target_a:
self.pwm.pulse_width(
0
) # If we reached of exceeded the rotation, stop the motor
self.target_a = 0 # remove the target
elif (self.target_a - self.count_a) < 30:
self.pwm.pulse_width(
7 * self.pwmscale
) # If we are very close to the target, slow down a lot
elif (self.target_a - self.count_a) < 60:
self.pwm.pulse_width(
15 *
self.pwmscale) # If we are close to the target, slow down
"""
Handler for interrupts caused by impulses on the B output of the encoder.
"""
def encb_handler(self, pin):
self.count_b += 1
self.elapsed_a_b = pyb.elapsed_millis(
self.time_a) # Memorize the duration since the last A impulse
"""
This is the handler of the timer interrupts to compute the rpms
"""
def rpm_handler(self, tim):
self.rpm = 4 * (self.count_a - self.rpm_last_a) # The timer is at 4Hz
self.rpm_last_a = self.count_a # Memorize the number of impulses on A
if self.cruise_rpm != 0: # If we have an RPM target
# Add a correction to the PWM according to the difference in RPMs
delta = abs(self.rpm - self.cruise_rpm)
if delta < 100:
corr = delta // 40
elif delta < 500:
corr = delta // 20
else:
corr = delta // 10
if self.cruise_rpm < self.rpm:
self.pwm.pulse_width(
max(5 * self.pwmscale,
self.pwm.pulse_width() - self.pwmscale * corr))
else:
self.pwm.pulse_width(
min(100 * self.pwmscale,
self.pwm.pulse_width() + self.pwmscale * corr))
"""
Set the power of the motor in percents.
Positive values go forward, negative values go backward.
"""
def throttle(self, pct):
if pct is None:
return
if pct < 0:
self.dir.on()
pct = -pct
else:
self.dir.off()
self.pwm.pulse_width(pct * self.pwmscale)
self.sleep.on()
"""
Get the current power as a percentage of the max power.
The result is positive if the motor runs forward, negative if it runs backward.
"""
def getThrottle(self):
thr = self.pwm.pulse_width() // self.pwmscale
if self.dir.value() > 0:
thr = -thr
return thr
"""
Release the motor to let it rotate freely.
"""
def release(self, release=True):
if release:
self.sleep.off()
else:
self.sleep.on()
"""
Perform 'turns' rotations of the wheel at 'power' percents of the max power.
If 'turns' is positive, the wheel turns forward, if it is negative, it turns backward.
"""
def rotatewheel(self, turns, power=20):
if turns < 0:
sign = -1
turns = -turns
else:
sign = 1
self.count_a = 0
self.count_b = 0
self.target_a = int(360 * turns)
self.throttle(sign * power)
"""
Wait for the rotations requested by 'rotatewheel' to be done.
"""
def wait(self):
while self.count_a < self.target_a:
pass
"""
Set a target RPMs. The wheel turns in its current rotation direction,
'rpm' should be non negative.
"""
def cruise(self, rpm):
self.cruise_rpm = int(rpm * 60)
"""
Get the current RPMs. This is always non negative, regardless of the rotation direction.
"""
def get_rpms(self):
return self.rpm / 60
"""
Cancel all targets of rotation and RPM
"""
def clear(self):
self.target_a = 0
self.cruise_rpm = 0
"""
Stop the motor.
"""
def stop(self):
self.clear()
self.throttle(0)
class tv:
def __init__(self,hres=64,progressive=False,lines=None,linetime=64,buf=None):
if lines == None:
lines = 262 if progressive else 525
self.lines = lines
if buf == None:
if progressive:
self.buf = bytearray(262*hres)
else:
self.buf = bytearray(525*hres)
else:
self.buf = buf
self.hres = hres
self.line_time = linetime
self.progressive = progressive
self.sync_level = 0
self.blanking_level = 56
self.black_level = 58
self.white_level = 73
self.phase = 0
self.buffer_dac = True
self.reinit()
def redac(self):
self.dac = DAC(Pin('X5'),buffering=self.buffer_dac)
self.bmv = memoryview(self.buf)[:len(self)]
self.dac_tim = Timer(6, freq=int(self.hres*1000000/self.line_time))
self.dac.write_timed(self.bmv,self.dac_tim,mode=DAC.CIRCULAR)
self.frame_tim = Timer(5, prescaler=self.dac_tim.prescaler(),period=self.dac_tim.period()*len(self))
self.frame_tim.counter(self.phase)
def reinit(self):
self.calc_porch_magic_numbers()
self.init()
def set_progressive(self,prog=True):
self.progressive = prog
self.calc_porch_magic_numbers()
self.init()
def __len__(self):
return self.hres*self.lines
def calc_porch_magic_numbers(self):
br = self.hres/self.line_time
w = round(4.7*br)
t = int(10.9*br+0.9)#round mostly up
s = round(1.5*br)
self.h_blank = [s,s+w,t]
self.v_blank_e = [6,12,18]
self.v_blank_o = [6,12,19]
hsl = round(18*br)
hsh = round(58*br)
self.h_safe = [hsl-t,hsh-t]
self.v_safe = [32*(2-self.progressive),208*(2-self.progressive)]
def init(self):
self.carrier = Pin('X1')
self.tim = Timer(2, prescaler=1,period=1)
self.ch = self.tim.channel(1, Timer.PWM, pin=self.carrier)
self.ch.pulse_width(1)
self.redac()
self.be = self.bmv
if not self.progressive:
self.bo = self.be[len(self)//2:]
self.fb = framebuf.FrameBuffer(self.buf,self.hres,self.lines,framebuf.GS8)
self.fbe_mv = self.be[self.hres*21+self.h_blank[2]:]
if not self.progressive:
self.fbo_mv = self.bo[self.hres*43//2+self.h_blank[2]:]
h = self.y_dim()//(2-self.progressive)
self.fbe = framebuf.FrameBuffer(self.fbe_mv,self.hres-self.h_blank[2],h,framebuf.GS8,self.hres)
if not self.progressive:
self.fbo = framebuf.FrameBuffer(self.fbo_mv,self.hres-self.h_blank[2],h,framebuf.GS8,self.hres)
self.clear()
def set_pixel(self,x,y,v):
if self.progressive:
self.fbe.pixel(x,y,int(v*(self.white_level-self.black_level)+self.black_level))
else:
[self.fbe,self.fbo][y&1].pixel(x,y//2,int(v*(self.white_level-self.black_level)+self.black_level))
def get_pixel(self,x,y):
if self.progressive:
return (self.fbe_mv[x+y*self.hres]-self.black_level)/(self.white_level-self.black_level)
else:
return ([self.fbe_mv,self.fbo_mv][y&1][x+(y//2)*self.hres]-self.black_level)/(self.white_level-self.black_level)
def set_carrier(self,pre=1,per=1,w=1):
self.tim.init(prescaler=pre,period=per)
self.ch.pulse_width(w)
def clear(self):
self.fb.fill(self.black_level)
self.syncs()
def syncs(self):
self.fb.fill_rect(0,0,self.h_blank[2],self.lines,self.blanking_level)
self.fb.fill_rect(self.h_blank[0],0,self.h_blank[1]-self.h_blank[0],self.lines,self.sync_level)
for y in range(self.v_blank_e[2]):
inv = self.v_blank_e[0] <= y < self.v_blank_e[1]
for x in range(self.hres//2):
val = self.blanking_level
if (self.h_blank[0] <= x < self.h_blank[1]) ^ inv:
val = self.sync_level
self.buf[y*self.hres//2+x] = val
if not self.progressive:
for y in range(self.v_blank_o[2]):
inv = self.v_blank_o[0] <= y < self.v_blank_o[1]
for x in range(self.hres//2):
val = self.blanking_level
if (self.h_blank[0] <= x < self.h_blank[1]) ^ inv:
val = self.sync_level
self.bo[y*self.hres//2+x] = val
def x_dim(self):
return self.hres-self.h_blank[2]
def y_dim(self):
return self.lines-(21 if self.progressive else 43)
def mandelbrot(self,imax=8,p=0,s=2,julia=False,il=0,x0=None,y0=None,x1=None,y1=None,asm=True,julia_seed=0):
x0 = self.h_safe[0] if x0 == None else x0
x1 = self.h_safe[1] if x1 == None else x1
y0 = self.v_safe[0] if y0 == None else y0
y1 = self.v_safe[1] if y1 == None else y1
for x in range(x0,x1):
for y in range(y0,y1):
c = (((x-x0)/(x1-x0-1)-.5)*2 + ((y-y0)/(y1-y0-1)-.5)*2j)*s+p
z = c
if julia:
c = julia_seed
if asm:
i = a_mandelbrot(z,c,imax)
else:
for i in range(imax):
if z.real*z.real+z.imag*z.imag > 4:
break
z = z*z+c
else:
self.set_pixel(x,y,il)
continue
if i == -1:
self.set_pixel(x,y,il)
else:
self.set_pixel(x,y,i/imax)
def demo(self,x0=None,y0=None,x1=None,y1=None):
x0 = self.h_safe[0] if x0 == None else x0
x1 = self.h_safe[1] if x1 == None else x1
w = x1-x0
y0 = self.v_safe[0] if y0 == None else y0
y1 = self.v_safe[1] if y1 == None else y1
h = y1-y0
mx = x0
my = y0
import pyb
import time
acc = pyb.Accel()
btn = pyb.Switch()
p = self.get_pixel(int(mx),int(my))
pos = 0
zoom = 2
it = 16
julia = False
jp = 0
self.mandelbrot(it,pos,zoom,julia,0,x0,y0,x1,y1,julia_seed=jp)
def paddles(c):
x = int(mx-.125*w)
xw = w//4
y = int(my-.125*h)
yw = h//4
y_0 = y0
y_1 = y1
if not self.progressive:
y //= 2
yw //= 2
y_0 //= 2
y_1 //= 2
self.fbe.hline(x,y_0,xw,c)
self.fbe.vline(x0,y,yw,c)
self.fbe.hline(x,y_1,xw,c)
self.fbe.vline(x1,y,yw,c)
if not self.progressive:
self.fbo.hline(x,y_0,xw,c)
self.fbo.vline(x0,y,yw,c)
self.fbo.hline(x,y_1,xw,c)
self.fbo.vline(x1,y,yw,c)
while 1:
paddles(self.black_level)
mx = min(x1-2,max(x0,mx*.98+(-acc.x()/24+.5)*w*.02))
my = min(y1-2,max(y0,my*.98+(acc.y()/24+.5)*h*.02))
paddles(self.white_level)
p = self.get_pixel(int(mx),int(my))
self.set_pixel(int(mx),int(my),(p+.5)%1)
pyb.delay(10)
self.set_pixel(int(mx),int(my),p)
if btn():
st = time.ticks_ms()
nit = it*2
while btn():
if time.ticks_diff(time.ticks_ms(),st) > 1000:
if acc.z()>0:
nit = it*2
else:
nit = "Julia"
self.fbe.fill_rect(x0,y0,w,10,self.black_level)
if not self.progressive:
self.fbo.fill_rect(x0,y0,w,10,self.black_level)
self.fbe.text(str(nit),x0+1,y0+1,self.white_level)
if not self.progressive:
self.fbo.text(str(nit),x0+1,y0+1,self.white_level)
cp = (((mx-x0)/w-.5)*2+2j*((my-y0)/h-.5))*zoom
if time.ticks_diff(time.ticks_ms(),st) > 1000:
if nit == "Julia":
julia ^= 1
jp = pos + cp
pos = 0
zoom = 2
it = 16
else:
it = nit
else:
pos += cp
zoom *= .25
self.mandelbrot(it,pos,zoom,julia,0,x0,y0,x1,y1,julia_seed=jp)
class PWM:
"""A PWM output on the given pin.
This uses the pins_af.py file created by the micropython build.
The timer argument is the name of the timer to use, as a string in
the form 'TIM#'. If it is not set, the first timer available for
the pin will be used. Either length or freq can be used to specify
the pulse length of the pwm signal. Length is the total pulse
length in microseconds, and frequency is Hz, so length=20000
implies freq=50, which is the default. If both are specified, freq
is ignored.
self.timer & self.channel are made available if you want to read or
adjust the settings after initialization."""
def __init__(self, pin, timer=None, length=None, freq=None):
timers = af_map['P' + pin.name()]
if not timers:
raise PwmError("Pin does not support PWM.")
if not timer:
timer = 'TIM'
for af, name in timers:
if name.startswith(timer):
timer_af, timer_name = af, name
timer_full, channel = timer_name.split('_')
if channel.startswith('CH'):
break
else:
raise PwmError("Pin does not support timer %s" % timer)
if length:
freq = 1000000 / length
elif not freq:
freq = 50
pin.init(Pin.OUT, alt=af)
self.timer = Timer(int(timer_full[3:]), freq=freq)
self.channel = self.timer.channel(
int(channel[2:3]),
Timer.PWM_INVERTED if channel.endswith('N') else Timer.PWM,
pin=pin)
self.length = 1000000 / self.timer.freq()
self.duty(0)
def duty(self, percentage=None):
"""Get/Set the duty cycle as a percentage.
The duty cycle is the time the output signal is on.
Returns the last set value if called with no arguments."""
if percentage is None:
return round(100 * self.channel.pulse_width() /
self.timer.period())
self.channel.pulse_width_percent(max(0, min(percentage, 100)))
def pulse_width(self, width=None, time=0):
"""Get/Set the pulse width in microseconds.
The width is the length of time the signal is on. Returns the
current value rounded to the nearest int if called with no
arguments.
Time is the number of milliseconds to take to get to the new
width. At least that many milliseconds will pass; if it's 0,
the change will happen now."""
if width is None:
return round(self.length * self.channel.pulse_width() /
self.timer.period())
target = self.timer.period() * min(width, self.length) / self.length
self.target = round(target)
if time == 0:
self.channel.pulse_width(self.target)
else:
self.delta = int((target - self.channel.pulse_width()) *
self.length / (time * 1000))
self.channel.callback(self.timed_change)
# No initial change so we get minimum length.
def timed_change(self, timer):
"""Make a timed change in width."""
old = self.channel.pulse_width()
if abs(old - self.target) > abs(self.delta):
self.channel.pulse_width(old + self.delta)
else:
self.channel.callback(None)
self.channel.pulse_width(self.target)
