def accelerometer():
serimu = globalv.serimu()
flagx = 0
flagy = 0
flagz = 0
serimu.flush()
while flagx == 0 or flagy == 0 or flagz == 0:
serimu.write(cmd_accelero)
t = time()
while abs(t - time() < 0.01) and (flagx == 0 or flagy == 0
or flagz == 0):
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("accelp = 00--"):
ax = line.split("--")
ax = float(ax[1])
#print "ax : "+str(ax)
flagx = 1
if line.startswith("01--"):
ay = line.split("--")
ay = float(ay[1])
#print "ay : "+str(ay)
flagy = 1
if line.startswith("02--"):
az = line.split("--")
az = float(az[1])
#print "az : "+str(az)
flagz = 1
a = [ax, ay, az]
return a
def gyro():
serimu = globalv.serimu()
flagx = 0
flagy = 0
flagz = 0
serimu.flush()
while flagx == 0 or flagy == 0 or flagz == 0:
serimu.write(cmd_gyro)
t = time()
while abs(t - time() < 0.01) and (flagx == 0 or flagy == 0
or flagz == 0):
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("gyrop = 00--"):
gx = line.split("--")
gx = float(gx[1])
#print "gx : "+str(gx)
flagx = 1
if line.startswith("01--"):
gy = line.split("--")
gy = float(gy[1])
#print "gy : "+str(gy)
flagy = 1
if line.startswith("02--"):
gz = line.split("--")
gz = float(gz[1])
#print "gz : "+str(gz)
flagz = 1
g = [gx, gy, gz]
return g
def pitch():
serimu = globalv.serimu()
flag = 0
serimu.flush()
while flag == 0:
serimu.write(cmd_pitch)
t = time()
while abs(t - time() < 0.01) and flag == 0:
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("pitch ="):
pitch = line.split("=")
pitch = float(pitch[1])
#print "pitch : "+str(pitch)
flag = 1
return pitch
def roll():
serimu = globalv.serimu()
flag = 0
serimu.flush()
while flag == 0:
serimu.write(cmd_roll)
t = time()
while abs(t - time() < 0.01) and flag == 0:
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("roll ="):
roll = line.split("=")
roll = float(roll[1])
#print "roll : "+str(roll)
flag = 1
return roll
def yaw():
serimu = globalv.serimu()
flag = 0
serimu.flush()
while flag == 0:
serimu.write(cmd_yaw)
t = time()
while abs(t - time() < 0.01) and flag == 0:
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("yaw ="):
yaw = line.split("=")
yaw = float(yaw[1])
#print "yaw : "+str(yaw)
flag = 1
return yaw
def temperature():
serimu = globalv.serimu()
flag = 0
serimu.flush()
while flag == 0:
serimu.write(cmd_temp)
t = time()
while abs(t - time() < 0.01) and flag == 0:
read = serimu.readline()
for line in read.split('\r'):
if line.startswith("temperature ="):
temp = line.split("=")
temp = float(temp[1])
#print "temperature : "+str(temp)
flag = 1
return temp
import serial
import datetime
import time
import globalv
import string
import numpy
serimu = globalv.serimu()
#Utility constants
cmd_yaw = "yaw di.\r\n"
cmd_roll = "roll di.\r\n"
cmd_pitch = "pitch di.\r\n"
cmd_accelero = "accelp di.\r\n"
cmd_gyro = "gyrop di.\r\n"
cmd_temp = "temperature di.\r\n"
radius = 0.85
#Error Variables
error_yaw=-0.0
error_pitch=0.0
error_ax=-0.0
error_ay=-0.0
error_az=0.0
error_roll=-0.0
error_temp=0.0
#Utility function
def time1():
t = datetime.datetime.now()
t = str(t)