我一直致力于为Raspberry pi的瘫痪人们开展一个项目。我写了一个脚本来从传感器中获取值。
我在这里使用IMU进行偏航,俯仰和翻滚值,我得到了它们。
我的问题是我想要一个python代码,它可以根据模式中重复信号的次数来调用不同的函数。如果模式出现,应该执行一些给定的函数。
到目前为止,我的代码如下所示
import serial
import time
import lcd
import math
import string
lcd.lcd_init()
lcd.stringlcd(0xC0,"welcome")
default_port='/dev/ttyUSB0'
#port = rospy.get_param('device', default_port)
# Check your COM port and baud rate
ser = serial.Serial(port=default_port,baudrate=9600, timeout=1)
##ser = serial.Serial('/dev/ttyUSB0', 9600,)#get data fromserial port(IMU)
grad2rad = 3.141592/180.0
roll=0
pitch=0
yaw=0
#rospy.sleep(5) # Sleep for 8 seconds to wait for the board to boot then only write command.
ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text
while 1:
line = ser.readline()
line = line.replace("#YPR=","")
line = line.replace("#YPRAMG=","") # Delete "#YPR="
#f.write(line) # Write to the output log file
words = string.split(line[4:],",") # Fields split
#wort5 = float(words[5])
if len(words) > 2:
try:
#yaw = float(words[0])*grad2rad
pitch = -float(words[1])*grad2rad
roll = -float(words[2])*grad2rad
if (pitch > 1):
lcd.stringlcd(0x80,"right")
print ("right")
elif (pitch < -0.5):
lcd.stringlcd(0x80,"left")
print ("left")
if (roll >0.5):
lcd.stringlcd(0x80,"forward")
print ("forward")
elif(roll < -0.5):
lcd.stringlcd(0x80,"resting")
print ("resting")
time.sleep(1)
ser.flushInput()
except Exception as chk:enter code here
print chk