def __init__(self):
		
		# set up motor with PWM and timer control
		self.A1 = Pin('X3',Pin.OUT_PP)	# A is right motor
		self.A2 = Pin('X4',Pin.OUT_PP)
		self.B1 = Pin('X7',Pin.OUT_PP)	# B is left motor
		self.B2 = Pin('X8',Pin.OUT_PP)
		self.PWMA = Pin('X1')			
		self.PWMB = Pin('X2')
		self.speed = 0		# +100 full speed forward, -100 full speed back
		self.turn = 0		# turn is +/-100; 0 = left/right same speed, 
							# ... +50 = left at speed, right stop, +100 = right back full
		# Configure counter 2 to produce 1kHz clock signal
		self.tim = Timer(2, freq = 1000)
		# Configure timer to provide PWM signal
		self.motorA = self.tim.channel(1, Timer.PWM, pin = self.PWMA)
		self.motorB = self.tim.channel(2, Timer.PWM, pin = self.PWMB)
		self.lsf = 0		# left motor speed factor +/- 1
		self.rsf = 0		# right motor speed factor +/- 1
		self.countA = 0			# speed pulse count for motorA
		self.countB = 0			# speed pulse count for motorB
		self.speedA = 0			# actual speed of motorA
		self.speedB = 0			# actual speed of motorB
		
		# Create external interrupts for motorA and motorB Hall Effect Senors
		self.motorA_int = pyb.ExtInt ('Y4', pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE,self.isr_motorA)
		self.motorB_int = pyb.ExtInt ('Y6', pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE,self.isr_motorB)
		self.speed_timer = pyb.Timer(8, freq=10)
		self.speed_timer.callback(self.isr_speed_timer)

class TrackBall:
    def __init__(self,qq):
        self.volEnQueueable  = EnQueueable((EnQueueable.INC,EnQueueable.VOL),qq)
        self.toneEnQueueable = EnQueueable((EnQueueable.INC,EnQueueable.TONE),qq)
        self.targCoilID = 0;
        self.x1=Pin(State.trackballStateDict['x1'], Pin.IN, Pin.PULL_DOWN)
        self.x2=Pin(State.trackballStateDict['x2'], Pin.IN, Pin.PULL_DOWN)
        self.y1=Pin(State.trackballStateDict['y1'], Pin.IN, Pin.PULL_DOWN)
        self.y2=Pin(State.trackballStateDict['y2'], Pin.IN, Pin.PULL_DOWN)
        self.extInts = (ExtInt(State.trackballStateDict['x1'],
                                   ExtInt.IRQ_RISING,
                                   Pin.PULL_DOWN,
                                   self.x11),
                        ExtInt(State.trackballStateDict['y1'],
                                   ExtInt.IRQ_RISING,
                                   Pin.PULL_DOWN,
                                   self.y11))
            
    def x11(self,unused):
        if self.x2.value():
            self.volEnQueueable.push(self.targCoilID,-1)
        else:
            self.volEnQueueable.push(self.targCoilID,1)

    def y11(self,unused):
        if self.y2.value():
            self.toneEnQueueable.push(self.targCoilID,-1)
        else:
            self.toneEnQueueable.push(self.targCoilID,1)

    def start(self, speed, direction):
        PWM_py_pin = Pin(self.PWMpin)
        DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP)

        tim = Timer(self.timer_id, freq=1000)
        ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin)

        if direction in ('cw', 'CW', 'clockwise'):
            DIR_py_pin.high()

        elif direction in ('ccw', 'CCW', 'counterclockwise'):
            DIR_py_pin.low()

        else:
            raise ValueError('Please enter CW or CCW')

        if 0 <= speed <= 100:
            ch.pulse_width_percent(speed)

        else:
            raise ValueError("Please enter a speed between 0 and 100")

        self.isRunning        = True
        self.currentDirection = direction
        self.currentSpeed     = speed

class Relay(object):
  """Control a relay board with an output pin.  Set on to True to drive the relay pin low
     which turns the relay on."""

  def __init__( self, pin ) :
    """Pin may be a pin name or pyb.Pin object set for output."""

    if type(pin) == str:
      self._pin = Pin(pin, Pin.OUT_PP, Pin.PULL_DOWN)
    elif type(pin) == Pin:
      self._pin = pin
    else:
      raise Exception("pin must be pin name or pyb.Pin")

    self.on = False

  @property
  def on( self ) : return self._pin.value()

  @on.setter
  def on( self, value ) :
    if value:
      self._pin.low()
    else:
      self._pin.high()

