1 ARDUINO PRO MINI 5V  – [beli disini]

1 MODULE 3 AXIS ACCELEROMETER + GYRO SENSORS (3V-5V)  – [beli disini]

2 Unit – Servo Ultra Micro Servo – [beli disini]

2 Trimmer Potentiometer (trimpot) 10k ohm

Plat alumuniun 0.5 mm secukup nya unt membuat arm gimbal

1 FTDI Basic Programmer (punya sendiri/pinjam orang)  – [beli disini]

1 Project board (buat testing)

1 set kabel jumper secukupnya

1 Foam doble tape yg agak tebal tapi empuk (sebagai shock absorber)

/*
GYRO GIMBAL CAMERA AR.Drone
Berikut revisi ke 1 :
log perubahan ada di file kalman.h:
“merubah variable measure dan bias untuk
mengurangi noise getaran yg terlalu sensitif”
*/

#include <Servo.h>
Servo xservo;  // create servo objects to control the servos
Servo yservo;
Servo zservo;

#include <Wire.h>

/*   create Kalman filters objects to eliminates noise from gyro and accelerometer   */
#include “Kalman.h”
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

/*   Correction potentiometer pins   */
int xpotpin = 1;
int ypotpin = 2;
int zpotpin = 3;
int xpotcor;
int ypotcor;
int zpotcor;

/*   variable servo   */
int moveX;
int moveY;
int moveZ;
int correctionX;
int correctionY;
int correctionZ;

/*   Correction angle   */
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double accZangle;
double temp;
double compAngleX = 90;       // Calculate the angle using a Kalman filter
double compAngleY = 90;
double compAngleZ = 90;
double kalAngleX;             // Calculate the angle using a Kalman filter
double kalAngleY;
double kalAngleZ;

uint32_t timer;

// ———-  VOID SETUP START ————– /

void setup() {
Serial.begin(115200);
xservo.attach(8);  // Assign servo on pin 8 to the xservo object
yservo.attach(9);  // Assign servo on pin 9 to the yservo object
zservo.attach(10);  // Assign servo on pin 10 to the zservo object

Wire.begin();
i2cWrite(0x6B,0x00);                             // Disable sleep mode
if(i2cRead(0x75,1)[0] != 0x68) {                 // Read “WHO_AM_I” register
Serial.print(F(“MPU-6050 with address 0x”));
Serial.print(IMUAddress,HEX);
Serial.println(F(” is not connected”));
while(1);
}
kalmanX.setAngle(90);                           // Set starting angle
kalmanY.setAngle(90);
kalmanZ.setAngle(90);
timer = micros();
}
// ———-  VOID SETUP END ————– /

// ———————- VOID LOOP START ————– /

void loop() {
/* Update all the values */
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]);

/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accZangle = (atan2(accZ,accY)+PI)*RAD_TO_DEG;

/*   pembagi dibawah bisa di setting utk menyesuaikan rasio gerakan servo, lebih besar akan lebih pelan   */
double gyroXrate = (double)gyroX/80.0;
double gyroYrate = -((double)gyroY/80.0);
double gyroZrate = -((double)gyroZ/80.0);

/*  Calculate the angle using a Kalman filter   */
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
timer = micros();

/*  baca value dari potensio meter untuk trim sumbu X,Y,Z   */
xpotcor = (analogRead(xpotpin))*0.1;
ypotcor = (analogRead(ypotpin))*0.1;
zpotcor = (analogRead(zpotpin))*0.1;

correctionX = xpotcor;
correctionY = ypotcor;
correctionZ = zpotcor;

/*  tentukan arah motor servo   */
moveX = 200 – (kalAngleX) + correctionX;
moveY = 240 – (kalAngleY) + correctionY;
moveZ = 200 – (kalAngleZ) + correctionZ;

/* fungsi penggerak motor servo   */
xservo.write(moveX);
yservo.write(moveY);
//zservo.write(moveZ);

/* debug print ke comm   */
//  Serial.print(moveX);Serial.print(”    —-    “);Serial.print(moveY);Serial.print(”    —-    “);Serial.print(moveZ);Serial.print(“\n”);

// delay(1); // The accelerometer’s maximum samples rate is 1kHz
}

// ———————- VOID LOOP END ————– /

// — FUNCTIONS START — /

void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don’t release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data[i] = Wire.read();
return data;
}

// — FUNCTIONS END — /

[/stextbox] [stextbox id=”grey” caption=”I2C.ino” collapsing=”true” collapsed=”true”] //const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
Serial.print(F(“i2cWrite failed: “));
Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don’t release the bus
if (rcode) {
Serial.print(F(“i2cRead failed: “));
Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() – timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F(“i2cRead timeout”));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}

[/stextbox] [stextbox id=”grey” caption=”kalman.h” collapsing=”true” collapsed=”true”] #ifndef _Kalman_h
#define _Kalman_h

class Kalman {
public:
Kalman() {
/* We will set the variables like so, these can also be tuned by the user */
Q_angle = 0.001;
Q_bias = 0;
R_measure = 1;

angle = 0; // Reset the angle
bias = 0; // Reset bias

P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so – see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0;
P[1][0] = 0;
P[1][1] = 0;
};
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
double getAngle(double newAngle, double newRate, double dt) {
// KasBot V2  –  Kalman filter module – http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

// Discrete Kalman filter time update equations – Time Update (“Predict”)
// Update xhat – Project the state ahead
/* Step 1 */
rate = newRate – bias;
angle += dt * rate;

// Update estimation error covariance – Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] – P[0][1] – P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;

// Discrete Kalman filter measurement update equations – Measurement Update (“Correct”)
// Calculate Kalman gain – Compute the Kalman gain
/* Step 4 */
S = P[0][0] + R_measure;
/* Step 5 */
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

// Calculate angle and bias – Update estimate with measurement zk (newAngle)
/* Step 3 */
y = newAngle – angle;
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;

// Calculate estimation error covariance – Update the error covariance
/* Step 7 */
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];

return angle;
};
void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
double getRate() { return rate; }; // Return the unbiased rate

/* These are used to tune the Kalman filter */
void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
void setRmeasure(double newR_measure) { R_measure = newR_measure; };

double getQangle() { return Q_angle; };
double getQbias() { return Q_bias; };
double getRmeasure() { return R_measure; };

private:
/* Kalman filter variables */
double Q_angle; // Process noise variance for the accelerometer
double Q_bias; // Process noise variance for the gyro bias
double R_measure; // Measurement noise variance – this is actually the variance of the measurement noise

double angle; // The angle calculated by the Kalman filter – part of the 2×1 state vector
double bias; // The gyro bias calculated by the Kalman filter – part of the 2×1 state vector
double rate; // Unbiased rate calculated from the rate and the calculated bias – you have to call getAngle to update the rate

double P[2][2]; // Error covariance matrix – This is a 2×2 matrix
double K[2]; // Kalman gain – This is a 2×1 vector
double y; // Angle difference
double S; // Estimate error
};

#endif