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

        # Create a microseconds counter.
        micros = pyb.Timer(2, prescaler=83, period=0x3fffffff)
        micros.counter(0)

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

        # Wait 'till whe pulse starts.
        while self.echo.value() == 0:
            start = micros.counter()

        # Wait 'till the pulse is gone.
        while self.echo.value() == 1:
            end = micros.counter()

        # Deinit the microseconds counter
        micros.deinit()

        # 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) / 2) / 29

        return dist_in_cm

    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	ultrasound():
	
	Trigger = Pin('X3', Pin.OUT_PP)
	Echo = Pin('X4',Pin.IN)
	
	# Create a microseconds counter.
	micros = pyb.Timer(2, prescaler=83, period=0x3fffffff)
	micros.counter(0)
	start = 0
	end = 0
	
	# Send a 20usec pulse every 10ms
	while True:
		Trigger.high()
		pyb.udelay(20)
		Trigger.low()
		
		# Wait until pulse starts
		while Echo.value() == 0:   # do nothing
			start = micros.counter()	# mark time at rising edge
		
		# Wait until pulse goes low
		while Echo.value() == 1:   # do nothing
			end = micros.counter()		# mark time at falling edge
		
		# Duration echo pulse = end - start
		# Divide this by 2 to take account of round-trip
		# Speed of sound in air is 340 m/s or 29 us/cm
		# Distance in cm = (pulse_width)*0.5/29
		distance = int(((end - start) / 2) / 29)
		print('Distance: ', distance, ' cm')
		pyb.delay(500)

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

    self.counter = 0

    #Send 10us pulse.
    self._tpin.high()
    udelay(10)
    self._tpin.low()

    while not self._epin.value():
      start = self.counter

    j = 0

    # Wait 'till the pulse is gone.
    while self._epin.value() and j < 1000:
      j += 1
      end = self.counter

    # 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).
    return (end - start) / 58

def init(set=True):
    global interruptOnLowClock
    cv(0)
    pyb.udelay(110)
    dv(0)
    if set:
        interruptOnLowClock = setInterrupt(ci)

 def get(self):
     self._trig.high()
     pyb.udelay(10)
     self._trig.low()
     d = pulseIn(self._echo, HIGH) / 29 / 2
     if d and d > D_MIN and d < D_MAX:
         return {'enable':True}

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 time_it(self, idx):
     sig = self.in_pin.value()
     while self.in_pin.value() == sig:
         self.buf[idx] += 1
         pyb.udelay(20)
         if  self.buf[idx] > 350:  # EOF
             raise Exception()

    def get_pixel_data(self):            # TODO: send data to serPort do not print the data here
        isFirstPixel = True

        # write to frame capture register to force capture of frame
        self.write_register(ADNS3080_FRAME_CAPTURE,0x83);
        
        # wait 3 frame periods + 10 nanoseconds for frame to be captured
        pyb.udelay(1510);  # min frame speed is 2000 frames/second so 1 frame = 500 nano seconds.  so 500 x 3 + 10 = 1510

        data = "[["
        # display the pixel data
        for i in range(ADNS3080_PIXELS_Y):
            for j in range(ADNS3080_PIXELS_X):
                regValue = self.read_register(ADNS3080_FRAME_CAPTURE)

                if( isFirstPixel and (regValue & 0x40) == 0 ):
                    print("failed to find first pixel\n")

                isFirstPixel = False
                pixelValue = ( regValue << 2) & 255             # Shift to the left and cut off the last to bits
                data += str(pixelValue)                       # Used to be -> pixelValue,DEC not sure what the ,DEC did do
                if( j!= ADNS3080_PIXELS_X-1 ):
                    data += ","
                pyb.udelay(50)

            data += "\n"
        data += "]]"
        return data

    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 update(self):
        # TODO: check for constants used
        # TODO: return x and y changes
        surface_quality = self.read_register(ADNS3080_SQUAL)
        # small delay
        pyb.udelay(50)

        # check for movement, update x,y values
        motion_reg = self.read_register(ADNS3080_MOTION)
        _overflow = ((motion_reg & 0x10) != 0)              # check if we've had an overflow # TODO: do something whit this info
        if( (motion_reg & 0x80) != 0 ):
            raw_dx = self.read_register(ADNS3080_DELTA_X, signed=True)
            # small delay
            pyb.udelay(50)
            raw_dy = self.read_register(ADNS3080_DELTA_Y, signed=True)
            self._motion = True
        else:
            raw_dx = 0
            raw_dy = 0

        last_update = pyb.millis()

        # Fix for orientation if needed
        #self.apply_orientation_matrix()

        self.dx = raw_dx
        self.dy = raw_dy

        self.x += raw_dx
        self.y += raw_dy

        return True

    def read_register(self, address, signed=False):
        # take the chip select low to select the device
        self._cs_pin.low()

        # send the device the register you want to read
        #junk = self.spi.send_recv((address).to_bytes(1))
        self.spi.send((address).to_bytes(1))
        junk = self.spi.recv(1)

        # small delay
        pyb.udelay(50)

        # end a value of 0 to read the first byte returned
        #result = self.spi.send_recv((0x00).to_bytes(1))
        self.spi.send((0x00).to_bytes(1))
        result = self.spi.recv(1)

        # take the chip select high to de-select
        self._cs_pin.high()

        # Fix for signed or unsigned bytes
        if signed:
            result = struct.unpack('b', result)[0]
        else:
            result = struct.unpack('B', result)[0]
        return result