class STAccel:
    def __init__(self):
        self.cs_pin = Pin('PE3', Pin.OUT_PP, Pin.PULL_NONE)
        self.cs_pin.high()
        self.spi = SPI(1, SPI.MASTER, baudrate=328125, polarity=0, phase=1, bits=8)

        self.who_am_i = self.read_id()

        if self.who_am_i == LIS302DL_WHO_AM_I_VAL:
            self.write_bytes(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF]))
            self.sensitivity = 18
        elif self.who_am_i == LIS3DSH_WHO_AM_I_VAL:
            self.write_bytes(LIS3DSH_CTRL_REG4_ADDR, bytearray([LIS3DSH_CTRL_REG4_CONF]))
            self.write_bytes(LIS3DSH_CTRL_REG5_ADDR, bytearray([LIS3DSH_CTRL_REG5_CONF]))
            self.sensitivity = 0.06 * 256
        else:
            raise Exception('LIS302DL or LIS3DSH accelerometer not present')

    def convert_raw_to_g(self, x):
        if x & 0x80:
            x = x - 256
        return x * self.sensitivity / 1000

    def read_bytes(self, addr, nbytes):
        if nbytes > 1:
            addr |= READWRITE_CMD | MULTIPLEBYTE_CMD
        else:
            addr |= READWRITE_CMD
        self.cs_pin.low()
        self.spi.send(addr)
        #buf = self.spi.send_recv(bytearray(nbytes * [0])) # read data, MSB first
        buf = self.spi.recv(nbytes)
        self.cs_pin.high()
        return buf

    def write_bytes(self, addr, buf):
        if len(buf) > 1:
            addr |= MULTIPLEBYTE_CMD
        self.cs_pin.low()
        self.spi.send(addr)
        for b in buf:
            self.spi.send(b)
        self.cs_pin.high()

    def read_id(self):
        return self.read_bytes(WHO_AM_I_ADDR, 1)[0]

    def x(self):
        return self.convert_raw_to_g(self.read_bytes(OUT_X_ADDR, 1)[0])

    def y(self):
        return self.convert_raw_to_g(self.read_bytes(OUT_Y_ADDR, 1)[0])

    def z(self):
        return self.convert_raw_to_g(self.read_bytes(OUT_Z_ADDR, 1)[0])

    def xyz(self):
        return (self.x(), self.y(), self.z())

 def __init__( self, forward, backward, speed ) :
   """forward pin name, backward pin name, speed = (pin name, timer#)
      Need to make sure the given timer # is associated with the speed
      pin or an exception will be raised.  The speed pin must support
      PWM."""
   self._forward = Pin(forward, Pin.OUT_PP)
   self._backward = Pin(backward, Pin.OUT_PP)
   self._speedControl = PWM(speed[0], speed[1])
   self._speed = 0

def check_joystick():

	adc_1 = ADC(Pin('X19'))
	adc_2 = ADC(Pin('X20'))
	J_sw = Pin('Y11', Pin.IN)

	while True:
		print('Vertical: ',adc_1.read(), 'Horizontal: ', adc_2.read(), 'Switch: ',J_sw.value())
		pyb.delay(2000)	

 def declare_channel(self, channel, direction):
     try:
         self.available_pins.index(channel)
         if self._find_channel(channel):
             return ;
         pin = Pin(channel)
         if direction:
             pin.init(Pin.OUT_PP)
         else:
             pin.init(Pin.IN, Pin.PULL_UP)
         self.channels += [pin]
     except:
         pass

 def __init__(self,qq):
     self.x1=Pin('Y9', Pin.IN, Pin.PULL_DOWN)
     self.x2=Pin('Y7', Pin.IN, Pin.PULL_DOWN)
     self.y1=Pin('Y10', Pin.IN, Pin.PULL_DOWN)
     self.y2=Pin('Y8', Pin.IN, Pin.PULL_DOWN)
     self.extInts = (ExtInt('Y9',
                            ExtInt.IRQ_RISING,
                            Pin.PULL_DOWN,
                            self.x11),
                     ExtInt('Y10',
                            ExtInt.IRQ_RISING,
                            Pin.PULL_DOWN,
                            self.y11))

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	flash_LEDs():

	rLED = Pin('Y9', Pin.OUT_PP)
	bLED = Pin('Y10',Pin.OUT_PP)

	while True:
		rLED.high()
		pyb.delay(250)
		bLED.high()
		pyb.delay(250)
		rLED.low()
		pyb.delay(250)
		bLED.low()
		pyb.delay(250)

class TrackBall:
    def __init__(self,qq):
        self.volEnQueueable  = EnQueueable((EnQueueable.INC,EnQueueable.VOL),qq)
        self.toneEnQueueable = EnQueueable((EnQueueable.INC,EnQueueable.TONE),qq)
        self.targCoilID = 0;
        self.x1=Pin(State.trackballStateDict['x1'], Pin.IN, Pin.PULL_DOWN)
        self.x2=Pin(State.trackballStateDict['x2'], Pin.IN, Pin.PULL_DOWN)
        self.y1=Pin(State.trackballStateDict['y1'], Pin.IN, Pin.PULL_DOWN)
        self.y2=Pin(State.trackballStateDict['y2'], Pin.IN, Pin.PULL_DOWN)
        self.extInts = (ExtInt(State.trackballStateDict['x1'],
                                   ExtInt.IRQ_RISING,
                                   Pin.PULL_DOWN,
                                   self.x11),
                        ExtInt(State.trackballStateDict['y1'],
                                   ExtInt.IRQ_RISING,
                                   Pin.PULL_DOWN,
                                   self.y11))
            
    def x11(self,unused):
        if self.x2.value():
            irq_state = disable_irq()
            self.volEnQueueable.push(self.targCoilID,-1)
            enable_irq(irq_state)
        else:
            irq_state = disable_irq()
            self.volEnQueueable.push(self.targCoilID,1)
            enable_irq(irq_state)
    def y11(self,unused):
        if self.y2.value():
            irq_state = disable_irq()
            self.toneEnQueueable.push(self.targCoilID,-1)
            enable_irq(irq_state)
        else:
            irq_state = disable_irq()
            self.toneEnQueueable.push(self.targCoilID,1)
            enable_irq(irq_state)

    def __repr__(self):
        res = 'TrackBall:'                                        + \
              '\nvolEnQueueable: \t' + repr(self.volEnQueueable)  + \
              '\ntoneEnQueueable:\t' + repr(self.toneEnQueueable) + \
              '\ntargCoilID:     \t' + str(self.targCoilID)       + \
              '\nx1:             \t' + str(self.x1)               + \
              '\nx2:             \t' + str(self.x2)               + \
              '\ny1:             \t' + str(self.y1)               + \
              '\ny2:             \t' + str(self.y2)               #+ \
              #'\nExtInts:        \t' + str([i for i in self.extInts])
        return res

 def __init__(self, bus=1, baudrate=328125, polarity=0, phase=0, ss='A4'):
     self.ss = Pin(ss, Pin.OUT)
     self.ss.high()
     self.spi = SPI(bus, SPI.MASTER, baudrate=baudrate, polarity=polarity,
                    phase=phase)
     self.msgbuf = bytearray(32)
     self.status = bytearray(4)

    def __init__(self, cs='PE3', spi=1, debug=False):
        self._debug = debug
        self.cs_pin = Pin(cs, Pin.OUT_PP, Pin.PULL_NONE)
        self.cs_pin.high()
        self.spi = SPI(spi, SPI.MASTER, baudrate=328125, polarity=0, phase=1,
                       bits=8)

        self.read_id()
        # First SPI read always returns 255 --> discard and read ID again
        self.who_am_i = self.read_id()
        self.debug("Accel-ID: %s" % self.who_am_i)

        if self.who_am_i == LIS302DL_WHO_AM_I_VAL:
            self.write_bytes(LIS302DL_CTRL_REG1_ADDR, LIS302DL_CONF)
            self.sensitivity = 18
        elif self.who_am_i == LIS3DSH_WHO_AM_I_VAL:
            self.write_bytes(LIS3DSH_CTRL_REG4_ADDR, LIS3DSH_CTRL_REG4_CONF)
            self.write_bytes(LIS3DSH_CTRL_REG5_ADDR, LIS3DSH_CTRL_REG5_CONF)
            self.sensitivity = 0.06 * 256
        else:
            msg = 'LIS302DL or LIS3DSH accelerometer not present'

            if self._debug:
                self.debug(msg)
            else:
                raise IOError(msg)

 def __init__(self):
     self.cs_pin = Pin('PE3', Pin.OUT_PP, Pin.PULL_NONE)
     self.cs_pin.high()
     self.spi = SPI(1, SPI.MASTER, baudrate=328125, polarity=0, phase=1, bits=8)
     self.write_bytes(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF]))
     if self.read_id() != LIS302DL_WHO_AM_I_VAL:
         raise Exception('LIS302DL accelerometer not present')

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

 def __init__(self, index):
   timer = timers[rc_pins_timers[index]]
   self.pin = Pin(rc_pins[index])
   self.channel = timer.channel(rc_pins_channels[index],
                                Timer.IC,
                                pin=self.pin,
                                polarity=Timer.BOTH)
   self.channel.callback(self.callback)

