print("Exiting....")
exit(1)
else:
sp.setDataRate(16)
#print sp.spatialData()
print("zeroing gyro...")
sp.zeroGyro()
time.sleep(2.2)
north = 0
for i in range(10000):
try:
print
gravity = sp.getAngularRate([0][0]), sp.getAngularRate(
[1][0]), sp.getAngularRate([2][0])
print "acc:", gravity
print "AngularRate:", sp.getAngularRate([0][0]), sp.getAngularRate(
[1][0]), sp.getAngularRate([2][0])
magField = sp.getMagneticField([0][0]), sp.getMagneticField(
[1][0]), sp.getMagneticField([2][0])
print "MagneticField:", magField
#print "compass = "******"north: ", north, (north / i) , i
#compassBearing = compass* (180.0 / math.pi);
#if (compassBearing > 360): compassBearing -= 360;
#if (compassBearing < 0): compassBearing += 360;
#print "compass: ", compass
while not kbhit() or getch() != " ":
#Medimos los valores actuales de los acelerometros en g=9.8 m/s**2
accX = spatial.getAcceleration(0)
accY = spatial.getAcceleration(1)
accZ = spatial.getAcceleration(2)
#Calculamos el giro (radianes) que nos dan los acelerometros
rotacionX = atan2(accY, accZ) #Roll
rotacionY = atan(-accX / sqrt(accY**2 + accZ**2)) #Pitch
rotacionXGrados = rotacionX * 180 / pi
rotacionYGrados = rotacionY * 180 / pi
#medimos los valores actuales de los giroscopos (en grados/s)
gyroX = spatial.getAngularRate(0)
gyroY = spatial.getAngularRate(1)
#Leemos el reloj del procesador para calcular más tarde el intervalo de muestreo
IntervaloTiempo0 = clock()
#(Gyro * intervalo muestreo (dt)) nos da el valor del giro medido por el giroscopo en grados
gyroXDelta = gyroX * IntervaloTiempo
gyroYDelta = gyroY * IntervaloTiempo
#Pasamos los datos de los acelerometros y el giroscopio por un Filtro Complementario
UltimoX = K * (UltimoX + gyroXDelta) + K1 * rotacionXGrados
UltimoY = K * (UltimoY + gyroYDelta) + K1 * rotacionYGrados
#Utilizamos el angulo calculado para el control PID
Ix = Ix + UltimoX * KI
print("Exiting....")
exit(1)
else:
sp.setDataRate(16)
#print sp.spatialData()
print ("zeroing gyro...")
sp.zeroGyro()
time.sleep(2.2)
north = 0
for i in range (10000):
try:
print
gravity = sp.getAngularRate([0][0]), sp.getAngularRate([1][0]), sp.getAngularRate([2][0])
print "acc:", gravity
print "AngularRate:", sp.getAngularRate([0][0]), sp.getAngularRate([1][0]), sp.getAngularRate([2][0])
magField = sp.getMagneticField([0][0]), sp.getMagneticField([1][0]), sp.getMagneticField([2][0])
print "MagneticField:", magField
#print "compass = "******"north: ", north, (north / i) , i
#compassBearing = compass* (180.0 / math.pi);
#if (compassBearing > 360): compassBearing -= 360;
#if (compassBearing < 0): compassBearing += 360;
#print "compass: ", compass
except:
print ('..................')