Paso 2: programación
Este circuito es la servo de la impulsión en el eje-X sólo. Sin embargo, datos del eje Y y Z son todavía necesarios para los respectivos giroscopio y acelerómetro. He intentado combinar aplicando cálculo del filtro de Kalman para reducir la salida de 'ruido' de giroscopio + acelerómetro para que el movimiento del servo es lisa y sin movimiento no deseado.
CÓDIGO:
/*
GIRO de cámara - saft7.com
Muestra vídeo de cámara de auto nivelación mediante giroscopio y acelerómetro con Arduino
El circuito:
Servo controlado por Arduino, con giroscopio y acelerómetro como referencia del movimiento.
Creado el 12 de marzo de 2013
por Firmansyah Saftari
www.saft7.com
Este código y el artículo completo pueden encontrarse en:
http://www.saft7.com/
Lenguaje de programación: C++
*/
#include < Servo.h >
Servo xservo;
#include < Wire.h >
#include "Kalman.h"
KalmanX Kalman;
KalmanY Kalman;
uint8_t IMUAddress = 0x68; Dirección MPU6050
/ * Datos IMU * /
accX int16_t;
int16_t Accy de la presión;
accZ int16_t;
tempRaw de int16_t;
int16_t Gyrox, S.l.;
int16_t gyroY;
int16_t gyroZ;
moveX int;
int mapX;
int correctionX;
doble accXangle;
doble accYangle;
doble gyroXangle = 9;
doble gyroYangle = 180;
compAngleX doble = 90;
compAngleY doble = 90;
doble kalAngleX;
doble kalAngleY;
temporizador de uint32_t;
---INICIO VOID SETUP---/
void setup() {}
Serial.Begin(115200);
xservo.Attach(10);
Wire.Begin();
i2cWrite(0x6B,0x00); Desactivar el modo de sueño
if(i2cRead(0x75,1) [0]! = 0x68) {/ / leer el registro de "WHO_AM_I"
Serial.Print (F ("MPU-6050 con dirección 0 x"));
Serial.Print(IMUAddress,HEX);
Serial.println (F ("no está conectado"));
while(1);
}
kalmanX.setAngle(90); Ángulo inicial del sistema
kalmanY.setAngle(90);
contador = micros();
}
---CONFIGURACIÓN VACÍO FINAL---/
---INICIO DE BUCLE VACÍO---/
void loop() {}
/ * Actualizar todos los valores * /
uint8_t * datos = i2cRead(0x3B,14);
accX = ((datos [0] << 8) | data[1]);
Accy de la presión = ((datos [2] << 8) | data[3]);
accZ = ((datos [4] << 8) | data[5]);
tempRaw = ((datos [6] << 8) | data[7]);
Gyrox, S.l. = ((datos [8] << 8) | data[9]);
gyroY = ((datos [10] << 8) | data[11]);
gyroZ = ((datos [12] << 8) | data[13]);
/ * Calcular los angls basado en el algoritmo y diferentes sensores * /
accYangle = (atan2(accX,accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY,accZ) + PI) * RAD_TO_DEG;
doble gyroXrate = Gyrox, S.l./131.0 (doble);
doble gyroYrate =-((doble) gyroY/131.0);
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); Calcular el ángulo de giro sin ningún filtro
gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); Calcular el ángulo de giro utilizando la tasa de imparcial
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); Calcular el ángulo con un filtro gratuito
kalAngleX = kalmanX.getAngle (accXangle, gyroXrate, (double)(micros()-timer)/1000000); Calcular el ángulo con un filtro de Kalman
contador = micros();
mapX = mapa (kalAngleX, 200, 0, 0, 179); Calcule la limitación de servo mecánico
// /////////////////////////////
correctionX = 27; MODIFICAR ESTE VALOR PARA EL ÁNGULO DE CORRECCIÓN DE SERVO
// ////////////////////////////
moveX = 270 - (kalAngleX) + correctionX;
---ENVIAR SERIAL INICIO IMPRESIÓN---/
Serial.Print ("saft7.com X Pos:");
Serial.Print(moveX); Serial.Print("\t");
Serial.Print("\n");
---ENVIAR A SERIE FINAL IMPRESIÓN---/
---ENVIAR A SERVO INICIO---/
xservo.Write(moveX); Enviar la señal al servo
Delay(15); retraso para permitir que los servos mover (ms)
---ENVIAR A SERVO FINAL---/
Delay(1); Tasa de muestras máximo del acelerómetro es 1kHz
}
// ---------------------- VOID LOOP END -------------- /
--INICIO FUNCIONES--
i2cWrite vacío (uint8_t registerAddress, uint8_t datos) {}
Wire.beginTransmission(IMUAddress);
Wire.Write(registerAddress);
Wire.Write(Data);
Wire.endTransmission(); Enviar stop
}
i2cRead * de uint8_t (uint8_t registerAddress, uint8_t nbytes) {}
datos de uint8_t [nbytes];
Wire.beginTransmission(IMUAddress);
Wire.Write(registerAddress);
Wire.endTransmission(false); No suelte el autobús
Wire.requestFrom (IMUAddress, nbytes); Enviar un comienzo repetido y luego suelte el autobús después de la lectura
para (uint8_t i = 0; i < nbytes; i ++)
datos [i] = Wire.read();
devolver datos;
}
--FINAL DE FUNCIONES--
GYROCAM POR SAFT7.COM / /
FINAL
