class Encoder: """This class control WSR-08834 encoder. One of the pyboard pin is used to count pulses (via an interrupt)""" def __init__(self, robot, int_pin): self._robot = robot #reference to robot an encoder belongs into (is used to stop its motors) self._int_pin = int_pin #pyboard pin used to count encoder pulses self._cnt_pulses = 0 #counts pulses from encoder self._stop_pulses = 0 #keeps destination number of pulses to stop motor when reached self._last_pulses = 0 #number of counted pulses during last check self._ext_int = ExtInt( int_pin, ExtInt.IRQ_RISING, Pin.PULL_NONE, self.__int_handler) #setup pin to trigger interrupt def __int_handler(self, line): #encoder interrupt handler self._cnt_pulses += 1 if self._cnt_pulses >= self._stop_pulses: #when destination number of pulses reached stop robot's motor self._ext_int.disable() self._robot.stop() def set_stop_pulses( self, stop_pulses): #setup destination number of pulses for the move self._cnt_pulses = 0 self._last_pulses = 0 self._stop_pulses = stop_pulses self._ext_int.enable() def get_stop_pulses(self): return self._stop_pulses def get_current_pulses(self): return self._cnt_pulses def force_stop_cnt(self): # change counters so movement end is forced to be reported by is_stopped method self._cnt_pulses = self._stop_pulses def reset(self): self._cnt_pulses = 0 self._stop_pulses = 0 self._last_pulses = 0 def is_stopped( self ): #check if movement ended i.e. if number of desired pulses reached if self._cnt_pulses >= self._stop_pulses: return True else: return False def is_moving(self): #check if uPyBot moving return not self.is_stopped() def is_changing(self): #check if number of pulses changed since last check changing = False if self._cnt_pulses != self._last_pulses: changing = True self._last_pulses = self._cnt_pulses return changing
class RotaryIRQ(Rotary): def __init__(self, pin_num_clk, pin_num_dt, min_val=0, max_val=10, reverse=False, range_mode=Rotary.RANGE_UNBOUNDED, pull_up=False, half_step=False, invert=False): super().__init__(min_val, max_val, reverse, range_mode, half_step, invert) if pull_up == True: self._pin_clk = Pin(pin_num_clk, Pin.IN, Pin.PULL_UP) self._pin_dt = Pin(pin_num_dt, Pin.IN, Pin.PULL_UP) else: self._pin_clk = Pin(pin_num_clk, Pin.IN) self._pin_dt = Pin(pin_num_dt, Pin.IN) self._pin_clk_irq = ExtInt(pin_num_clk, ExtInt.IRQ_RISING_FALLING, Pin.PULL_NONE, self._process_rotary_pins) self._pin_dt_irq = ExtInt(pin_num_dt, ExtInt.IRQ_RISING_FALLING, Pin.PULL_NONE, self._process_rotary_pins) # turn on 3.3V output to power the rotary encoder (pyboard D only) if 'PYBD' in os.uname().machine: Pin('EN_3V3').value(1) def _enable_clk_irq(self): self._pin_clk_irq.enable() def _enable_dt_irq(self): self._pin_dt_irq.enable() def _disable_clk_irq(self): self._pin_clk_irq.disable() def _disable_dt_irq(self): self._pin_dt_irq.disable() def _hal_get_clk_value(self): return self._pin_clk.value() def _hal_get_dt_value(self): return self._pin_dt.value() def _hal_enable_irq(self): self._enable_clk_irq() self._enable_dt_irq() def _hal_disable_irq(self): self._disable_clk_irq() self._disable_dt_irq() def _hal_close(self): self._hal_disable_irq()
class InterruptHandler(): def __init__(self, pin_interrupt, callback=None, **arguments): self._enabled = False self._calling_callback = False self.pin = pin_interrupt self._callback = None self._callback_arguments = {} self._allocation_scheduled_callback = self.scheduled_callback self._interrupt = None self.interrupt_time = utime.ticks_ms() if callback: self.enable(callback, arguments) def enable(self, callback=None, trigger=ExtInt.IRQ_FALLING, initial_check=False, **arguments): if self._interrupt: if not self._calling_callback: self._interrupt.enable() elif callback: self._callback = callback if arguments: self._callback_arguments = arguments print("Creating interrupt on pin", self.pin, "on", trigger) self._interrupt = ExtInt( self.pin, trigger, Pin.PULL_NONE, self.interrupt_callback ) print() else: raise NotImplementedError("No callback defined") self._enabled = True def disable(self): self._enabled = False self._interrupt.disable() def time_since_interrupt(self): return utime.ticks_diff(utime.ticks_ms(), self.interrupt_time) def interrupt_callback(self, line=None): self.interrupt_time = utime.ticks_ms() if self._calling_callback: return if self._interrupt: self._interrupt.disable() self._calling_callback = True micropython.schedule(self._allocation_scheduled_callback, line) def scheduled_callback(self, line): self._callback_arguments["line"] = line self._callback(**self._callback_arguments) self._calling_callback = False if self._interrupt and self._enabled: self._interrupt.enable()
class Button(object): def __init__(self): self.extint = ExtInt(Pin('ON/OFF'), ExtInt.IRQ_FALLING, Pin.PULL_UP, self.__callback__) self._status = False def __callback__(self, line): #print('1, button __callback__', line) self.extint.disable() delay(500) self._status = False if self._status else True self.extint.enable() def reset(self): irq_state = disable_irq() self._status = False enable_irq(irq_state) def on(self): #print('switch status=', self._status) return True if self._status else False
async def _calibration_loop(self): # start RTC print("syncedclock_rtc: start rtc") self._rtc.init() # initalise gps self._gps = GPS(self._uart) ppsint = ExtInt(self._pps_pin, ExtInt.IRQ_RISING, Pin.PULL_NONE, self._pps) ppsint.disable() self._pps_event.clear() await asyncio.sleep(0) while True: print("syncedclock_rtc: initalise gps") await self._gps.set_auto_messages(['RMC'], 1) await self._gps.set_pps_mode(GPS.PPS_Mode.FIX, 42, GPS.PPS_Polarity.ACTIVE_HIGH, 0) print("syncedclock_rtc: waiting for gps lock (30s)") res = await asyncio.wait_for(self._wait_gpslock(), 30) if (res == False): continue print( "syncedclock_rtc: gps locked, start pps interrupt and wait for pps (3s)" ) #self._pps_pin.irq(trigger=Pin.IRQ_RISING, handler=self._pps) ppsint.enable() res = await asyncio.wait_for(self._wait_pps(), 3) if (res == False): print( "syncedclock_rtc: pps signal never recieved, bad wiring?") print("syncedclock_rtc: terminating") return # PPS signal leads GPS data by about half a second or so # so the GPS data contains the *previous* second at this point # add 1 second and reset RTC print("syncedclock_rtc: pps pulse recieved, set RTC clock") date = self._gps.date() time = self._gps.time() # helpfully utime and pyb.RTC use different order in the tuple now = utime.localtime( utime.mktime((date[2], date[1], date[0], time[0], time[1], time[2], 0, 0)) + 1) self._rtc.datetime( (now[0], now[1], now[2], 0, now[3], now[4], now[5], 0)) print("syncedclock_rtc: rtc clock now", self._rtc.datetime()) await asyncio.sleep(0) print("syncedclock_rtc: calibration loop started") # ensure we ignore the first cycle last_rtc_ss = -1 # count 32 seconds and calculate the error count = 0 tick_error = 0 while True: # each time we get an PPS event, work out the ticks difference res = await asyncio.wait_for(self._wait_pps(), 3) if (res == False): print("syncedclock_rtc: lost pps signal, restarting") self._locked = False #self._pps_pin.irq(handler=None) ppsint.disable() break rtc_ss = self._pps_rtc await asyncio.sleep(0) # first pass, just discard the value if (last_rtc_ss == -1): last_rtc_ss = rtc_ss continue await asyncio.sleep(0) count += 1 # compute the difference in ticks between this and the last error = (rtc_ss - last_rtc_ss) if (abs(error) > _RTC_MAX - 50): # probably actually rollover problem if (rtc_ss < last_rtc_ss): error = (rtc_ss + _RTC_MAX + 1 - last_rtc_ss) else: error = (rtc_ss - last_rtc_ss + _RTC_MAX + 1) await asyncio.sleep(0) #print(error) tick_error += (error) last_rtc_ss = rtc_ss # always use the last top of second as current offset if it's not a huge error # when locked if (self._locked and tick_error > -1 and tick_error < 1): self._ss_offset = rtc_ss await asyncio.sleep(0) if (count == 32): # if the tick error is +/-1 ticks, it's locked enough # we're only then about 3.81ppm but that's close enough if (self._locked == True and (tick_error > 1 or tick_error < -1)): print("syncedclock_rtc: lost lock") self._locked = False await asyncio.sleep(0) if (self._locked == False and (tick_error <= 1 and tick_error >= -1)): print("syncedclock_rtc: locked with", self._rtc.calibration()) # only cache top of second when we enter lock #self._ss_offset = (_RTC_MAX-rtc_ss) self._locked = True await asyncio.sleep(0) if (self._locked == True): # update reference clock point self._refclk = self._rtc.datetime() await asyncio.sleep(0) if (self._locked == False): print("syncedclock_rtc: error now", tick_error) # the total ticks missing should be applied to the calibration # we do this continously so we can ensure the clock is always remaining in sync await asyncio.sleep(0) try: self._rtc.calibration(self._rtc.calibration() + tick_error) except: print("syncedclock_rtc: error too large, ignoring") # allow us to to be interrupted now await asyncio.sleep(0) #print(rtc.calibration()) count = 0 tick_error = 0