Stap 2: programmeren
Dit circuit is alleen om station servo in as-X alleen. Gegevens uit as Y en Z zijn echter nog steeds vereist voor de respectieve gyroscoop en de Accelerometer. Ik heb geprobeerd te combineren door Kalman Filter berekening te verminderen 'ruis'-uitgang van de gyroscoop + versnellingsmeter zodat servo beweging glad is en geen ongewenste beweging toe te passen.
CODE:
/*
GYRO CAMERA - saft7.com
Auto-herverdeling Camera Video demonstreert met behulp van Gyro & versnellingsmeter met Arduino
Het circuit:
Servo gecontroleerd door Arduino, met Gyro en versnellingsmeter als verwijzing verkeer.
Gemaakt van 12 maart 2013
door Firmansyah Saftari
www.saft7.com
Deze code en het volledige artikel kunnen worden gevonden op:
http://www.saft7.com/
Programming Language: C++
*/
#include < Servo.h >
Servo xservo;
#include < Wire.h >
#include "Kalman.h"
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress = 0x68; MPU6050 adres
/ * IMU gegevens * /
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;
dubbele accXangle;
dubbele accYangle;
dubbele gyroXangle = 9;
dubbele gyroYangle = 180;
dubbele compAngleX = 90;
dubbele compAngleY = 90;
dubbele kalAngleX;
dubbele kalAngleY;
uint32_t timer;
---VOID SETUP START---/
VOID Setup {}
Serial.begin(115200);
xservo.attach(10);
Wire.begin();
i2cWrite(0x6B,0x00); Slaapstand uitschakelen
if(i2cRead(0x75,1) [0]! = 0x68) {/ / lezen "WHO_AM_I" register
Serial.Print (F ("MPU-6050 met adres 0 x"));
Serial.Print(IMUAddress,hex);
Serial.println (F ('is niet verbonden"));
while(1);
}
kalmanX.setAngle(90); Set beginhoek
kalmanY.setAngle(90);
timer = micros();
}
---VOID SETUP EINDE---/
---VOID LOOP START---/
void loop {}
/ * Alle waarden bijwerken * /
uint8_t * gegevens = i2cRead(0x3B,14);
accX = ((gegevens [0] << 8) | data[1]);
accY = ((gegevens [2] << 8) | data[3]);
accZ = ((gegevens [4] << 8) | data[5]);
tempRaw = ((gegevens [6] << 8) | data[7]);
gyroX = ((gegevens [8] << 8) | data[9]);
gyroY = ((gegevens [10] << 8) | data[11]);
gyroZ = ((gegevens [12] << 8) | data[13]);
/ * Het berekenen van de angls op basis van de verschillende sensoren en algoritme * /
accYangle = (atan2(accX,accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY,accZ) + PI) * RAD_TO_DEG;
dubbele gyroXrate = (dubbele) gyroX/131.0;
dubbele gyroYrate =-((dubbele) gyroY/131.0);
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); Gyro hoek zonder een filter te berekenen
gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); Gyro hoek met behulp van de onbevooroordeelde tarief berekenen
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); Bereken de hoek met behulp van een gratis filter
kalAngleX = kalmanX.getAngle (accXangle, gyroXrate, (double)(micros()-timer)/1000000); Bereken de hoek met een Kalman-filter
timer = micros();
mapX = kaart (kalAngleX, 0, 0, 200 179); beperking van de servo mechanische berekenen
// /////////////////////////////
correctionX = 27; DEZE WAARDE VOOR SERVO CORRECTIE HOEK BEWERKEN
// ////////////////////////////
moveX = 270 - (kalAngleX) + correctionX;
---VERZENDEN SERIËLE AFDRUKKEN START---/
Serial.Print ("saft7.com X-Pos:");
Serial.Print(moveX); Serial.Print("\t");
Serial.Print("\n");
---VERZENDEN SERIËLE PRINT EINDE---/
---VERZENDEN SERVO START---/
xservo.write(moveX); Signaal afgeven aan de servo
delay(15); vertragen zodat servo's te verplaatsen (ms)
---VERZENDEN SERVO EINDE---/
delay(1); De versnellingsmeter van bedraagt maximale monsters 1kHz
}
// ---------------------- VOID LOOP END -------------- /
--FUNCTIES START--
VOID i2cWrite (uint8_t registerAddress, uint8_t gegevens) {}
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(Data);
Wire.endTransmission(); Stuur stop
}
uint8_t * i2cRead (uint8_t registerAddress, uint8_t nbytes) {}
uint8_t gegevens [nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); De bus niet vrijgeven
Wire.requestFrom (IMUAddress, nbytes); Stuur een herhaalde start en laat u vervolgens de bus na lezing
voor (uint8_t ik = 0; ik < nbytes; i ++)
gegevens [i] = Wire.read();
retourneren van gegevens;
}
--FUNCTIES EINDE--
GYROCAM DOOR SAFT7.COM / /
EINDE
