def dist(self):
        start = 0
        end = 0

        # Send a 10us pulse.
        self.trigger.high()
        pyb.udelay(10)
        self.trigger.low()

        # Wait 'till whe pulse starts.
        start_tout = pyb.micros() + 1000

        while self.echo.value() == 0:
            start = pyb.micros() 
            if start > start_tout:
                print("start_tout")
                return -1

        # Wait 'till the pulse is gone.
        end_tout = pyb.micros() + 10000 

        while self.echo.value() == 1:
            end = pyb.micros() 
            if end > end_tout:
                print("end_tout")
                return -1

        # Calc the duration of the recieved pulse, divide the result by
        # 2 (round-trip) and divide it by 29 (the speed of sound is
        # 340 m/s and that is 29 us/cm).
        dist_in_cm = end - start

        return dist_in_cm

def balance():
    gangle = 0.0
    start = pyb.micros()
    controlspeed = 0
    fspeed = 0
    while abs(gangle) < 45:  # give up if inclination angle >=45 degrees
        angle  = imu.pitch()
        rate   = imu.get_gy()
        gangle = compf(gangle, angle, rate, pyb.elapsed_micros(start),0.99)         
        start = pyb.micros()
        # speed control
        actualspeed = (motor1.get_speed()+motor2.get_speed())/2
        fspeed = 0.95 * fspeed + 0.05 * actualspeed
        cmd = radio.poll() # cmd[0] is turn speed, cmd[1] is fwd/rev speed
        tangle = speedcontrol(800*cmd[1],fspeed)
         # stability control
        controlspeed += stability(tangle, gangle, rate)           
        controlspeed = constrain(controlspeed,-MAX_VEL,MAX_VEL)
        # set motor speed
        motor1.set_speed(-controlspeed-int(300*cmd[0]))
        motor2.set_speed(-controlspeed+int(300*cmd[0]))
        pyb.udelay(5000-pyb.elapsed_micros(start))
    # stop and turn off motors
    motor1.set_speed(0)
    motor2.set_speed(0)
    motor1.set_off()
    motor2.set_off()

def ppt():
    """ 
    Pyboard times:
    Time to wait for not t
    10
    Time to wait for f
    10
    """

    f =False
    t =True

    print('Time to wait for not t')
    u = pyb.micros()
    while not t: 
        None
    t = pyb.micros()-u
    print (t)

    print('Time to wait for f')
    u = pyb.micros()
    while f: 
        None
    t = pyb.micros()-u
    print (t)

def intCH4(line):
    global ch_up,ch_up_set,fresh
    if pin( ch_pin[3]  ):
        ch_up[3] = pyb.micros()
    else:
        ch_up[4] = pyb.micros()
        for i in range(5):
            ch_up_set[i] = ch_up[i]
        fresh = True

    def update_nomag(self, accel, gyro):    # 3-tuples (x, y, z) for accel, gyro
        ax, ay, az = accel                  # Units G (but later normalised)
        gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
        if self.start_time is None:
            self.start_time = pyb.micros()  # First run
        q1, q2, q3, q4 = (self.q[x] for x in range(4))   # short name local variable for readability
        # Auxiliary variables to avoid repeated arithmetic
        _2q1 = 2 * q1
        _2q2 = 2 * q2
        _2q3 = 2 * q3
        _2q4 = 2 * q4
        _4q1 = 4 * q1
        _4q2 = 4 * q2
        _4q3 = 4 * q3
        _8q2 = 8 * q2
        _8q3 = 8 * q3
        q1q1 = q1 * q1
        q2q2 = q2 * q2
        q3q3 = q3 * q3
        q4q4 = q4 * q4

        # Normalise accelerometer measurement
        norm = sqrt(ax * ax + ay * ay + az * az)
        if (norm == 0):
            return # handle NaN
        norm = 1 / norm        # use reciprocal for division
        ax *= norm
        ay *= norm
        az *= norm

        # Gradient decent algorithm corrective step
        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
        norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
        s1 *= norm
        s2 *= norm
        s3 *= norm
        s4 *= norm

        # Compute rate of change of quaternion
        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4

        # Integrate to yield quaternion
        deltat = pyb.elapsed_micros(self.start_time) / 1000000
        self.start_time = pyb.micros()
        q1 += qDot1 * deltat
        q2 += qDot2 * deltat
        q3 += qDot3 * deltat
        q4 += qDot4 * deltat
        norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)    # normalise quaternion
        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm

