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
