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)