def erase_chip():
    cs.low()
    spi.send(CMD_WRITE_ENABLE)
    cs.high()
    cs.low()
    spi.send(CMD_ERASE_CHIP)
    cs.high()
    t = pyb.micros()
    wait()
    t = pyb.micros() - t
    print ("erase chip",t)

 def getTempHum(self, buzz=True):
     id_bit = 0 # identificador del bit que estamos tratando
     vbit = 0  # valor de la palabra de 40 bits que se lee, incluye checksum
     self.oneWire.init(pyb.Pin.OUT_PP)
     self.oneWire.high()
     retardo=250
     if buzz:
         #suena buzzer
         self.ch2.pulse_width(12000)
         pyb.delay(80) #in msecs
         self.ch2.pulse_width(0)
         retardo=170
     #pull the pin high and wait 250 milliseconds (ya esta en high, esperamos 250ms de todos modos)
     pyb.delay(retardo)
     #Host pulls low 1.5 ms
     start = pyb.micros()
     self.oneWire.low()
     pyb.udelay(1500)
     #Host pulls up 30us
     self.oneWire.high()
     pyb.udelay(30)
     #Paso a INPUT
     self.oneWire.init(pyb.Pin.IN)
     #sensor pulls low 80us
     while self.oneWire.value() != 0:
         pass
     #sensor pulls up 80us
     while self.oneWire.value() != 1:
         pass
     #sensor pulls low 50us // start bit
     while self.oneWire.value() != 0:
         pass
     while(True):
         #bit starts
         while self.oneWire.value() != 1:
             pass
         start = pyb.micros()
         #bit ends
         while self.oneWire.value() != 0:
             pass
         if (pyb.micros()-start) > 50:
             vbit = (vbit << 1) | 0x1 
         else:
             vbit = (vbit << 1)
         id_bit = id_bit + 1
         if id_bit >= 40:
             check_rec = vbit & 0xff   # checksum recibido
             vbit = vbit >> 8  #aqui "vbit" contiene el valor medido (32 bits)
             check_cal = (vbit & 0xff) + ((vbit >> 8) & 0xff) + ((vbit >> 16) & 0xff) + ((vbit >> 24) & 0xff) # checksum calculado 
             if check_cal != check_rec:
                 self.frt.setAlarmBit(2)
             break
     return vbit

 def integrate_continuously(self, nap=10):
     #print("integrating continuously, napping %d" % nap)
     tscale = 1 / 1000000
     then = pyb.micros()
     #should_be_less_than = (nap + 30) * 1000
     while True:
         dt = pyb.elapsed_micros(then)
         #if dt >= should_be_less_than:
         #    print("integration dt was", dt)
         then = pyb.micros()
         self.integrate(dt * tscale)
         self.show_balls()
         yield from sleep(nap)

def erase(cmd,addr):
    cs.low()
    spi.send(CMD_WRITE_ENABLE)
    cs.high()
    cs.low()
    spi.send(cmds[cmd])
    spi.send(addr>>16)
    spi.send(addr>>8)
    spi.send(addr)
    cs.high()
    t = pyb.micros()
    wait()
    t = pyb.micros() - t
    print ("erase",cmd,t,'us')

def p(c='X8'):
    """
    time get a pin value
    13
    """
    p = pyb.Pin(c,pyb.Pin.OUT_PP,pull=pyb.Pin.PULL_NONE)
    p.low()
    print(p.value())

    print('time get a pin value')

    u = pyb.micros()
    p.value()
    t = pyb.micros()-u
    print (t)

	def tx_rx(self, tx, nr_chars):
		""" Send command to and receive respons from SA7S"""
		''' rx = uart.readall() # Receive respons TAKES 1.0 sec ALWAYS (after uart.any) TimeOut!!!! '''
		i = 0
		rx = ''
		# todo: do check if respons == same as sent: repeat otherwise
		self.uart.write(tx)  # Send to unit
		# print("uart.write: i, tx: ", i,  tx[:-1])
		while True:  # Typiskt 2–3 (search: 4) varv i loopen
			i += 1
			if self.uart.any():  # returns True if any characters wait
				dbg.high()
				strt = micros()
				rx = b''
				j = 0
				while True:  # Typically 10–20 (search: 12; M, R & W0144: 1) loops
					j += 1
					rxb = self.uart.read(nr_chars)  # uart.readln och uart.readall ger båda timeout (1s default)
					rx = rx + rxb
					if (len(rx) >= nr_chars) or (rxb == b'\r'):  # End of search returns \r
						break
				dbg.low()
				##print("uart.read: i, j, tx, rx, ∆time ", i, j, tx[:-1], rx, len(rx), elapsed_micros(strt) / 1e6, 's')
				delay(84)
				break
			else:
				delay(10)
		return rx

