class gesture: active = 0 #Registers and variables for the gesture sensor #GES_REACTION_TIME =.500 # You can adjust the reaction time according to the actual circumstance. GES_REACTION_TIME = .250 # You can adjust the reaction time according to the actual circumstance. #GES_ENTRY_TIME =.800 # When you want to recognize the Forward/Backward gestures, your gestures' reaction time must less than GES_ENTRY_TIME(0.8s). GES_ENTRY_TIME = .400 # When you want to recognize the Forward/Backward gestures, your gestures' reaction time must less than GES_ENTRY_TIME(0.8s). #GES_QUIT_TIME =1.000 GES_QUIT_TIME = 0.500 BANK0 = 0 BANK1 = 1 bus = None busnum = -1 case = 1 PAJ7620_ADDR_BASE = 0x00 #REGISTER BANK SELECT PAJ7620_REGITER_BANK_SEL = (PAJ7620_ADDR_BASE + 0xEF) #W #DEVICE ID PAJ7620_ID = 0x73 #REGISTER BANK 0 PAJ7620_ADDR_SUSPEND_CMD = (PAJ7620_ADDR_BASE + 0x3) #W PAJ7620_ADDR_GES_PS_DET_MASK_0 = (PAJ7620_ADDR_BASE + 0x41) #RW PAJ7620_ADDR_GES_PS_DET_MASK_1 = (PAJ7620_ADDR_BASE + 0x42) #RW PAJ7620_ADDR_GES_PS_DET_FLAG_0 = (PAJ7620_ADDR_BASE + 0x43) #R PAJ7620_ADDR_GES_PS_DET_FLAG_1 = (PAJ7620_ADDR_BASE + 0x44) #R PAJ7620_ADDR_STATE_INDICATOR = (PAJ7620_ADDR_BASE + 0x45) #R PAJ7620_ADDR_PS_HIGH_THRESHOLD = (PAJ7620_ADDR_BASE + 0x69) #RW PAJ7620_ADDR_PS_LOW_THRESHOLD = (PAJ7620_ADDR_BASE + 0x6A) #RW PAJ7620_ADDR_PS_APPROACH_STATE = (PAJ7620_ADDR_BASE + 0x6B) #R PAJ7620_ADDR_PS_RAW_DATA = (PAJ7620_ADDR_BASE + 0x6C) #R #REGISTER BANK 1 PAJ7620_ADDR_PS_GAIN = (PAJ7620_ADDR_BASE + 0x44) #RW PAJ7620_ADDR_IDLE_S1_STEP_0 = (PAJ7620_ADDR_BASE + 0x67) #RW PAJ7620_ADDR_IDLE_S1_STEP_1 = (PAJ7620_ADDR_BASE + 0x68) #RW PAJ7620_ADDR_IDLE_S2_STEP_0 = (PAJ7620_ADDR_BASE + 0x69) #RW PAJ7620_ADDR_IDLE_S2_STEP_1 = (PAJ7620_ADDR_BASE + 0x6A) #RW PAJ7620_ADDR_OP_TO_S1_STEP_0 = (PAJ7620_ADDR_BASE + 0x6B) #RW PAJ7620_ADDR_OP_TO_S1_STEP_1 = (PAJ7620_ADDR_BASE + 0x6C) #RW PAJ7620_ADDR_OP_TO_S2_STEP_0 = (PAJ7620_ADDR_BASE + 0x6D) #RW PAJ7620_ADDR_OP_TO_S2_STEP_1 = (PAJ7620_ADDR_BASE + 0x6E) #RW PAJ7620_ADDR_OPERATION_ENABLE = (PAJ7620_ADDR_BASE + 0x72) #RW #PAJ7620_REGITER_BANK_SEL PAJ7620_BANK0 = 0 PAJ7620_BANK1 = 1 #PAJ7620_ADDR_SUSPEND_CMD PAJ7620_I2C_WAKEUP = 1 PAJ7620_I2C_SUSPEND = 0 #PAJ7620_ADDR_OPERATION_ENABLE PAJ7620_ENABLE = 1 PAJ7620_DISABLE = 0 #ADC, delete REG_ADDR_RESULT = 0x00 REG_ADDR_ALERT = 0x01 REG_ADDR_CONFIG = 0x02 REG_ADDR_LIMITL = 0x03 REG_ADDR_LIMITH = 0x04 REG_ADDR_HYST = 0x05 REG_ADDR_CONVL = 0x06 REG_ADDR_CONVH = 0x07 GES_RIGHT_FLAG = 1 << 0 GES_LEFT_FLAG = 1 << 1 GES_UP_FLAG = 1 << 2 GES_DOWN_FLAG = 1 << 3 GES_FORWARD_FLAG = 1 << 4 GES_BACKWARD_FLAG = 1 << 5 GES_CLOCKWISE_FLAG = 1 << 6 GES_COUNT_CLOCKWISE_FLAG = 1 << 7 GES_WAVE_FLAG = 1 << 0 #Gesture output FORWARD = 1 BACKWARD = 2 RIGHT = 3 LEFT = 4 UP = 5 DOWN = 6 CLOCKWISE = 7 ANTI_CLOCKWISE = 8 WAVE = 9 #Initial register state initRegisterArray = ([0xEF, 0x00], [0x32, 0x29], [0x33, 0x01], [0x34, 0x00], [0x35, 0x01], [0x36, 0x00], [0x37, 0x07], [0x38, 0x17], [0x39, 0x06], [0x3A, 0x12], [0x3F, 0x00], [0x40, 0x02], [0x41, 0xFF], [0x42, 0x01], [0x46, 0x2D], [0x47, 0x0F], [0x48, 0x3C], [0x49, 0x00], [0x4A, 0x1E], [0x4B, 0x00], [0x4C, 0x20], [0x4D, 0x00], [0x4E, 0x1A], [0x4F, 0x14], [0x50, 0x00], [0x51, 0x10], [0x52, 0x00], [0x5C, 0x02], [0x5D, 0x00], [0x5E, 0x10], [0x5F, 0x3F], [0x60, 0x27], [0x61, 0x28], [0x62, 0x00], [0x63, 0x03], [0x64, 0xF7], [0x65, 0x03], [0x66, 0xD9], [0x67, 0x03], [0x68, 0x01], [0x69, 0xC8], [0x6A, 0x40], [0x6D, 0x04], [0x6E, 0x00], [0x6F, 0x00], [0x70, 0x80], [ 0x71, 0x00 ], [0x72, 0x00], [0x73, 0x00], [0x74, 0xF0], [ 0x75, 0x00 ], [0x80, 0x42], [0x81, 0x44], [0x82, 0x04], [ 0x83, 0x20 ], [0x84, 0x20], [0x85, 0x00], [0x86, 0x10], [0x87, 0x00], [0x88, 0x05], [0x89, 0x18], [0x8A, 0x10], [0x8B, 0x01], [ 0x8C, 0x37 ], [0x8D, 0x00], [0x8E, 0xF0], [0x8F, 0x81], [ 0x90, 0x06 ], [0x91, 0x06], [0x92, 0x1E], [0x93, 0x0D], [ 0x94, 0x0A ], [0x95, 0x0A], [0x96, 0x0C], [0x97, 0x05], [ 0x98, 0x0A ], [0x99, 0x41], [0x9A, 0x14], [0x9B, 0x0A], [ 0x9C, 0x3F ], [0x9D, 0x33], [0x9E, 0xAE], [0x9F, 0xF9], [ 0xA0, 0x48 ], [0xA1, 0x13], [0xA2, 0x10], [0xA3, 0x08], [ 0xA4, 0x30 ], [0xA5, 0x19], [0xA6, 0x10], [0xA7, 0x08], [ 0xA8, 0x24 ], [0xA9, 0x04], [0xAA, 0x1E], [0xAB, 0x1E], [ 0xCC, 0x19 ], [0xCD, 0x0B], [0xCE, 0x13], [0xCF, 0x64], [ 0xD0, 0x21 ], [0xD1, 0x0F], [0xD2, 0x88], [0xE0, 0x01], [ 0xE1, 0x04 ], [0xE2, 0x41], [0xE3, 0xD6], [0xE4, 0x00], [ 0xE5, 0x0C ], [0xE6, 0x0A], [0xE7, 0x00], [0xE8, 0x00], [ 0xE9, 0x00 ], [0xEE, 0x07], [0xEF, 0x01], [0x00, 0x1E], [ 0x01, 0x1E ], [0x02, 0x0F], [0x03, 0x10], [0x04, 0x02], [ 0x05, 0x00 ], [0x06, 0xB0], [0x07, 0x04], [0x08, 0x0D], [ 0x09, 0x0E ], [0x0A, 0x9C], [0x0B, 0x04], [0x0C, 0x05], [ 0x0D, 0x0F ], [0x0E, 0x02], [0x0F, 0x12], [0x10, 0x02], [ 0x11, 0x02 ], [0x12, 0x00], [0x13, 0x01], [0x14, 0x05], [ 0x15, 0x07 ], [0x16, 0x05], [0x17, 0x07], [0x18, 0x01], [ 0x19, 0x04 ], [0x1A, 0x05], [0x1B, 0x0C], [0x1C, 0x2A], [ 0x1D, 0x01 ], [0x1E, 0x00], [0x21, 0x00], [0x22, 0x00], [ 0x23, 0x00 ], [0x25, 0x01], [0x26, 0x00], [0x27, 0x39], [ 0x28, 0x7F ], [0x29, 0x08], [0x30, 0x03], [0x31, 0x00], [ 0x32, 0x1A ], [0x33, 0x1A], [0x34, 0x07], [0x35, 0x07], [ 0x36, 0x01 ], [0x37, 0xFF], [0x38, 0x36], [0x39, 0x07], [ 0x3A, 0x00 ], [0x3E, 0xFF], [0x3F, 0x00], [0x40, 0x77], [ 0x41, 0x40 ], [0x42, 0x00], [0x43, 0x30], [0x44, 0xA0], [ 0x45, 0x5C ], [0x46, 0x00], [0x47, 0x00], [0x48, 0x58], [ 0x4A, 0x1E ], [0x4B, 0x1E], [0x4C, 0x00], [0x4D, 0x00], [ 0x4E, 0xA0 ], [0x4F, 0x80], [0x50, 0x00], [0x51, 0x00], [ 0x52, 0x00 ], [0x53, 0x00], [0x54, 0x00], [0x57, 0x80], [ 0x59, 0x10 ], [0x5A, 0x08], [0x5B, 0x94], [0x5C, 0xE8], [ 0x5D, 0x08 ], [0x5E, 0x3D], [0x5F, 0x99], [0x60, 0x45], [ 0x61, 0x40 ], [0x63, 0x2D], [0x64, 0x02], [0x65, 0x96], [ 0x66, 0x00 ], [0x67, 0x97], [0x68, 0x01], [0x69, 0xCD], [ 0x6A, 0x01 ], [0x6B, 0xB0], [0x6C, 0x04], [0x6D, 0x2C], [ 0x6E, 0x01 ], [0x6F, 0x32], [0x71, 0x00], [0x72, 0x01], [0x73, 0x35], [0x74, 0x00], [0x75, 0x33], [0x76, 0x31], [0x77, 0x01], [0x7C, 0x84], [0x7D, 0x03], [0x7E, 0x01]) #Enable debug message debug = 0 #Initialize the sensors def init(self, busno, caseflag): self.busnum = busno self.bus = Bus(busno) self.case = caseflag time.sleep(.001) self.paj7620SelectBank(self.BANK0) self.paj7620SelectBank(self.BANK0) data0 = self.paj7620ReadReg(0, 1)[0] data1 = self.paj7620ReadReg(1, 1)[0] if self.debug: print("data0:", data0, "data1:", data1) if data0 != 0x20: #or data1 <> 0x76: if self.debug: print("Error with sensor") #return 0xff if data0 == 0x20: if self.debug: print("wake-up finish.") for i in range(len(self.initRegisterArray)): self.paj7620WriteReg(self.initRegisterArray[i][0], self.initRegisterArray[i][1]) self.paj7620SelectBank(self.BANK0) self.active = 1 print("Paj7620 initialize register finished.") def paj7620Suspend(self): self.bus.write_word_data(self.PAJ7620_ID, 0x03, 0x01) self.bus.write_word_data(self.PAJ7620_ID, 0xEE, 0x01) #Write a byte to a register on the Gesture sensor def paj7620WriteReg(self, addr, cmd): self.bus.write_word_data(self.PAJ7620_ID, addr, cmd) #Select a register bank on the Gesture Sensor def paj7620SelectBank(self, bank): if bank == self.BANK0: self.paj7620WriteReg(self.PAJ7620_REGITER_BANK_SEL, self.PAJ7620_BANK0) #Read a block of bytes of length "qty" starting at address "addr" from the Gesture sensor def paj7620ReadReg(self, addr, qty): return self.bus.read_i2c_block_data(self.PAJ7620_ID, addr, qty) #Print the values from the gesture sensor def print_gesture(self): #data=self.paj7620ReadReg(0x43,1)[0] #data_wave=self.paj7620ReadReg(0x44, 1)[0] #if (data_wave == self.GES_WAVE_FLAG): # return "W" if self.case else "w" data = self.paj7620ReadReg(0x43, 1)[0] if data == self.GES_RIGHT_FLAG: time.sleep(self.GES_ENTRY_TIME) data = self.paj7620ReadReg(0x43, 1)[0] if data == self.GES_FORWARD_FLAG: return "F" if self.case else "f" time.sleep(self.GES_QUIT_TIME) elif data == self.GES_BACKWARD_FLAG: time.sleep(self.GES_QUIT_TIME) else: return "U" if self.case else "u" elif data == self.GES_LEFT_FLAG: time.sleep(self.GES_ENTRY_TIME) data = self.paj7620ReadReg(0x43, 1)[0] if data == self.GES_FORWARD_FLAG: return "F" if self.case else "f" time.sleep(self.GES_QUIT_TIME) elif data == self.GES_BACKWARD_FLAG: print(ba) time.sleep(self.GES_QUIT_TIME) else: return "D" if self.case else "d" elif data == self.GES_UP_FLAG: time.sleep(self.GES_ENTRY_TIME) data = self.paj7620ReadReg(0x43, 1)[0] if data == self.GES_FORWARD_FLAG: return "F" if self.case else "f" time.sleep(self.GES_QUIT_TIME) elif data == self.GES_BACKWARD_FLAG: return "B" if self.case else "b" time.sleep(self.GES_QUIT_TIME) else: return "L" if self.case else "l" elif data == self.GES_DOWN_FLAG: time.sleep(self.GES_ENTRY_TIME) data = self.paj7620ReadReg(0x43, 1)[0] if data == self.GES_FORWARD_FLAG: time.sleep(self.GES_QUIT_TIME) return "F" if self.case else "f" elif data == self.GES_BACKWARD_FLAG: return "B" if self.case else "b" time.sleep(self.GES_QUIT_TIME) else: return "R" if self.case else "r" elif data == self.GES_FORWARD_FLAG: return "F" if self.case else "f" time.sleep(self.GES_QUIT_TIME) elif data == self.GES_BACKWARD_FLAG: return "B" if self.case else "b" time.sleep(self.GES_QUIT_TIME) elif data == self.GES_CLOCKWISE_FLAG: return "C" if self.case else "c" elif data == self.GES_COUNT_CLOCKWISE_FLAG: return "A" if self.case else "a" #else: # data1=self.paj7620ReadReg(0x44, 1)[0] # if (data1 == self.GES_WAVE_FLAG): # return "W" if self.case else "w" def _gesture(self): data = self.paj7620ReadReg(0x43, 1)[0] if data == 1: return "U" if self.case else "u" elif data == 2: return "D" if self.case else "d" elif data == 16: return "F" if self.case else "f" elif data == 4: return "L" if self.case else "l" elif data == 32: return "B" if self.case else "b" elif data == 128: return "A" if self.case else "a" elif data == 64: return "C" if self.case else "c" elif data == 8: return "R" if self.case else "r"
class I2CStepperMotor(StepperMotor): __REG_GET_PID = 0x00 __REG_GET_VID = 0x01 __REG_GET_VER = 0x02 __REG_STP_EN = 0x1A __REG_STP_DIS = 0x1B __REG_STP_RUN = 0x1C __REG_STP_INTERVAL = 0x1D __REG_SEQ_LEN = 0x20 __REG_SEQ_XET = 0x21 __REG_SET_SPEED = 0x82 __REG_SET_FREQ = 0x84 __REG_SET_A = 0xA1 __REG_SET_B = 0xA5 __REG_SET_DIR = 0xAA def __init__(self, arguments, address=0x0F): super(I2CStepperMotor, self).__init__(arguments) self._addr = address self._bus = Bus() self._ang_left = 0 self._load_seq(arguments["sequences"]) def __del__(self): self.set_speed(0, 0) pass #Maps speed from 0-100 to 0-255 def _map_vals(self, value, leftMin, leftMax, rightMin, rightMax): #http://stackoverflow.com/questions/1969240/mapping-a-range-of-values-to-another # Figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # Convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # Convert the 0-1 range into a value in the right range. return int(rightMin + (valueScaled * rightSpan)) #Set motor speed def set_speed(self, speed1=0, speed2=0): s1 = self._map_vals(speed1, 0, 100, 0, 255) s2 = self._map_vals(speed2, 0, 100, 0, 255) self._bus.write_i2c_block_data(self._addr, self.__REG_SET_SPEED, [s1, s2]) time.sleep(.02) #Set motor direction def set_dir(self, clock_wise1=True, clock_wise2=True): dir1 = 0b10 if clock_wise1 else 0b01 dir2 = 0b10 if clock_wise2 else 0b01 dir = (dir2 << 2) | dir1 self._bus.write_i2c_block_data(self._addr, self.__REG_SET_DIR, [dir, 0]) time.sleep(.02) def _load_seq(self, seq): length = len(seq) self._bus.write_word_data(self._addr, self.__REG_SEQ_LEN, length) for i in range(len(seq)): self._bus.write_word_data(self._addr, self.__REG_SEQ_XET, seq[i]) def _enable(self, en): cmd = self.__REG_STP_EN if en else self.__REG_STP_DIS if en: self.set_speed(self.DC_SPEED_MAX, self.DC_SPEED_MAX) else: self.set_speed(0, 0) self._bus.write_i2c_block_data(self._addr, cmd, [self._dir, 0]) time.sleep(0.001) def _speed(self, rpm): self._dir = self._DIR_CLKWISE if rpm >= 0 else self._DIR_ANTI_CLKWISE # absolute angle per second aps = abs(rpm) * 360.0 / 60.0 # steps per second, include reductor ratio. sps = self._angle2steps(aps) period = int(1000000 / sps) # us period = period // 10 # STP_INTERVAL, 10 us # print("period = %d us" % (period * 10)) self._bus.write_word_data(self._addr, self.__REG_STP_INTERVAL, period) time.sleep(0.001) def _rotate(self, angle=None): if not angle is None: angle = abs(angle) self._ang_left = angle steps = self._angle2steps(angle) # print("steps set = {}".format(steps)) self._bus.write_word_data(self._addr, self.__REG_STP_RUN, steps) time.sleep(0.001) return angle while True: # reading interface unstable when working try: steps = self._bus.read_word_data(self._addr, self.__REG_STP_RUN) # print("steps left = {}".format(steps)) ang_left = self._steps2angle(steps) if ang_left > self._ang_left: time.sleep(0.01) continue self._ang_left = ang_left return ang_left except IOError: continue