From 92869fc2667fa17cbe681cd088bde2c0d9d02cc3 Mon Sep 17 00:00:00 2001 From: Guy Carver Date: Mon, 26 Feb 2018 10:31:37 -0500 Subject: [PATCH] update apds9960.py --- lib/APDS9960.py | 254 ++++++++++++++---- lib/apds.py | 669 ++++++++++++++++++++++++++++++++++++++++++++++++ lib/apds2.py | 116 +++++++++ lib/bts7960.py | 64 +++++ main.py | 6 +- 5 files changed, 1057 insertions(+), 52 deletions(-) create mode 100644 lib/apds.py create mode 100644 lib/apds2.py create mode 100644 lib/bts7960.py diff --git a/lib/APDS9960.py b/lib/APDS9960.py index 1f98bc4..07f2a72 100644 --- a/lib/APDS9960.py +++ b/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) diff --git a/lib/apds.py b/lib/apds.py new file mode 100644 index 0000000..e957c2e --- /dev/null +++ b/lib/apds.py @@ -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) + diff --git a/lib/apds2.py b/lib/apds2.py new file mode 100644 index 0000000..53a161c --- /dev/null +++ b/lib/apds2.py @@ -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) + diff --git a/lib/bts7960.py b/lib/bts7960.py new file mode 100644 index 0000000..5a378ef --- /dev/null +++ b/lib/bts7960.py @@ -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 + diff --git a/main.py b/main.py index 480dfb9..5c616d8 100644 --- a/main.py +++ b/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()