def thr_instrument(objSch, lstResult):
    yield                          # Don't measure initialisation phase (README.md)
    while True:
        start = pyb.micros()       # More typically we'd measure our own code
        yield                      # but here we're measuring yield delays
        lstResult[0] = max(lstResult[0], pyb.elapsed_micros(start))
        lstResult[1] += 1

    def read_status_packet(self):
        """Reads a status packet and returns it.

        Rasises a bioloid.bus.BusError if an error occurs.

        """
        pkt = packet.Packet()
        while True:
            start = pyb.micros()
            byte = self.serial_port.read_byte()
            if byte is None:
                raise BusError(packet.ErrorCode.TIMEOUT)
            err = pkt.process_byte(byte)
            if err != packet.ErrorCode.NOT_DONE:
                break
        if err != packet.ErrorCode.NONE:
            raise BusError(err)
        if self.show & Bus.SHOW_COMMANDS:
            log('Rcvd Status: {}'.format(packet.ErrorCode(err)))
        if self.show & Bus.SHOW_PACKETS:
            dump_mem(pkt.pkt_bytes, prefix='  R', show_ascii=True, log=log)
        err = pkt.error_code()
        if err != packet.ErrorCode.NONE:
            raise BusError(err)
        return pkt

def ni():
    print ('Native: how many instructions can we do in 100 us')
    count = 0
    u = pyb.micros()
    while pyb.elapsed_micros(u) < 100:
        count+=1
    print(count)

def testfunc(a):
    start = pyb.micros()
    while not a.mag_ready:
        pass
    dt = pyb.elapsed_micros(start)
    print("Wait time = {:5.2f}mS".format(dt / 1000))
    start = pyb.micros()
    xyz = a.mag.xyz
    dt = pyb.elapsed_micros(start)
    print("Time to get = {:5.2f}mS".format(dt / 1000))
    print("x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(xyz[0], xyz[1], xyz[2]))
    print("Mag status should be not ready (False): ", a.mag_ready)
    print(
        "Correction factors: x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(
            a.mag_correction[0], a.mag_correction[1], a.mag_correction[2]
        )
    )

def timing():                           # Check magnetometer call timings
    imu.mag_triggered = False           # May have been left True by above code
    start = pyb.micros()
    imu.get_mag_irq()
    t1 = pyb.elapsed_micros(start)
    start = pyb.micros()
    imu.get_mag_irq()
    t2 = pyb.elapsed_micros(start)
    pyb.delay(200)
    start = pyb.micros()
    imu.get_mag_irq()
    t3 = pyb.elapsed_micros(start)

    # 1st call initialises hardware
    # 2nd call tests it (not ready)
    # 3rd call tests ready (it will be after 200mS) and reads data
    print(t1, t2, t3) # 1st call takes 265uS second takes 175uS. 3rd takes 509uS

def  evaluate(total, lcd):
    lcd.light(False)
    yield
    starttime = pyb.micros()
    yield
    endtime = pyb.micros()
    t = total[0]
    lcd.light(True)
    us = endtime - starttime

    text = "c/s: %f " % (t/ 10)
    lcd.text(text, 0, 10, 1)
    lcd.show()

    log.info("Total micros in 10 second runtime: %f", us / 1000000)
    log.info("Total counts:  %d counts /sec: %f ", t, t / 10)
    yield asyncio.KillOs()

def timeItRunner():
    global start,co,avg,sm,nb
    sm=nb=0
    start=pyb.micros()
    co.value(1)
    while True:
        avg = 0 if not nb else sm/nb
        print('avg:\t' + str(avg))

def writerCirc():
    global handlerIndex,outData,tbInData,interruptCounter,period,start
    if outData.nbElts:
        tbInData.put(outData.get())
    else:
        handlerIndex ^=1
    period[interruptCounter] = pyb.elapsed_micros(start)
    interruptCounter+=1
    start=pyb.micros()

def writer():
    global handlerIndex,outData,interruptCounter,period,start
    if outData.nbElts:
        pdov(outData.get()%2)
    else:
        handlerIndex ^=1
    period[interruptCounter] = pyb.elapsed_micros(start)
    interruptCounter+=1
    start=pyb.micros()