kopia lustrzana https://github.com/GuyCarver/MicroPython
update apds9960.py
Guy Carver
rodzic
e5c70b2b29
commit
92869fc266
254
lib/APDS9960.py
254
lib/APDS9960.py
|
|
@ -198,11 +198,23 @@ class apds9960(object) :
|
|||
_GWTIME_30_8MS = const(6)
|
||||
_GWTIME_39_2MS = const(7)
|
||||
|
||||
_NONE = const(0)
|
||||
_UP = const(1)
|
||||
_DOWN = const(2)
|
||||
_LEFT = const(3)
|
||||
_RIGHT = const(4)
|
||||
_THRESHOLD_OUT = const(10)
|
||||
_SENSITIVITY_1 = const(50)
|
||||
_SENSITIVITY_2 = const(20)
|
||||
|
||||
_DIR_NONE = const(0)
|
||||
_DIR_UP = const(1)
|
||||
_DIR_DOWN = const(2)
|
||||
_DIR_LEFT = const(3)
|
||||
_DIR_RIGHT = const(4)
|
||||
_DIR_NEAR = const(5)
|
||||
_DIR_FAR = const(6)
|
||||
_DIR_ALL = const(7)
|
||||
|
||||
_NA_STATE = const(0)
|
||||
_NEAR_STATE = const(1)
|
||||
_FAR_STATE = const(2)
|
||||
_ALL_STATE = const(3)
|
||||
|
||||
_DefaultIntTimeMS = const(10)
|
||||
|
||||
|
|
@ -225,9 +237,24 @@ class apds9960(object) :
|
|||
self._status = 0 #Status
|
||||
self._gstatus = 0 #GStatus
|
||||
|
||||
self.resetcounts()
|
||||
#gesture read variables.
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._near_count = 0
|
||||
self._far_count = 0
|
||||
self._state = 0
|
||||
self._motion = _DIR_NONE
|
||||
self._udata = bytearray(32)
|
||||
self._ddata = bytearray(32)
|
||||
self._ldata = bytearray(32)
|
||||
self._rdata = bytearray(32)
|
||||
self._index = 0
|
||||
self._total_gestures= 0
|
||||
self._fifo_data = bytearray(128)
|
||||
|
||||
self._millis = 0
|
||||
self.resetcounts()
|
||||
|
||||
self._b1 = bytearray(1)
|
||||
self._b2 = bytearray(2)
|
||||
|
|
@ -240,6 +267,10 @@ class apds9960(object) :
|
|||
self._i2c.mem_read(self._b1, _ADDRESS, aLoc)
|
||||
return self._b1[0]
|
||||
|
||||
def readbuffer( self, aBuffer, aLoc ) :
|
||||
"""Read 8 bit values into given buffer."""
|
||||
self._i2c.mem_read(aBuffer, _ADDRESS, aLoc)
|
||||
|
||||
def read16( self, aLoc ) :
|
||||
"""Read 16 bit value and return."""
|
||||
self._i2c.mem_read(self._b2, _ADDRESS, aLoc)
|
||||
|
|
@ -494,59 +525,182 @@ class apds9960(object) :
|
|||
self.enabled = True
|
||||
sleep_us(50)
|
||||
|
||||
def resetgestureparams( self ) :
|
||||
self._index = 0
|
||||
self._total_gestures = 0
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._near_count = 0
|
||||
self._far_count = 0
|
||||
self._state = 0
|
||||
self._motion = _DIR_NONE
|
||||
|
||||
def readgesture( self ) :
|
||||
#todo: implement
|
||||
while True :
|
||||
ud_diff = 0
|
||||
lr_diff = 0
|
||||
gesturereceived = 0
|
||||
if not self.gesturevalid :
|
||||
return 0
|
||||
if self.gesturevalid :
|
||||
while True :
|
||||
sleep_us(30) #Wait a bit to make sure fifo buffer is full.
|
||||
|
||||
sleep_us(20) #todo: find correct value for this.
|
||||
gstatus = self.getgstatus(GSTATUS_GVALID)
|
||||
if gstatus :
|
||||
fifo_level = self.read(_GFLVL)
|
||||
if fifo_level > 0 :
|
||||
bread = fifo_level * 4
|
||||
self.readbuffer(self._fifo_data, _GFIFO_U)
|
||||
|
||||
toread = self.read(_GFLVL)
|
||||
buf = self._i2c.mem_read(toread, _ADDRESS, _GFIFO_U)
|
||||
for i in range(0, fifo_level):
|
||||
i2 = i * 4
|
||||
self._udata[i] = self._fifo_data[i2]
|
||||
self._ddata[i] = self._fifo_data[i2 + 1]
|
||||
self._ldata[i] = self._fifo_data[i2 + 2]
|
||||
self._rdata[i] = self._fifo_data[i2 + 3]
|
||||
|
||||
diff = buf[0] - buf[1]
|
||||
if abs(diff) > 13 :
|
||||
ud_diff += diff
|
||||
self._index += fifo_level - 1
|
||||
self._total_gestures += fifo_level - 1
|
||||
|
||||
diff = buf[2] - buf[3]
|
||||
if abs(diff) > 13 :
|
||||
lr_diff += diff
|
||||
if self.processgesturedata() :
|
||||
self._index = 0
|
||||
self._total_gestures = 0
|
||||
else:
|
||||
sleep_us(30)
|
||||
self.decodegesture()
|
||||
self.resetgestureparams()
|
||||
return self._motion
|
||||
|
||||
if ud_diff != 0 :
|
||||
if ud_diff < 0 :
|
||||
if self._dcount > 0 :
|
||||
gesturereceived = _UP
|
||||
else:
|
||||
self._ucount += 1
|
||||
elif ud_diff > 0 :
|
||||
if self._ucount > 0 :
|
||||
gesturereceived = _DOWN
|
||||
else:
|
||||
self._dcount += 1
|
||||
return _DIR_NONE
|
||||
|
||||
if lr_diff != 0 :
|
||||
if lr_diff < 0 :
|
||||
if self._rcount > 0 :
|
||||
gesturereceived = _LEFT
|
||||
else:
|
||||
self._lcount += 1
|
||||
elif lr_diff > 0 :
|
||||
if self._lcount > 0 :
|
||||
gesturereceived = _RIGHT
|
||||
else:
|
||||
self._rcount += 1
|
||||
def processgesturedata( self ) :
|
||||
if self._total_gestures > 4 :
|
||||
ufirst = 0
|
||||
dfirst = 0
|
||||
lfirst = 0
|
||||
rfirst = 0
|
||||
ulast = 0
|
||||
dlast = 0
|
||||
llast = 0
|
||||
rlast = 0
|
||||
|
||||
if (ud_diff != 0) or (lr_diff != 0) :
|
||||
self._millis = ticks_ms()
|
||||
if 0 < self._total_gestures <= 32 :
|
||||
for i in range(0, self._total_gestures) :
|
||||
u = self._udata[i]
|
||||
d = self._ddata[i]
|
||||
l = self._ldata[i]
|
||||
r = self._rdata[i]
|
||||
if u > _THRESHOLD_OUT and d > _THRESHOLD_OUT and l > _THRESHOLD_OUT and r > _THRESHOLD_OUT :
|
||||
ufirst = u
|
||||
dfirst = d
|
||||
lfirst = l
|
||||
rfirst = r
|
||||
break
|
||||
|
||||
if gesturereceived or (ticks_ms() - self._millis > 300) :
|
||||
self.resetcounts()
|
||||
if ufirst == 0 or dfirst == 0 or lfirst == 0 or rfirst == 0 :
|
||||
return False
|
||||
|
||||
return gesturereceived
|
||||
for i in range(self._total_gestures - 1, -1, -1) :
|
||||
u = self._udata[i]
|
||||
d = self._ddata[i]
|
||||
l = self._ldata[i]
|
||||
r = self._rdata[i]
|
||||
if u > _THRESHOLD_OUT and d > _THRESHOLD_OUT and l > _THRESHOLD_OUT and r > _THRESHOLD_OUT :
|
||||
ulast = u
|
||||
dlast = d
|
||||
llast = l
|
||||
rlast = r
|
||||
break
|
||||
|
||||
ud_ratio_first = ((ufirst - dfirst) * 100) // (ufirst + dfirst)
|
||||
lr_ratio_first = ((lfirst - rfirst) * 100) // (lfirst + rfirst)
|
||||
ud_ratio_last = ((ulast - dlast) * 100) // (ulast + dlast)
|
||||
lr_ratio_last = ((llast - rlast) * 100) // (llast + rlast)
|
||||
|
||||
ud_delta = ud_ratio_last - ud_ratio_first
|
||||
lr_delta = lr_ratio_last - lr_ratio_first
|
||||
|
||||
self._ud_delta += ud_delta
|
||||
self._lr_delta += lr_delta
|
||||
|
||||
if self._ud_delta >= _SENSITIVITY_1 :
|
||||
self._ud_count = 1
|
||||
elif self._ud_delta <= -_SENSITIVITY_1 :
|
||||
self._ud_count = -1
|
||||
else:
|
||||
self._ud_count = 0
|
||||
|
||||
if self._lr_delta >= _SENSITIVITY_1 :
|
||||
self._lr_count = 1
|
||||
elif self._lr_delta <= -_SENSITIVITY_1 :
|
||||
self._lr_count = -1
|
||||
else:
|
||||
self._lr_count = 0
|
||||
|
||||
if self._ud_count == 0 and self._lr_count == 0 :
|
||||
if abs(ud_delta) < _SENSITIVITY_2 and abs(lr_delta) < _SENSITIVITY_2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._near_count += 1
|
||||
elif ud_delta != 0 and lr_delta != 0 :
|
||||
self._far_count += 1
|
||||
|
||||
if self._near_count >= 10 and self._far_count >= 2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._state = _NEAR_STATE
|
||||
elif ud_delta != 0 and lr_delta != 0 :
|
||||
self._state = _FAR_STATE
|
||||
|
||||
return True
|
||||
else:
|
||||
if abs(ud_delta) < _SENSITIVITY_2 and abs(lr_delta) < _SENSITIVITY_2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._near_count += 1
|
||||
|
||||
if self._near_count >= 10 :
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
|
||||
return False
|
||||
|
||||
def decodegesture( self ) :
|
||||
if self._state == _NEAR_STATE :
|
||||
self._motion = _DIR_NEAR
|
||||
return True
|
||||
elif self._state == _FAR_STATE :
|
||||
self._motion = _DIR_FAR
|
||||
return True
|
||||
|
||||
if self._ud_count == -1 and self._lr_count == 0 :
|
||||
self._motion = _DIR_UP
|
||||
elif self._ud_count == 1 and self._lr_count == 0 :
|
||||
self._motion = _DIR_DOWN
|
||||
elif self._ud_count == 0 and self._lr_count == 1 :
|
||||
self._motion == _DIR_RIGHT
|
||||
elif self._ud_count == 0 and self._lr_count == -1 :
|
||||
self._motion == _DIR_LEFT
|
||||
elif self._ud_count == -1 and self._lr_count == 1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_UP
|
||||
else:
|
||||
self._motion = _DIR_RIGHT
|
||||
elif self._ud_count == 1 and self._lr_count == -1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_DOWN
|
||||
else:
|
||||
self._motion = _DIR_LEFT
|
||||
elif self._ud_count == -1 and self._lr_count == -1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_UP
|
||||
else:
|
||||
self._motion = _DIR_LEFT
|
||||
elif self._ud_count == 1 and self._lr_count == 1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_DOWN
|
||||
else:
|
||||
self._motion = _DIR_RIGHT
|
||||
else:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
# def calculatecolortemp( self, aRed, aGreen, aBlue ) :
|
||||
# x = (-0.14282 * aRed) + (1.54924 * aGreen) + (-0.95641 * aBlue)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,669 @@
|
|||
# MicroPython apds9960 motion detection device driver.
|
||||
|
||||
#todo: Have a mix of styles here. Some use properties for setters while others use functions.
|
||||
|
||||
import pyb
|
||||
from utime import *
|
||||
|
||||
class apds(object) :
|
||||
|
||||
_ADDRESS = const(0x39)
|
||||
_RAM = const(0x00)
|
||||
_ENABLE = const(0x80)
|
||||
_ATIME = const(0x81)
|
||||
_WTIME = const(0x83)
|
||||
_AILTL = const(0x84)
|
||||
_AILTH = const(0x85)
|
||||
_AIHTL = const(0x86)
|
||||
_AIHTH = const(0x87)
|
||||
_PILT = const(0x89)
|
||||
_PIHT = const(0x8B)
|
||||
_PERS = const(0x8C)
|
||||
_CONFIG1 = const(0x8D)
|
||||
_PPULSE = const(0x8E)
|
||||
_CONTROL = const(0x8F)
|
||||
_CONFIG2 = const(0x90)
|
||||
_ID = const(0x92)
|
||||
_STATUS = const(0x93)
|
||||
_CDATAL = const(0x94)
|
||||
_CDATAH = const(0x95)
|
||||
_RDATAL = const(0x96)
|
||||
_RDATAH = const(0x97)
|
||||
_GDATAL = const(0x98)
|
||||
_GDATAH = const(0x99)
|
||||
_BDATAL = const(0x9A)
|
||||
_BDATAH = const(0x9B)
|
||||
_PDATA = const(0x9C)
|
||||
_POFFSET_UR = const(0x9D)
|
||||
_POFFSET_DL = const(0x9E)
|
||||
_CONFIG3 = const(0x9F)
|
||||
_GPENTH = const(0xA0)
|
||||
_GEXTH = const(0xA1)
|
||||
_GCONF1 = const(0xA2)
|
||||
_GCONF2 = const(0xA3)
|
||||
_GOFFSET_U = const(0xA4)
|
||||
_GOFFSET_D = const(0xA5)
|
||||
_GOFFSET_L = const(0xA7)
|
||||
_GOFFSET_R = const(0xA9)
|
||||
_GPULSE = const(0xA6)
|
||||
_GCONF3 = const(0xAA)
|
||||
_GCONF4 = const(0xAB)
|
||||
_GFLVL = const(0xAE)
|
||||
_GSTATUS = const(0xAF)
|
||||
_IFORCE = const(0xE4)
|
||||
_PICLEAR = const(0xE5)
|
||||
_CICLEAR = const(0xE6)
|
||||
_AICLEAR = const(0xE7)
|
||||
_GFIFO_U = const(0xFC)
|
||||
_GFIFO_D = const(0xFD)
|
||||
_GFIFO_L = const(0xFE)
|
||||
_GFIFO_R = const(0xFF)
|
||||
|
||||
_PON = const(0x01)
|
||||
_AEN = const(0x02)
|
||||
_PEN = const(0x04)
|
||||
_WEN = const(0x08)
|
||||
_AIEN = const(0x10)
|
||||
_PIEN = const(0x20)
|
||||
_GEN = const(0x40)
|
||||
|
||||
_GVALID = const(0x01)
|
||||
|
||||
_THRESHOLD_OUT = const(10)
|
||||
_SENSITIVITY_1 = const(50)
|
||||
_SENSITIVITY_2 = const(20)
|
||||
|
||||
_ERROR = const(0xFF)
|
||||
|
||||
#Device IDs
|
||||
_ID_1 = const(0xAB)
|
||||
_ID_2 = const(0x9C)
|
||||
|
||||
_FIFO_PAUSE_TIME = const(30) # Wait period (ms) between FIFO reads
|
||||
|
||||
#P Gain
|
||||
_PGAIN_1 = const(0) #1x gain
|
||||
_PGAIN_2 = const(1) #2x gain
|
||||
_PGAIN_4 = const(2) #4x gain
|
||||
_PGAIN_8 = const(3) #8x gain
|
||||
|
||||
#LED Drive
|
||||
_LEDDRIVE_100MA = const(0) #100mA
|
||||
_LEDDRIVE_50MA = const(1) #50mA
|
||||
_LEDDRIVE_25MA = const(2) #25mA
|
||||
_LEDDRIVE_12_5MA = const(3) #12.5mA
|
||||
|
||||
#LED Boost
|
||||
_LEDBOOST_100 = const(0)
|
||||
_LEDBOOST_150 = const(1)
|
||||
_LEDBOOST_200 = const(2)
|
||||
_LEDBOOST_300 = const(3)
|
||||
|
||||
#G Gain
|
||||
_GGAIN_1 = const(0)
|
||||
_GGAIN_2 = const(1)
|
||||
_GGAIN_4 = const(2)
|
||||
_GGAIN_8 = const(3)
|
||||
|
||||
#G Gain
|
||||
_AGAIN_1 = const(0)
|
||||
_AGAIN_4 = const(1)
|
||||
_AGAIN_16 = const(2)
|
||||
_AGAIN_64 = const(3)
|
||||
|
||||
#Following are the bytes describing the data packed into each 8 bit value on the chip.
|
||||
#Nibble 1 = position, 0 = bits
|
||||
|
||||
_POWER = const(0)
|
||||
_AMBIENT_LIGHT = const(1)
|
||||
_PROXIMITY = const(2)
|
||||
_WAIT = const(3)
|
||||
_AMBIENT_LIGHT_INT = const(4)
|
||||
_PROXIMITY_INT = const(5)
|
||||
_GESTURE = const(6)
|
||||
_ALL = const(7)
|
||||
|
||||
# Gesture wait time values
|
||||
GWTIME_0MS = const(0)
|
||||
GWTIME_2_8MS = const(1)
|
||||
GWTIME_5_6MS = const(2)
|
||||
GWTIME_8_4MS = const(3)
|
||||
GWTIME_14_0MS = const(4)
|
||||
GWTIME_22_4MS = const(5)
|
||||
GWTIME_30_8MS = const(6)
|
||||
GWTIME_39_2MS = const(7)
|
||||
|
||||
_DEFAULT_ATIME = const(219) # 103ms
|
||||
_DEFAULT_WTIME = const(246) # 27ms
|
||||
_DEFAULT_PROX_PPULSE = const(0x87) # 16us, 8 pulses
|
||||
_DEFAULT_GESTURE_PPULSE = const(0x89) # 16us, 10 pulses
|
||||
_DEFAULT_POFFSET_UR = const(0) # 0 offset
|
||||
_DEFAULT_POFFSET_DL = const(0) # 0 offset
|
||||
_DEFAULT_CONFIG1 = const(0x60) # No 12x wait (WTIME) factor
|
||||
_DEFAULT_LDRIVE = const(0) #_LED_DRIVE_100MA
|
||||
_DEFAULT_PGAIN = const(2) #_PGAIN_4
|
||||
_DEFAULT_AGAIN = const(1) #_AGAIN_4
|
||||
_DEFAULT_PILT = const(0) # Low proximity threshold
|
||||
_DEFAULT_PIHT = const(50) # High proximity threshold
|
||||
_DEFAULT_AILT = const(0xFFFF) # Force interrupt for calibration
|
||||
_DEFAULT_AIHT = const(0)
|
||||
_DEFAULT_PERS = const(0x11) # 2 consecutive prox or ALS for int.
|
||||
_DEFAULT_CONFIG2 = const(0x01) # No saturation interrupts or LED boost
|
||||
_DEFAULT_CONFIG3 = const(0) # Enable all photodiodes, no SAI
|
||||
_DEFAULT_GPENTH = const(40) # Threshold for entering gesture mode
|
||||
_DEFAULT_GEXTH = const(30) # Threshold for exiting gesture mode
|
||||
_DEFAULT_GCONF1 = const(0x40) # 4 gesture events for int., 1 for exit
|
||||
_DEFAULT_GGAIN = const(2) #_GGAIN_4
|
||||
_DEFAULT_GLDRIVE = const(0) #_LED_DRIVE_100MA
|
||||
_DEFAULT_GWTIME = const(1) #_GWTIME_2_8MS
|
||||
_DEFAULT_GOFFSET = const(0) # No offset scaling for gesture mode
|
||||
_DEFAULT_GPULSE = const(0xC9) # 32us, 10 pulses
|
||||
_DEFAULT_GCONF3 = const(0) # All photodiodes active during gesture
|
||||
_DEFAULT_GIEN = const(0) # Disable gesture interrupts
|
||||
|
||||
_DIR_NONE = const(0)
|
||||
_DIR_LEFT = const(1)
|
||||
_DIR_RIGHT = const(2)
|
||||
_DIR_UP = const(3)
|
||||
_DIR_DOWN = const(4)
|
||||
_DIR_NEAR = const(5)
|
||||
_DIR_FAR = const(6)
|
||||
_DIR_ALL = const(7)
|
||||
|
||||
_NA_STATE = const(0)
|
||||
_NEAR_STATE = const(1)
|
||||
_FAR_STATE = const(2)
|
||||
_ALL_STATE = const(3)
|
||||
|
||||
def __init__( self, aLoc ) :
|
||||
"""aLoc I2C pin location is either 1, 'X', 2 or'Y'."""
|
||||
self.i2c = pyb.I2C(aLoc, pyb.I2C.MASTER)
|
||||
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._near_count = 0
|
||||
self._far_count = 0
|
||||
self._state = 0
|
||||
self._motion = _DIR_NONE
|
||||
self._udata = bytearray(32)
|
||||
self._ddata = bytearray(32)
|
||||
self._ldata = bytearray(32)
|
||||
self._rdata = bytearray(32)
|
||||
self._index = 0
|
||||
self._total_gestures= 0
|
||||
self._in_threshold = 0
|
||||
self._out_threshold = 0
|
||||
|
||||
self._b1 = bytearray(1)
|
||||
|
||||
sleep_us(50)
|
||||
self.init()
|
||||
|
||||
def read( self, aLoc ) :
|
||||
"""Read 8 bit value and return."""
|
||||
self.i2c.mem_read(self._b1, _ADDRESS, aLoc)
|
||||
# print('Read {:02x} from {:02x}.'.format(self._b1[0], aLoc))
|
||||
return self._b1[0]
|
||||
|
||||
def readblock( self, aLoc, aDest ) :
|
||||
return self.i2c.mem_read(aDest, _ADDRESS, aLoc)
|
||||
|
||||
def write( self, aLoc, aVal ) :
|
||||
"""Write 8 bit value to given address. aVal may be an int buffer."""
|
||||
self.i2c.mem_write(aVal, _ADDRESS, aLoc)
|
||||
# print('write {:02x} to {:02x}.'.format(aVal, aLoc))
|
||||
|
||||
def init( self ) :
|
||||
id = self.read(_ID)
|
||||
|
||||
if id != _ID_1 and id != _ID_2 :
|
||||
raise Exception('Incorrect apds9960 ID. {}'.format(id))
|
||||
|
||||
self.setmode(_ALL, False)
|
||||
|
||||
self.write(_ATIME, _DEFAULT_ATIME)
|
||||
self.write(_WTIME, _DEFAULT_WTIME)
|
||||
self.write(_PPULSE, _DEFAULT_PROX_PPULSE)
|
||||
self.write(_POFFSET_UR, _DEFAULT_POFFSET_UR)
|
||||
self.write(_POFFSET_DL, _DEFAULT_POFFSET_DL)
|
||||
self.write(_CONFIG1, _DEFAULT_CONFIG1)
|
||||
|
||||
self.setleddrive(_DEFAULT_LDRIVE)
|
||||
self.setproximitygain(_DEFAULT_PGAIN)
|
||||
self.setambientlightgain(_DEFAULT_AGAIN)
|
||||
self.setproxintlowthresh(_DEFAULT_PILT)
|
||||
self.setproxinthighthresh(_DEFAULT_PIHT)
|
||||
self.setlightintlowthresh(_DEFAULT_AILT)
|
||||
self.setlightinthighthresh(_DEFAULT_AIHT)
|
||||
|
||||
self.write(_PERS, _DEFAULT_PERS)
|
||||
self.write(_CONFIG2, _DEFAULT_CONFIG2)
|
||||
self.write(_CONFIG3, _DEFAULT_CONFIG3)
|
||||
|
||||
self.setgestureenterthresh(_DEFAULT_GPENTH)
|
||||
self.setgestureexitthresh(_DEFAULT_GEXTH)
|
||||
|
||||
self.write(_GCONF1, _DEFAULT_GCONF1)
|
||||
|
||||
self.setgesturegain(_DEFAULT_GGAIN)
|
||||
self.setgestureleddrive(_DEFAULT_GLDRIVE)
|
||||
self.setgesturewaittime(_DEFAULT_GWTIME)
|
||||
|
||||
self.write(_GOFFSET_U, _DEFAULT_GOFFSET)
|
||||
self.write(_GOFFSET_D, _DEFAULT_GOFFSET)
|
||||
self.write(_GOFFSET_L, _DEFAULT_GOFFSET)
|
||||
self.write(_GOFFSET_R, _DEFAULT_GOFFSET)
|
||||
self.write(_GPULSE, _DEFAULT_GPULSE)
|
||||
self.write(_GCONF3, _DEFAULT_GCONF3)
|
||||
|
||||
self.setgestureintenable(_DEFAULT_GIEN)
|
||||
|
||||
def getmode( self ) :
|
||||
return self.read(_ENABLE)
|
||||
|
||||
def setmode( self, aMode, aEnable ) :
|
||||
v = self.getmode()
|
||||
if 0 <= aMode <= 6 :
|
||||
aMode = 1 << aMode
|
||||
if aEnable :
|
||||
v |= aMode
|
||||
else:
|
||||
v &= ~aMode
|
||||
elif aMode == _ALL :
|
||||
v = 0x7F if aEnable else 0
|
||||
|
||||
self.write(_ENABLE, v)
|
||||
|
||||
def enablelightsensor( self, aInt ) :
|
||||
self.setambientlightgain(_DEFAULT_AGAIN)
|
||||
self.setambientlightintenable(aInt)
|
||||
self.enablepower()
|
||||
self.setmode(_AMBIENT_LIGHT, True)
|
||||
|
||||
def disablelightsensor( self ) :
|
||||
self.setambientlightintenable(False)
|
||||
self.setmode(_AMBIENT_LIGHT, False)
|
||||
|
||||
def enableproximitysensor( self, aInt ) :
|
||||
self.setproximitygain(_DEFAULT_PGAIN)
|
||||
self.setleddrive(_DEFAULT_LDRIVE)
|
||||
self.setproximityintenable(aInt)
|
||||
self.enablepower()
|
||||
self.setmode(_PROXIMITY, True)
|
||||
|
||||
def disableproximitysensor( self ) :
|
||||
self.setproximityintenable(False)
|
||||
self.setmode(_PROXIMITY, False)
|
||||
|
||||
def enablegesturesensor( self, aInt ) :
|
||||
self.resetgestureparams()
|
||||
self.write(_WTIME, 0xFF)
|
||||
self.write(_PPULSE, _DEFAULT_GESTURE_PPULSE)
|
||||
self.setledboost(_LEDBOOST_300)
|
||||
self.setgestureintenable(aInt)
|
||||
self.setgesturemode(True)
|
||||
self.enablepower()
|
||||
self.setmode(_WAIT, True)
|
||||
self.setmode(_PROXIMITY, True)
|
||||
self.setmode(_GESTURE, True)
|
||||
|
||||
def disablegesturesensor( self ) :
|
||||
self.resetgestureparams()
|
||||
self.setgestureintenable(False)
|
||||
self.setgesturemode(True)
|
||||
self.setmode(_GESTURE, False)
|
||||
|
||||
def isgestureavailable( self ) :
|
||||
v = self.read(_GSTATUS)
|
||||
return (v & _GVALID) != 0
|
||||
|
||||
def readgesture( self ) :
|
||||
if self.isgestureavailable() and (self.getmode() & 0x41) :
|
||||
while True :
|
||||
time.sleep_us(_FIFO_PAUSE_TIME)
|
||||
|
||||
gstatus = self.read(_GSTATUS)
|
||||
if (gstatus & _GVALID) == _GVALID :
|
||||
fifo_level = self.read(_GFLVL)
|
||||
if fifo_level > 0 :
|
||||
bread = self.readblock(_GFIFO_U, self._fifo_data, fifo_level * 4)
|
||||
|
||||
if bread >= 4 :
|
||||
for i in range(0, bread):
|
||||
self._udata[self.index] = self._fifo_data[i]
|
||||
self._ddata[self.index] = self._fifo_data[i + 1]
|
||||
self._ldata[self.index] = self._fifo_data[i + 2]
|
||||
self._rdata[self.index] = self._fifo_data[i + 3]
|
||||
self._index += 1
|
||||
self._total_gestures += 1
|
||||
|
||||
if self.processgesturedata() :
|
||||
self._index = 0
|
||||
self._total_gestures = 0
|
||||
else:
|
||||
time.sleep_us(_FIFO_PAUSE_TIME)
|
||||
self.decodegesture()
|
||||
self.resetgestureparams()
|
||||
return self._motion
|
||||
|
||||
return _DIR_NONE
|
||||
|
||||
def enablepower( self, aOn ) :
|
||||
self.setmode(_POWER, aOn)
|
||||
|
||||
def readlight( self, aData ) :
|
||||
l = self.read(aData)
|
||||
h = self.read(aData + 1)
|
||||
return (h << 8) | l
|
||||
|
||||
def readambientlight( self ) :
|
||||
return self.readlight(_CDATAL)
|
||||
|
||||
def readredlight( self ) :
|
||||
return self.readlight(_RDATAL)
|
||||
|
||||
def readgreenlight( self ) :
|
||||
return self.readlight(_GDATAL)
|
||||
|
||||
def readbluelight( self ) :
|
||||
return self.readlight(_BDATAL)
|
||||
|
||||
def readproximity( self ) :
|
||||
return self.read(_PDATA)
|
||||
|
||||
def resetgestureparams( self ) :
|
||||
self._index = 0
|
||||
self._total_gestures = 0
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._near_count = 0
|
||||
self._far_count = 0
|
||||
self._state = 0
|
||||
self._motion = _DIR_NONE
|
||||
|
||||
def processgesturedata( self ) :
|
||||
if self._total_gestures > 4 :
|
||||
if 0 < self._total_gestures <= 32 :
|
||||
found = False
|
||||
for i in range(0, self._total_gestures) :
|
||||
u = self._udata[i]
|
||||
d = self._ddata[i]
|
||||
l = self._ldata[i]
|
||||
r = self._rdata[i]
|
||||
if u > _THRESHOLD_OUT and d > _THRESHOLD_OUT and l > _THRESHOLD_OUT and r > _THRESHOLD_OUT :
|
||||
ufirst = u
|
||||
dfirst = d
|
||||
lfirst = l
|
||||
rfirst = r
|
||||
found = True
|
||||
break
|
||||
if found :
|
||||
for i in range(self._total_gestures - 1, -1, -1) :
|
||||
u = self._udata[i]
|
||||
d = self._ddata[i]
|
||||
l = self._ldata[i]
|
||||
r = self._rdata[i]
|
||||
ulast = u
|
||||
dlast = d
|
||||
llast = l
|
||||
rlast = r
|
||||
break
|
||||
ud_ratio_first = ((ufirst - dfirst) * 100) / (ufirst + dfirst)
|
||||
lr_ratio_first = ((lfirst - rfirst) * 100) / (lfirst + rfirst)
|
||||
ud_ratio_last = ((ulast - dlast) * 100) / (ulast + dlast)
|
||||
lr_ratio_last = ((llast - rlast) * 100) / (llast + rlast)
|
||||
|
||||
ud_delta = ud_ratio_last - ud_ratio_first
|
||||
lr_delta = lr_ratio_last - lr_ratio_first
|
||||
|
||||
self._ud_delta += ud_delta
|
||||
self._lr_delta += lr_delta
|
||||
|
||||
if self._ud_delta >= _SENSITIVITY_1 :
|
||||
self._ud_count = 1
|
||||
elif self._ud_delta <= -_SENSITIVITY_1 :
|
||||
self._ud_count = -1
|
||||
else:
|
||||
self._ud_count = 0
|
||||
|
||||
if self._lr_delta >= _SENSITIVITY_1 :
|
||||
self._lr_count = 1
|
||||
elif self._lr_delta <= -_SENSITIVITY_1 :
|
||||
self._lr_count = -1
|
||||
else:
|
||||
self._lr_count = 0
|
||||
|
||||
if self._ud_count == 0 and self._lr_count == 0 :
|
||||
if abs(ud_delta) < _SENSITIVITY_2 and abs(lr_delta) < _SENSITIVITY_2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._near_count += 1
|
||||
elif ud_delta != 0 and lr_delta != 0 :
|
||||
self._far_count += 1
|
||||
|
||||
if self._near_count >= 10 and self._far_count >= 2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._state = _NEAR_STATE
|
||||
elif ud_delta != 0 and lr_delta != 0 :
|
||||
self._state = _FAR_STATE
|
||||
|
||||
return True
|
||||
else:
|
||||
if abs(ud_delta) < _SENSITIVITY_2 and abs(lr_delta) < _SENSITIVITY_2 :
|
||||
if ud_delta == 0 and lr_delta == 0 :
|
||||
self._near_count += 1
|
||||
|
||||
if self._near_count >= 10 :
|
||||
self._ud_count = 0
|
||||
self._lr_count = 0
|
||||
self._ud_delta = 0
|
||||
self._lr_delta = 0
|
||||
|
||||
return False
|
||||
|
||||
def decodegesture( self ) :
|
||||
if self._state == _NEAR_STATE :
|
||||
self._motion = _DIR_NEAR
|
||||
return True
|
||||
elif self._state == _FAR_STATE :
|
||||
self._motion = _DIR_FAR
|
||||
return True
|
||||
|
||||
if self._ud_count == -1 and self._lr_count == 0 :
|
||||
self._motion = _DIR_UP
|
||||
elif self._ud_count == 1 and self._lr_count == 0 :
|
||||
self._motion = _DIR_DOWN
|
||||
elif self._ud_count == 0 and self._lr_count == 1 :
|
||||
self._motion == _DIR_RIGHT
|
||||
elif self._ud_count == 0 and self._lr_count == -1 :
|
||||
self._motion == _DIR_LEFT
|
||||
elif self._ud_count == -1 and self._lr_count == 1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_UP
|
||||
else:
|
||||
self._motion = _DIR_RIGHT
|
||||
elif self._ud_count == 1 and self._lr_count == -1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_DOWN
|
||||
else:
|
||||
self._motion = _DIR_LEFT
|
||||
elif self._ud_count == -1 and self._lr_count == -1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_UP
|
||||
else:
|
||||
self._motion = _DIR_LEFT
|
||||
elif self._ud_count == 1 and self._lr_count == 1 :
|
||||
if abs(self._ud_delta) > abs(self._lr_delta) :
|
||||
self._motion = _DIR_DOWN
|
||||
else:
|
||||
self._motion = _DIR_RIGHT
|
||||
else:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def getproxintlowthresh( self ) :
|
||||
return self.read(_PILT)
|
||||
|
||||
def setproxintlowthresh( self, aThresh ) :
|
||||
self.write(_PILT, aThresh)
|
||||
|
||||
def getproxinthighthresh( self ) :
|
||||
return self._read(_PIHT)
|
||||
|
||||
def setproxinthighthresh( self, aThresh ) :
|
||||
self.write(_PIHT, aThresh)
|
||||
|
||||
def getleddrive( self ) :
|
||||
v = self.read(_CONTROL)
|
||||
return (v >> 6) & 0x03
|
||||
|
||||
def setleddrive( self, aDrive ) :
|
||||
v = self.read(_CONTROL)
|
||||
aDrive &= 0x03
|
||||
aDrive <<= 6
|
||||
v &= 0x111111
|
||||
v |= aDrive
|
||||
|
||||
self.write(_CONTROL, v)
|
||||
|
||||
def getproximitygain( self ) :
|
||||
v = self.read(_CONTROL)
|
||||
return (v >> 2) & 0x03
|
||||
|
||||
def setproximitygain( self, aGain ) :
|
||||
v = self.read(_CONTROL)
|
||||
aGain &= 0x03
|
||||
aGain <<= 2
|
||||
v &= 0xF3
|
||||
v |= aGain
|
||||
self.write(_CONTROL, v)
|
||||
|
||||
def getambientlightgain( self ) :
|
||||
return self.read(_CONTROL) & 0x03
|
||||
|
||||
def setambientlightgain( self, aGain ) :
|
||||
v = self.read(_CONTROL)
|
||||
aGain &= 0x03
|
||||
v &= 0xFC
|
||||
v |= aGain
|
||||
self.write(_CONTROL, v)
|
||||
|
||||
def getledboost( self ) :
|
||||
v = self.read(_CONFIG2)
|
||||
return (v >> 4) & 0x03
|
||||
|
||||
def setledboost( self, aBoost ) :
|
||||
v = self.read(_CONFIG2)
|
||||
|
||||
aBoost &= 0x03
|
||||
aBoost <<= 4
|
||||
v &= 0xFC
|
||||
v |= aBoost
|
||||
self.write(_CONFIG2, v)
|
||||
|
||||
def getproxgaincompenable( self ) :
|
||||
v = self.read(_CONFIG3)
|
||||
return (v >> 5) & 0x01
|
||||
|
||||
def setproxgaincompenable( self, aEnable ) :
|
||||
v = self.read(_CONFIG3)
|
||||
|
||||
aEnable &= 0x01
|
||||
aEnable <<= 5
|
||||
v &= 0xDF
|
||||
v |= aEnable
|
||||
|
||||
self.write(_CONFIG3, v)
|
||||
|
||||
def setgestureenterthresh( self, aThresh ) :
|
||||
self.write(_GPENTH, aThresh)
|
||||
|
||||
def setgestureexitthresh( self, aThresh ) :
|
||||
self.write(_GEXTH, aThresh)
|
||||
|
||||
def setgesturegain( self, aGain ) :
|
||||
v = self.read(_GCONF2)
|
||||
|
||||
aGain &= 0x03
|
||||
aGain <<= 5
|
||||
v &= 0x9F
|
||||
v |= aGain
|
||||
|
||||
self.write(_GCONF2, v)
|
||||
|
||||
def setgestureleddrive( self, aDrive ) :
|
||||
v = self.read(_GCONF2)
|
||||
|
||||
aDrive &= 0x03
|
||||
aDrive <<= 3
|
||||
v &= 0xE7
|
||||
v |= aDrive
|
||||
|
||||
self.write(_GCONF2, v)
|
||||
|
||||
def setgesturewaittime( self, aTime ) :
|
||||
v = self.read(_GCONF2)
|
||||
|
||||
aTime &= 0x07
|
||||
v &= 0xF8
|
||||
v |= aTime
|
||||
|
||||
self.write(_GCONF2, v)
|
||||
|
||||
def setlightintlowthresh( self, aThresh ) :
|
||||
vl = aThresh & 0xFF
|
||||
vh = (aThresh >> 8) & 0xFF
|
||||
|
||||
self.write(_AILTL, vl)
|
||||
self.write(_AILTH, vh)
|
||||
|
||||
def setlightinthighthresh( self, aThresh ) :
|
||||
vl = aThresh & 0xFF
|
||||
vh = (aThresh >> 8) & 0xFF
|
||||
|
||||
self.write(_AIHTL, vl)
|
||||
self.write(_AIHTH, vh)
|
||||
|
||||
def setambientlightintenable( self, aEnable ) :
|
||||
v = self.read(_ENABLE)
|
||||
|
||||
aEnable &= 0x01
|
||||
aEnable <<= 4
|
||||
v &= 0xEF
|
||||
v |= aEnable
|
||||
|
||||
self.write(_ENABLE, v)
|
||||
|
||||
def setproximityintenable( self, aEnable ) :
|
||||
v = self.read(_ENABLE)
|
||||
|
||||
aEnable &= 0x01
|
||||
aEnable <<= 5
|
||||
v &= 0xDF
|
||||
v |= aEnable
|
||||
|
||||
self.write(_ENABLE, v)
|
||||
|
||||
def setgestureintenable( self, aEnable ) :
|
||||
v = self.read(_GCONF4)
|
||||
|
||||
aEnable &= 0x01
|
||||
aEnable <<= 1
|
||||
v &= 0xFD
|
||||
v |= aEnable
|
||||
|
||||
self.write(_GCONF4, v)
|
||||
|
||||
def setgesturemode( self, aMode ) :
|
||||
v = self.read(_GCONF4)
|
||||
|
||||
aMode &= 0x01
|
||||
v &= 0xFE
|
||||
v |= aMode
|
||||
|
||||
self.write(_GCONF4, v)
|
||||
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
import pyb
|
||||
|
||||
class apds:
|
||||
|
||||
_ADDRESS = const(0x39)
|
||||
# Register addresses
|
||||
REG_ENABLE = 0x80
|
||||
REG_ATIME = 0x81
|
||||
REG_WTIME = 0x83
|
||||
REG_AILTL = 0x84
|
||||
REG_AILTH = 0x85
|
||||
REG_AIHTL = 0x86
|
||||
REG_AIHTH = 0x87
|
||||
REG_PILT = 0x89
|
||||
REG_PIHT = 0x8B
|
||||
REG_PERS = 0x8C
|
||||
REG_CONFIG1 = 0x8D
|
||||
REG_PPULSE = 0x8E
|
||||
REG_CONTROL = 0x8F
|
||||
REG_CONFIG2 = 0x90
|
||||
REG_ID = 0x92
|
||||
REG_STATUS = 0x93
|
||||
REG_CDATAL = 0x94
|
||||
REG_CDATAH = 0x95
|
||||
REG_RDATAL = 0x96
|
||||
REG_RDATAH = 0x97
|
||||
REG_GDATAL = 0x98
|
||||
REG_GDATAH = 0x99
|
||||
REG_BDATAL = 0x9A
|
||||
REG_BDATAH = 0x9B
|
||||
REG_PDATA = 0x9C
|
||||
REG_POFFSET_UR = 0x9D
|
||||
REG_POFFSET_DL = 0x9E
|
||||
REG_CONFIG3 = 0x9F
|
||||
REG_GPENTH = 0xA0
|
||||
REG_GEXTH = 0xA1
|
||||
REG_GCONF1 = 0xA2
|
||||
REG_GCONF2 = 0xA3
|
||||
REG_GOFFSET_U = 0xA4
|
||||
REG_GOFFSET_D = 0xA5
|
||||
REG_GOFFSET_L = 0xA7
|
||||
REG_GOFFSET_R = 0xA9
|
||||
REG_GPULSE = 0xA6
|
||||
REG_GCONF3 = 0xAA
|
||||
REG_GCONF4 = 0xAB
|
||||
REG_GFLVL = 0xAE
|
||||
REG_GSTATUS = 0xAF
|
||||
REG_IFORCE = 0xE4
|
||||
REG_PICLEAR = 0xE5
|
||||
REG_CICLEAR = 0xE6
|
||||
REG_AICLEAR = 0xE7
|
||||
REG_GFIFO_U = 0xFC
|
||||
REG_GFIFO_D = 0xFD
|
||||
REG_GFIFO_L = 0xFE
|
||||
REG_GFIFO_R = 0xFF
|
||||
# Enable register bits
|
||||
ENABLE_GEN = 0b01000000 # Gesture enable
|
||||
ENABLE_PIEN = 0b00100000 # Proximity Interrupt Enable
|
||||
ENABLE_AIEN = 0b00010000 # ALS Interrupt Enable
|
||||
ENABLE_WEN = 0b00001000 # Wait Enable
|
||||
ENABLE_PEN = 0b00000100 # Proximity Enable
|
||||
ENABLE_AEN = 0b00000010 # ALS Enable
|
||||
ENABLE_PON = 0b00000001 # Power ON
|
||||
# Congiguration register 2
|
||||
CONFIG2_LEDBOOST_150 = (1 << 4) # LED boost 150%
|
||||
CONFIG2_LEDBOOST_200 = (2 << 4) # LED boost 200%
|
||||
CONFIG2_LEDBOOST_300 = (3 << 4) # LED boost 300%
|
||||
GCONFIG3_GDIMS_LR = 2
|
||||
GCONFIG3_GDIMS_UD = 1 # 01
|
||||
GCONFIG4_GMODE = 1 # Gesture mode
|
||||
|
||||
def __init__( self, aLoc ) :
|
||||
self.i2c = pyb.I2C(aLoc, pyb.I2C.MASTER)
|
||||
self._b1 = bytearray(1)
|
||||
self.init()
|
||||
|
||||
def read( self, aLoc ) :
|
||||
"""Read 8 bit value and return."""
|
||||
self.i2c.mem_read(self._b1, _ADDRESS, aLoc)
|
||||
# print('Read {:02x} from {:02x}.'.format(self._b1[0], aLoc))
|
||||
return self._b1[0]
|
||||
|
||||
def write( self, aLoc, aVal ) :
|
||||
"""Write 8 bit value to given address. aVal may be an int buffer."""
|
||||
self.i2c.mem_write(aVal, _ADDRESS, aLoc)
|
||||
# print('write {:02x} to {:02x}.'.format(aVal, aLoc))
|
||||
|
||||
def init( self ) :
|
||||
if self.get_device_id() != 0xAB :
|
||||
return False
|
||||
|
||||
self.write(self.REG_ENABLE, self.ENABLE_PON | self.ENABLE_PEN | self.ENABLE_GEN)
|
||||
self.write(self.REG_CONFIG2, self.CONFIG2_LEDBOOST_300)
|
||||
self.write(self.REG_GPENTH, 10)
|
||||
self.write(self.REG_GEXTH, 5)
|
||||
self.write(self.REG_GOFFSET_U, 0) #70)
|
||||
self.write(self.REG_GOFFSET_D, 0)
|
||||
self.write(self.REG_GOFFSET_L, 0) #10)
|
||||
self.write(self.REG_GOFFSET_R, 0) #34)
|
||||
self.write(self.REG_GCONF3, self.GCONFIG3_GDIMS_UD | self.GCONFIG3_GDIMS_LR)
|
||||
self.write(self.REG_GCONF4, self.GCONFIG4_GMODE)
|
||||
|
||||
def get_device_id( self ) :
|
||||
return self.read(self.REG_ID)
|
||||
|
||||
def readgesture( self ) :
|
||||
level = self.read(self.REG_GFLVL)
|
||||
if level == 0 :
|
||||
return # no data
|
||||
fifo_u = self.read(self.REG_GFIFO_U)
|
||||
fifo_d = self.read(self.REG_GFIFO_D)
|
||||
fifo_l = self.read(self.REG_GFIFO_L)
|
||||
fifo_r = self.read(self.REG_GFIFO_R)
|
||||
|
||||
return (fifo_u, fifo_d, fifo_l, fifo_r)
|
||||
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
#Driver for the bts7960 43A high power motor controller.
|
||||
|
||||
from pwm import pwm
|
||||
from pyb import Pin
|
||||
from utime import sleep_ms
|
||||
|
||||
class motor( ):
|
||||
"""Control a motor connected to the bts7960 motor controller."""
|
||||
|
||||
def __init__( self, aOnOff, aForward, aBackward ) :
|
||||
"""aOnOff is the pin to control free movement of the motor.
|
||||
aForward and aBackward are tuples indicating pin and timer channel # for PWM pins.
|
||||
aForward = (pin name, timer channel #)
|
||||
aBackward = (pin name, timer channel #)
|
||||
Need to make sure the given timer channel # is associated with the pin
|
||||
or an exception will be raised.
|
||||
#Examples:
|
||||
m1 = motor('Y1', ('Y2', 8), ('Y3', 10))
|
||||
m2 = motor('X1', ('X2', 5), ('X3', 5)) """
|
||||
self._onoff = Pin(aOnOff, Pin.OUT_PP)
|
||||
self._forward = pwm(*aForward)
|
||||
self._backward = pwm(*aBackward)
|
||||
self._speed = 0
|
||||
|
||||
self.seton( self, aOn ) :
|
||||
'''Set on/off (free wheeling) state of motor.'''
|
||||
if aOn :
|
||||
self._onoff.high()
|
||||
else:
|
||||
self._onoff.low()
|
||||
|
||||
@property
|
||||
def speed( self ) : return self._speed
|
||||
|
||||
@speed.setter
|
||||
def speed( self, aValue ) :
|
||||
'''Set velocity and direction of motor with -100 <= aValue <= 100.'''
|
||||
self._speed = aValue
|
||||
|
||||
on = False
|
||||
f = 0
|
||||
b = 0
|
||||
|
||||
if aValue < 0 :
|
||||
on = True
|
||||
f = 0
|
||||
b = min(100, -aValue)
|
||||
else:
|
||||
on = True
|
||||
f = min(100, aValue)
|
||||
b = 0
|
||||
|
||||
self.seton(on)
|
||||
self._forward.pulse_width_percent = f
|
||||
self._backward.pulse_width_percent = b
|
||||
|
||||
def brake( self ) :
|
||||
""" Brake the motor by sending power both directions, then shut it all down. """
|
||||
self._forward.pulse_width_percent = 100
|
||||
self._backward.pulse_width_percent = 100
|
||||
self.seton(True)
|
||||
sleep_ms(500)
|
||||
self.speed = 0
|
||||
|
||||
6
main.py
6
main.py
|
|
@ -56,8 +56,8 @@ def reboot( ) : pyb.hard_reset()
|
|||
|
||||
# l2082 Motor control ------------------------------------
|
||||
|
||||
# from l298n import motor
|
||||
# m1 = motor('Y1', 'Y2', ('Y3', 4))
|
||||
#from l298n import motor
|
||||
#m1 = motor('Y1', 'Y2', ('Y3', 4))
|
||||
# m2 = motor('Y5', 'Y6', ('Y4', 4))
|
||||
|
||||
# IR or SR04 distance display ----------------------------
|
||||
|
|
@ -147,11 +147,13 @@ def check():
|
|||
c = a.colordata() if cr else empty
|
||||
p = a.proximity()
|
||||
v = a.gesturedata()
|
||||
g = a.readgesture()
|
||||
if v == None :
|
||||
v = empty
|
||||
|
||||
o.clear()
|
||||
o.text((0, 10), "{:3}".format(p), 1, terminalfont)
|
||||
# o.text((0, 20), "{:3}".format(g), 1, terminalfont)
|
||||
o.text((0, 20), "{:3} {:3} {:3} {:3}".format(*v), 1, terminalfont)
|
||||
o.text((0, 30), "{:3} {:3} {:3} {:3}".format(*c), 1, terminalfont)
|
||||
o.display()
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue