Gyro kamera för motorcykel (2 / 4 steg)
Steg 2: programmering
Denna krets är endast att driva servon i axel-X bara. Data från axel Y och Z är dock fortfarande krävs för respektive gyroskop och Accelerometer. Jag försökte kombinera dem genom att tillämpa Kalmanfilter beräkning för att minska "brus" utdata från gyroskop + Accelerometer så att servo rörelse är slät och inga oönskade rörelser.
KOD:
/*
GYRO kamera - saft7.com
Visar Automatisk horisontering kamera Video med Gyro & Accelerometer med Arduino
Krets:
Servo kontrolleras av Arduino, med Gyro och Accelerometer som referens av rörelse.
Skapad den 12 mars, 2013
av Firmansyah Saftari
www.saft7.com
Denna kod och hela artikeln kan hittas på:
http://www.saft7.com/
Programmeringsspråk: C ++
*/
#include < Servo.h >
Servo xservo;
#include < Wire.h >
#include "Kalman.h"
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress = 0x68; MPU6050 adress
/ * IMU Data * /
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
int moveX;
int mapX;
int correctionX;
dubbla accXangle;
dubbla accYangle;
Double gyroXangle = 9;
Double gyroYangle = 180;
Double compAngleX = 90.
Double compAngleY = 90.
dubbla kalAngleX;
dubbla kalAngleY;
uint32_t timer;
---VOID SETUP START---/
void setup() {
Serial.BEGIN(115200);
xservo.attach(10);
Wire.BEGIN();
i2cWrite(0x6B,0x00); Inaktivera viloläge
IF(i2cRead(0x75,1) [0]! = 0x68) {/ / läsa "WHO_AM_I" register
Serial.Print (F ("MPU-6050 med adress 0 x"));
Serial.Print(IMUAddress,hex);
Serial.println (F ("är inte ansluten"));
While(1);
}
kalmanX.setAngle(90); Ställ in start vinkel
kalmanY.setAngle(90);
timer = micros();
}
---VOID SETUP SLUTET---/
---OGILTIG LOOP START---/
void loop() {
/ * Uppdatera alla värden * /
uint8_t * data = i2cRead(0x3B,14);
accX = ((data [0] << 8) | data[1]);
accY = ((data [2] << 8) | data[3]);
accZ = ((data [4] << 8) | data[5]);
tempRaw = ((data [6] << 8) | data[7]);
gyroX = ((data [8] << 8) | data[9]);
gyroY = ((data [10] << 8) | data[11]);
gyroZ = ((data [12] << 8) | data[13]);
/ * Beräkna angls olika sensorer och algoritm * /
accYangle = (atan2(accX,accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY,accZ) + PI) * RAD_TO_DEG;
Double gyroXrate = (dubbel) gyroX/131.0;
Double gyroYrate =-((dubbel) gyroY/131.0);
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); Beräkna gyro vinkel utan några filter
gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); Beräkna gyro vinkel med utgångspunkt från den opartiska
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); Beräkna vinkeln med hjälp av ett gratis filter
kalAngleX = kalmanX.getAngle (accXangle, gyroXrate, (double)(micros()-timer)/1000000); Beräkna vinkeln med en Kalmanfilter
timer = micros();
mapX = karta (kalAngleX, 0, 0, 200 179); beräkna begränsning av servo mekaniska
// /////////////////////////////
correctionX = 27. REDIGERA VÄRDET FÖR SERVO KORRIGERING VINKEL
// ////////////////////////////
moveX = 270 - (kalAngleX) + correctionX;
---SKICKA SERIELLA TRYCK START---/
Serial.Print ("saft7.com X Pos:");
Serial.Print(moveX); Serial.Print("\t");
Serial.Print("\n");
---SKICKA SERIELLA UTSKRIFT SYFTE---/
---SKICKA TILL SERVO START---/
xservo.write(moveX); Signal till servo
Delay(15); fördröjning för att tillåta servon att flytta (ms)
---SKICKA TILL SERVO SLUTET---/
Delay(1); Den accelerometer är maximala prover 1kHz
}
// ---------------------- VOID LOOP END -------------- /
--FUNKTIONER START--
void i2cWrite (uint8_t registerAddress, uint8_t data) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); Skicka stopp
}
uint8_t * i2cRead (uint8_t registerAddress, uint8_t nbytes) {
uint8_t data [nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); Inte släppa bussen
Wire.requestFrom (IMUAddress, nbytes); Skicka en upprepade start och släpp sedan bussen efter behandlingen
för (uint8_t jag = 0; jag < nbytes; i ++)
data [i] = Wire.read();
returnera data;
}
--FUNKTIONER SLUT--
GYROCAM AV SAFT7.COM / /
SLUTET