def interpolateStep(self):
     if not self.interpolating:
         return
     complete = self.servoCount
     while (pyb.millis() - self.lastFrame < BIOLOID_FRAME_LENGTH):
         pass
     self.lastFrame = pyb.millis()
     for i in range(self.servoCount):
         diff = self.nextPose[i] - self.pose[i]
         if diff == 0:
             complete -= 1
         else:
             if diff > 0:
                 if diff < self.speed[i]:
                     self.pose[i] = self.nextPose[i]
                     complete -= 1
                 else:
                     self.pose[i] += self.speed[i]
             else:
                 if (-diff) < self.speed[i]:
                     self.pose[i] = self.nextPose[i]
                     complete -= 1
                 else:
                     self.pose[i] -= self.speed[i]
     if complete <= 0:
         self.interpolating = False
     self.writePose()

    def makegauge(self):
        '''
        Generator refreshing the raw measurments.
        '''
        delays = (5, 8, 14, 25)
        while True:
            self._bmp_i2c.mem_write(0x2E, self._bmp_addr, 0xF4)
            t_start = pyb.millis()
            while pyb.elapsed_millis(t_start) <= 5: # 5mS delay
                yield None
            try:
                self.UT_raw = self._bmp_i2c.mem_read(2, self._bmp_addr, 0xF6)
            except:
                yield None

            self._bmp_i2c.mem_write((0x34+(self.oversample_setting << 6)),
                                    self._bmp_addr,
                                    0xF4)

            t_pressure_ready = delays[self.oversample_setting]
            t_start = pyb.millis()
            while pyb.elapsed_millis(t_start) <= t_pressure_ready:
                yield None
            try:
                self.MSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF6)
                self.LSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF7)
                self.XLSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF8)
            except:
                yield None
            yield True

    def pb(self,who,unused=None,unusedA=None):
        if ((pyb.millis()  - self.pbTime) < State.pbBounceDelay):
            self.pbTime = pyb.millis()
            State.printT('PB BOUNCE!! delta millis:\t' +  str(pyb.millis()  - self.pbTime))
            return False
        State.printT('PB delta millis:\t' +  str(pyb.millis()  - self.pbTime))
        self.pbTime = pyb.millis()
        whoFuncs = (
            # this either steps the seq or toggles splitpot tracking if not sequencing
            (self.pb0Func,),  # pb 0 # either step sequence or toggle tracking if not sequencing
            # this is the one saves the preset,      
            (self.turnOnYellowLed, self.saveCurrentConfAsPreset),          # pb 1
            # Tremolo
            (self.toggleTrem,),                       # pb 2
            # Vibrato
            (self.toggleVib,),                        # pb 3
            (self.lcdMgr.onLeftButton,),              # pb 4
            (self.lcdMgr.onRightButton,))             # pb 5

        State.printT('PB:\t' + str(who))  
        res = False         
        for f in whoFuncs[who]:
            res = f() or res
        State.printT('pb returning: ' +str(res))
        return res # True if who in [2,3] else False

    def send(self, buf, timeout=500):
        # power up
        self.reg_write(CONFIG, (self.reg_read(CONFIG) | PWR_UP) & ~PRIM_RX)
        pyb.udelay(150)

        # send the data
        self.cs.low()
        self.spi.send(W_TX_PAYLOAD)
        self.spi.send(buf)
        if len(buf) < self.payload_size:
            self.spi.send(b'\x00' * (self.payload_size - len(buf))) # pad out data
        self.cs.high()

        # enable the chip so it can send the data
        self.ce.high()
        pyb.udelay(15) # needs to be >10us
        self.ce.low()

        # blocking wait for tx complete
        start = pyb.millis()
        while pyb.millis() - start < timeout:
            status = self.reg_read_ret_status(OBSERVE_TX)
            if status & (TX_DS | MAX_RT):
                break

        # get and clear all status flags
        status = self.reg_write(STATUS, RX_DR | TX_DS | MAX_RT)
        if not (status & TX_DS):
            raise OSError("send failed")

        # power down
        self.reg_write(CONFIG, self.reg_read(CONFIG) & ~PWR_UP)

 def enterObstacleAvoidanceScanState(self):
     # this function shuts down the FSM for a second or two
     self.log('Entering ObstacleAvoidanceScanState')
     self.ikEngine.travelX = 0
     self.ikEngine.travelRotZ = 0
     self.ikEngine.setupForWalk()
     self.ikEngine.bodyPosX = -50  # move the body forwards so the head clears the legs
     self.ikEngine.setupForWalk()
     self.ikEngine.bodyPosX = 0  # it will move back the next time the IK engine runs
     blue = pyb.LED(BLUE_LED)
     blue.on()
     openAngle = self.mapObstacleSpace()
     blue.off()
     if openAngle is None:
         # we didn't find any open areas, so switch to walking mode which will re-trigger obstacle mode again
         self.log("No openings found from scan")
         self.obstacleScanTurnTime = pyb.millis()
     else:
         # The IK Engine uses radians/s for rotation rate, so figure out the delta in radians and thus given a fixed
         # rotation rate figure out how long we need to turn in order to end up pointing in that direction
         openAngleRadians = math.radians(openAngle)
         self.obstacleScanTurnTime = pyb.millis() + int((abs(openAngleRadians) / FRONT_OBSTACLE_TURN_SPEED) * 1000)
         if openAngle > 0:
             self.log("Found opening at angle %d - turning left" % openAngle)
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log("Found opening at angle %d - turning right" % openAngle)
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED

        def master():
            fut = Future()
            self.assertEqual((yield from x(10, 'happy')), 'happy')
            self.assertEqual((yield from w(x(10, 'pleased'), fut)), 'pleased')
            self.assertEqual(fut.result(), 'pleased')
            #logging.basicConfig(level=logging.DEBUG)
            #log.debug("Now %f", self.loop.time())

            # One that completes before the timeout
            t0 = pyb.millis()
            fut = Future()
            self.loop.call_soon(w(x(20, 'good'), fut))
            v = yield from wait_for(fut, 50)
            self.assertEqual(v, 'good')
            et = pyb.elapsed_millis(t0)
            self.assertTrue(20 <= et < 25,
                            'et was %rms (expected 20-25ms)' % et)

            # One that hits the timeout
            t0 = pyb.millis()
            fut = Future()
            self.loop.call_soon(w(x(20, 'fine'), fut))
            with self.assertRaises(TimeoutError):
                v = yield from wait_for(fut, 10)
            et = pyb.elapsed_millis(t0)
            self.assertTrue(10 <= et < 15,
                            'et was %rms (expected 10-15ms)' % et)

 def update(self):
     if pyb.millis() - self.lastDebounceTime > self.debounceDelay:
         reading = self.pin.value()
         if reading != self.lastReading:
             # we got a new value
             self.lastDebounceTime = pyb.millis()
             self.lastReading = reading
             if reading == self.onState and self.doOn:
                 self.doOn()

