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 ('..................')