def update(t):                  # Interrupt handler may not be able to acquire the lock
    global var1, var2           # if main loop has it
    if mutex.test():            # critical section start
        var1 += 1
        pyb.udelay(200)
        var2 += 1
        mutex.release()         # critical section end

def init():
    global moduleInit
    cv(0)
    pyb.udelay(110)
    dv(0)
    if not moduleInit:
        moduleInit=True
        setInterrupt(ci)

 def step(self, num):
     for i in range(num):
         phase = self.phase.__next__()
         self.pin1.value(phase[0])
         self.pin2.value(phase[1])
         self.pin3.value(phase[2])
         self.pin4.value(phase[3])
         pyb.udelay(self.delay_time)

 def info(self):
     self.pinCS.low()
     pyb.udelay(1000)                        # FLASH wake up delay
     self.spi.send(FLASH_RDID)
     manufacturer = self.spi.send_recv(FLASH_NOP)[0]
     id_high = self.spi.send_recv(FLASH_NOP)[0]
     id_low = self.spi.send_recv(FLASH_NOP)[0]
     self.pinCS.high()
     return manufacturer, id_high << 8 | id_low

    def reset(self):
        # return immediately if the reset pin is not defined
        if( ADNS3080_RESET == 0):
            return

        self._reset_pin.high()                  # reset sensor
        pyb.udelay(10)
        self._reset_pin.low()                   # return sensor to normal
        pyb.udelay(10)

 def read(self,read_addr,nr_bytes):
     buf = bytearray(1)
     buf[0]=read_addr
     self.CS.low()
     pyb.udelay(self.delay)
     self._write(buf)
     result = self._read(nr_bytes)
     self.CS.high()
     return result

def playSlurred(note, Hz, length):
#    print(note, Hz, int(Hz * length))
    for duration in range(0, int(Hz * length), 2):
#        print('Running note:', note + ':', 1/Hz, duration, '/', int(Hz * length))
        phone.high()
        udelay(int(1e6/Hz))
        phone.low()
        udelay(int(1e6/Hz))
    print('Done')

 def __init__(self, sensor_table, srv):
     self.light_sensor = LightSensor()
     self.mutex = esp.mutex()
     self.sensor_table = sensor_table
     self.sensors = dict()
     self.task = esp.os_task(callback=lambda task: handler(task, srv, self))
     for sensor_name, sensor_port in sensor_table.items():
         self.sensors[sensor_name] = Sensor(sensor_name, sensor_port, 10000, task=self.task, mutex=self.mutex)
         pyb.udelay(10000)