def tone(f,t,b=0):
    global bz
    p = int(1000000/f)
    t1 = pyb.millis()+t
    while pyb.millis() < t1:
        bz.high()
        pyb.udelay(p)
        bz.low()
        pyb.udelay(p)
    if b!=0: pyb.delay(b)

def stableValue(pin,stabilityPeriod=20):
    res = pin.value()
    if res:
        return res
    readTime = millis()
    while(millis()-readTime < stabilityPeriod):
        if res != pin.value():
            res = pin.value()
            readTime = millis()
        delay(1)
    return res

 def update(self):
     ok,v = self.readA()
     if ok:  # read went ok
         curZone = self.detectionZone(v - self.baseVal)
         if (curZone): # we have passed a threshold,
             if curZone != self.prevZone: # changed zones!
                 self.tf() #tfunc must be not None
                 self.lastActionTime = millis()
                 self.prevZone = curZone
         if millis() - self.lastActionTime > self.timeOut:
             self.of() # oFunc must be not None
             self.lastActionTime = millis()

    def _get_B5(self):
        '''
        Calculates and sets compensation value B5.
        '''
        if (self._B5 is None) or ((pyb.millis()-self._t_B5) > self._dt_B5):
            self.get_temperature()
            X1 = (self._UT-self._AC6)*self._AC5/2**15
            X2 = self._MC*2**11/(X1+self._MD)
            self._B5 = X1+X2
            self._t_B5 = pyb.millis()

        return

def FLASH_WaitForLastOperation(timeout):
    tickstart = pyb.millis()
    while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_BSY) != RESET):
        if (timeout != HAL_MAX_DELAY):
            if (timeout == 0 or (pyb.millis() - tickstart) > timeout):
                return False
    if __HAL_FLASH_GET_FLAG(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |
                            FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR |
                            FLASH_FLAG_PGSERR | FLASH_FLAG_RDERR) != RESET:
        # FLASH_SetErrorCode()
        return False
    return True

  def run( self ) :
    self._display.fill(0)
    sw = pyb.Switch()
    lasttime = pyb.millis()
    while sw() == False :
      pyb.delay(DISPLAY_DELAY)
      distance = self._getdistance()

      thistime = pyb.millis()
      t = thistime - lasttime
      self.printdistance(distance)
      self._rangepoint.update(self._display, distance, t / 1000.0)
      lasttime = thistime