class JoyStick:
    # expo formula
    # ouput =((EXPO*POW(input,3))+((1-EXPO)*input))*RATE
    # where input & output are on  [-1,1]
    
    def __init__(self,xp,yp, pbp, pbFunc):  # last arg is a pointer to the interrupt handler
         self.XPin = ADC(Pin(xp))
         self.YPin = ADC(Pin(yp))
         self.PBPin = Pin(pbp, Pin.IN, Pin.PULL_UP)
                          
         self.maxAnalog = 4095
         self.minAnalog = 0
         self.maxOutput = 100
         self.minOutput = -100
         self.pinExpo = 25

         self.onPB = pbFunc
         self.changeDelta   = 400   # ms
         self.lastChangeTime = 0    # ms

         self._calibrateXY()

    def _calibrateXY(self):
        xSum = 0
        ySum = 0

        for i in range(100):
            xSum += self.XPin.read()
            ySum += self.YPin.read()

        self.X0 = round(xSum/100.0)
        self.Y0 = round(ySum/100.0)

    def checkPB(self):
        now = time.ticks_ms()
        if now-self.lastChangeTime > self.changeDelta:
            if not self.PBPin.value():
                self.onPB()
                self.lastChangeTime = now
                
    def _read(self, x):
        pin = self.XPin
        V0 =  self.X0
        if not x:
            pin = self.YPin
            V0 =  self.Y0
            
        val = pin.read()
        if abs(val - V0) < self.pinExpo:
            return(0)
        return arduino_map(val,V0,self.maxAnalog,0,self.maxOutput) \
            if val > self.X0 else \
               arduino_map(val,self.minAnalog,V0,self.minOutput,0) 

    def readX(self):
        return self._read(True)
    def readY(self):
        return self._read(False)

 def __init__(self, width, height, depth, red, green, blue, a, b, c, d, clk, latch, oe):
     self.__width = width
     self.__bwidth = width//4*3
     self.__height = height
     self.__BYTES_PER_WEIGHT= 3*width*height>>3
     self.__BITPERCOLOR = depth
     self.__red = tuple(Pin(i, Pin.OUT_PP) for i in red)
     self.__green = tuple(Pin(i, Pin.OUT_PP) for i in green)
     self.__blue = tuple(Pin(i, Pin.OUT_PP) for i in blue)
     self.__color = list(Pin(i, Pin.OUT_PP) for i in color_sel)
     self.__color.extend(list(Pin(i, Pin.OUT_PP) for i in green))
     self.__color.extend(list(Pin(i, Pin.OUT_PP) for i in blue))
     print(self.__color)
     self.__a = Pin(a, Pin.OUT_PP)
     self.__b = Pin(b, Pin.OUT_PP)
     self.__c = Pin(c, Pin.OUT_PP)
     self.__d = None
     self.__cycle_max = 8
     if d is not None:
         self.__d = Pin(d, Pin.OUT_PP)
         self.__cycle_max = 16
     self.__clk = Pin(clk, Pin.OUT_PP)
     self.__latch = Pin(latch, Pin.OUT_PP)
     self.__oe = Pin(oe, Pin.OUT_PP)
     self.__buffer = bytearray(self.__width*self.__height*2)
     self.__next_linenr = 0
     self.__next_ln2weight = 1
     self.__set_hi = []
     self.__set_lo = []
     if self.DEBUG:
         print(self)

 def __init__(self,spiOnX,latchPin):
     # create an SPI.MASTER instance on the 'X' side of the board, 
     # first arg=1 means 'X side' of the board
     boardSide = 1
     if not spiOnX:
         boardSide = 2
     self.spi = SPI(boardSide,SPI.MASTER)
     # create the stcp pin on the "latch" pin
     self.stcp = Pin(latchPin, Pin.OUT_PP)