#include<Wire.

h>
#include<I2Cdev.h>
#include<MPU6050.h>
#ifndef _Kalman_h_
#define _Kalman_h_
float speeds;
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
}
class Kalman {
public:
Kalman();
 // The angle should be in degrees and the rate should be in degrees per second
and the delta time in seconds
float getAngle(float newAngle, float newRate, float dt);

void setAngle(float angle); // Used to set angle, this should be set as the
starting angle
float getRate(); // Return the unbiased rate

/* These are used to tune the Kalman filter */

void setQangle(float Q_angle);
/**
* setQbias(float Q_bias)
* Default value (0.003f) is in Kalman.cpp.
* Raise this to follow input more closely,
* lower this to smooth result of kalman filter.
*/
void setQbias(float Q_bias);
void setRmeasure(float R_measure);

float getQangle();
float getQbias();
float getRmeasure();

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

float angle; // The angle calculated by the Kalman filter - part of the 2x1
state vector
float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1
state vector
float rate; // Unbiased rate calculated from the rate and the calculated bias -
you have to call getAngle to update the rate

float P[2][2]; // Error covariance matrix - This is a 2x2 matrix

};

#endif
MPU6050 mpu;

int IN1 = 4;
int IN2 = 8;
int IN3 = 5;
int IN4 = 7;
#define enA 3
#define enB 6
int16_t ay, az;
int16_t gx;
float a_goc;
float g_goc;
float KalAngleX;
float er, per = 0, ier =0;
unsigned long timer;
float goc;
uint8_t i2cData[14];
const float Kp = 30;
const float Ki = 1;
const float Kd = 8;
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis()
- _lasttime) > (t);_lasttime += (t))
void canbang(int left, int right)
{
if(left>=0)
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(enA,left);
}
else{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(enA, 255+left);
}
if(right>=0)
{
digitalWrite(IN4, LOW);
digitalWrite(IN3, HIGH);
analogWrite(enB, right);
}
else{
digitalWrite(IN4,HIGH);
digitalWrite(IN3, LOW);
analogWrite(enB, 255+right);
}
}
void stop()
{
speeds = map(speeds,0,-150,0,150);
analogWrite(enA, speeds);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(enB, speeds);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
}

void setup()
{
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(13, OUTPUT);
Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro
filtering, 8 KHz sampling
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false));
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
 if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100);
//Kalman====================================================
while(i2cRead(0x3B,i2cData,6));
ay = ((i2cData[2] << 8) | i2cData[3]);
az = ((i2cData[4] << 8) | i2cData[5]);
a_goc = (atan2(ay,az)+PI)*RAD_TO_DEG;
Kalman().setAngle(a_goc);
g_goc = a_goc;
timer = micros();
}
void loop()
{
Serial.println(ay);
Serial.println(az);
Serial.println(a_goc);
Serial.println(goc);
runEvery(25)
{
kal();
if(goc<=179&&goc>=178.5)
{
stop();
}
else{
if(goc<230&&goc>130)
{
pid();
canbang(speeds,speeds);

}
else{
stop();
}
}
}
}

void pid()
{
er = 180 - goc;
ier += er ;
speeds=constrain((Kd*er + Ki*er*0.005 + Kd*(er- per)/0.005), -255, 255);
per = er;
}
void kal()
{
while(i2cRead(0x3B,i2cData,14));

ay = ((i2cData[2] << 8) | i2cData[3]);

az = ((i2cData[4] << 8) | i2cData[5]);

gx = ((i2cData[8] << 8) | i2cData[9]);

a_goc = (atan2(ay,az)+PI)*RAD_TO_DEG;
float gyroXrate = (double)gx/131.0;
 goc = Kalman().getAngle(a_goc, gyroXrate, (float)(micros()-timer)/1000000);
timer = micros();
}