def master():
    nrf = NRF24L01(SPI(2), Pin('Y5'), Pin('Y4'), payload_size=8)

    nrf.open_tx_pipe(pipes[0])
    nrf.open_rx_pipe(1, pipes[1])
    nrf.start_listening()

    num_needed = 16
    num_successes = 0
    num_failures = 0
    led_state = 0

    print('NRF24L01 master mode, sending %d packets...' % num_needed)

    while num_successes < num_needed and num_failures < num_needed:
        # stop listening and send packet
        nrf.stop_listening()
        millis = pyb.millis()
        led_state = max(1, (led_state << 1) & 0x0f)
        print('sending:', millis, led_state)
        try:
            nrf.send(struct.pack('ii', millis, led_state))
        except OSError:
            pass

        # start listening again
        nrf.start_listening()

        # wait for response, with 250ms timeout
        start_time = pyb.millis()
        timeout = False
        while not nrf.any() and not timeout:
            if pyb.elapsed_millis(start_time) > 250:
                timeout = True

        if timeout:
            print('failed, respones timed out')
            num_failures += 1

        else:
            # recv packet
            got_millis, = struct.unpack('i', nrf.recv())

            # print response and round-trip delay
            print('got response:', got_millis, '(delay', pyb.millis() - got_millis, 'ms)')
            num_successes += 1

        # delay then loop
        pyb.delay(250)

    print('master finished sending; succeses=%d, failures=%d' % (num_successes, num_failures))

 def update(self):
     if self.autoOff:  # we look for timeout, otherwise keep going
         if millis() - self.lastActionTime > self.timeOut:
             self.of() # oFunc must be not None
             self.lastActionTime = millis()
             return  # we're done!
     ok,v = self.readA()
     if ok:  # read went ok
         curZone = self.detectionZone(v - self.baseVal)
         if (curZone): # we have passed a threshold,
             if curZone != self.prevZone: # changed zones!
                 self.tf() #tfunc must be not None
                 self.lastActionTime = millis()
                 self.prevZone = curZone

def setPWM(input):
    global trigcheck
    global trig_time

    for i,val in enumerate(input):
        out = int(val) * 10
        if out == 90: out = 100
        pin[i].pwm(out)

        # As its slow check for more triggers whilst writing pins
        if adc1.read() > 50 and (millis() - trig_time) >= 500:
            trigcheck = True
            print('blip')
            trig_time = millis()
            return

 def get_pid(self, error, scaler):
   tnow = millis()
   dt = tnow - self._last_t
   output = 0
   if self._last_t == 0 or dt > 1000:
     dt = 0
     self.reset_I()
   self._last_t = tnow
   delta_time = float(dt) / float(1000)
   output += error * self._kp
   if abs(self._kd) > 0 and dt > 0:
     if isnan(self._last_derivative):
       derivative = 0
       self._last_derivative = 0
     else:
       derivative = (error - self._last_error) / delta_time
     derivative = self._last_derivative + \
                  ((delta_time / (self._RC + delta_time)) * \
                   (derivative - self._last_derivative))
     self._last_error = error
     self._last_derivative = derivative
     output += self._kd * derivative
   output *= scaler
   if abs(self._ki) > 0 and dt > 0:
     self._integrator += (error * self._ki) * scaler * delta_time
     if self._integrator < -self._imax: self._integrator = -self._imax
     elif self._integrator > self._imax: self._integrator = self._imax
     output += self._integrator
   return output

 def new_fix_time(self):
     """Updates a high resolution counter with current time when fix is updated. Currently only triggered from
     GGA, GSA and RMC sentences"""
     try:
         self.fix_time = pyb.millis()
     except NameError:
         self.fix_time = time.time()

 def _frame_fixed_timed(self, fixed_value, stage_time):
     t_start = pyb.millis()
     t_elapsed = -1
     while t_elapsed < stage_time: 
         for line in range(LINES_PER_DISPLAY -1, -1, -1): 
             self._line_fixed(line, fixed_value, set_voltage_limit = False)
         t_elapsed = pyb.elapsed_millis(t_start)

 def enterObstacleAvoidanceState(self):
     self.log('Entering ObstacleAvoidanceState')
     self.ikEngine.travelX = 0
     self.turnTimeoutTime = pyb.millis() + FRONT_OBSTACLE_TURN_TIMEOUT
     if (self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE) & (
         self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE):
         # stuff on both sides, pick a direction at random to turn
         if pyb.rng() & 1 == 0:
             self.log('Obstacles on both sides, turning right')
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log('Obstacles on both sides, turning left')
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
     elif self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
         self.log('Obstacle on left side, turning right')
         self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
     elif self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
         self.log('Obstacle on right side, turning left')
         self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
     else:  # nothing on either side, so pick a side at random
         if pyb.rng() & 1 == 0:
             self.log('Only front obstacle, turning right')
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log('Only front obstacle, turning left')
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED