mirror of
https://github.com/ocogeclub/ocoge.git
synced 2024-11-21 23:29:48 +00:00
534 lines
13 KiB
Python
534 lines
13 KiB
Python
#!/usr/bin/env python
|
|
#
|
|
# GrovePi Library for using the Grove - Gesture Sensor v1.0(http://www.seeedstudio.com/depot/Grove-Gesture-p-2463.html)
|
|
#
|
|
# The GrovePi connects the Raspberry Pi and Grove sensors. You can learn more about GrovePi here: http://www.dexterindustries.com/GrovePi
|
|
#
|
|
# Have a question about this library? Ask on the forums here: http://forum.dexterindustries.com/c/grovepi
|
|
#
|
|
# History
|
|
# ------------------------------------------------
|
|
# Author Date Comments
|
|
# Karan 31 Dec 15 Initial Authoring
|
|
#
|
|
# Code derived from the basic Arduino library for the Gesture Sensor by Seeed: https://github.com/Seeed-Studio/Gesture_PAJ7620
|
|
|
|
# Released under the MIT license (http://choosealicense.com/licenses/mit/).
|
|
# For more information see https://github.com/DexterInd/GrovePi/blob/master/LICENSE
|
|
|
|
import time,sys
|
|
import RPi.GPIO as GPIO
|
|
import smbus
|
|
|
|
# use the bus that matches your raspi version
|
|
rev = GPIO.RPI_REVISION
|
|
if rev == 2 or rev == 3:
|
|
bus = smbus.SMBus(1)
|
|
else:
|
|
bus = smbus.SMBus(0)
|
|
|
|
class gesture:
|
|
#Registers and variables for the gesture sensor
|
|
GES_REACTION_TIME =.500 # 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_QUIT_TIME =1.000
|
|
|
|
BANK0 = 0
|
|
BANK1 = 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):
|
|
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, flush=True, end="")
|
|
if data0 != 0x20 :#or data1 <> 0x76:
|
|
print("Error with sensor", flush=True, end="")
|
|
#return 0xff
|
|
if data0 == 0x20:
|
|
print("wake-up finish.", flush=True, end="")
|
|
|
|
for i in range(len(self.initRegisterArray)):
|
|
self.paj7620WriteReg(self.initRegisterArray[i][0],self.initRegisterArray[i][1])
|
|
|
|
self.paj7620SelectBank(self.BANK0)
|
|
|
|
print("Paj7620 initialize register finished.", flush=True, end="")
|
|
|
|
#Write a byte to a register on the Gesture sensor
|
|
def paj7620WriteReg(self,addr,cmd):
|
|
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 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]
|
|
if data==self.GES_RIGHT_FLAG:
|
|
time.sleep(self.GES_ENTRY_TIME)
|
|
data=self.paj7620ReadReg(0x43, 1)[0]
|
|
if data == self.GES_FORWARD_FLAG:
|
|
print("1", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
print("2", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
print("3", flush=True, end="")
|
|
|
|
elif data==self.GES_LEFT_FLAG:
|
|
time.sleep(self.GES_ENTRY_TIME)
|
|
data=self.paj7620ReadReg(0x43, 1)[0]
|
|
if data == self.GES_FORWARD_FLAG:
|
|
print("1", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
print("2", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
print("4", flush=True, end="")
|
|
|
|
elif data==self.GES_UP_FLAG:
|
|
time.sleep(self.GES_ENTRY_TIME)
|
|
data=self.paj7620ReadReg(0x43, 1)[0]
|
|
if data == self.GES_FORWARD_FLAG:
|
|
print("1", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
print("2", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
print("5", flush=True, end="")
|
|
|
|
elif data==self.GES_DOWN_FLAG:
|
|
time.sleep(self.GES_ENTRY_TIME)
|
|
data=self.paj7620ReadReg(0x43, 1)[0]
|
|
if data == self.GES_FORWARD_FLAG:
|
|
print("1", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
print("2", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
print("6", flush=True, end="")
|
|
|
|
elif data==self.GES_FORWARD_FLAG:
|
|
print("1", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
|
|
elif data==self.GES_BACKWARD_FLAG:
|
|
print("2", flush=True, end="")
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
|
|
elif data==self.GES_CLOCKWISE_FLAG:
|
|
print("7", flush=True, end="")
|
|
|
|
elif data==self.GES_COUNT_CLOCKWISE_FLAG:
|
|
print("8", flush=True, end="")
|
|
|
|
else:
|
|
data1=self.paj7620ReadReg(0x44, 1)[0]
|
|
if (data1 == self.GES_WAVE_FLAG):
|
|
print("9", flush=True, end="")
|
|
|
|
#Return a vlaue from the gestire sensor which can be used in a program
|
|
# 0:nothing
|
|
# 1:Forward
|
|
# 2:Backward
|
|
# 3:Right
|
|
# 4:Left
|
|
# 5:Up
|
|
# 6:Down
|
|
# 7:Clockwise
|
|
# 8:anti-clockwise
|
|
# 9:wave
|
|
def return_gesture(self):
|
|
|
|
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 1
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
return 2
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
return 3
|
|
|
|
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 1
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
return 2
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
return 4
|
|
|
|
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 1
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
return 2
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
return 5
|
|
|
|
elif data==self.GES_DOWN_FLAG:
|
|
time.sleep(self.GES_ENTRY_TIME)
|
|
data=self.paj7620ReadReg(0x43, 1)[0]
|
|
if data == self.GES_FORWARD_FLAG:
|
|
return 1
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
elif data == self.GES_BACKWARD_FLAG:
|
|
return 2
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
else:
|
|
return 6
|
|
|
|
elif data==self.GES_FORWARD_FLAG:
|
|
return 1
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
|
|
elif data==self.GES_BACKWARD_FLAG:
|
|
return 2
|
|
time.sleep(self.GES_QUIT_TIME)
|
|
|
|
elif data==self.GES_CLOCKWISE_FLAG:
|
|
return 7
|
|
|
|
elif data==self.GES_COUNT_CLOCKWISE_FLAG:
|
|
return 8
|
|
|
|
else:
|
|
data1=self.paj7620ReadReg(0x44, 1)[0]
|
|
if (data1 == self.GES_WAVE_FLAG):
|
|
return 9
|
|
return 0
|
|
|
|
if __name__ == "__main__":
|
|
g=gesture()
|
|
g.init()
|
|
while True:
|
|
g.print_gesture()
|
|
time.sleep(.1)
|
|
# print g.return_gesture()
|
|
# time.sleep(.1)
|