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
Beispiel #2
0
    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
Beispiel #3
0
    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 ('..................')