mega MPU6050 segway

// CHRIS LONGBOTTOM BALANCING CODE FOR ARDUINO MEGA 2560 AND MPU6050, with BTS7960B controllers.
#include "Wire.h"
#include <math.h>

#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

#define LED_PIN 13
bool blinkState = false;

#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
#define M_PI 3.14159265359
#define dt 0.01
#define MAX_ANGLE 25


float angle = 0;
float angle_old = 0;
float angle_dydx = 0;
float angle_integral = 0;
float balancetorque = 0;
float rest_angle = 0;
float currentspeed = 0;
int steeringZero = 0;
int steering = 0;
int steeringTemp = 0;

float p = 8; //2
float i = 0; //0.005
float d = 1300; //1000

float gyro_integration = 0;
float xZero = 0;
int gZero = 445; //this is always fixed, hence why no initialisation routine

int L_pwmL, L_pwmR;
int R_pwmL, R_pwmR;

boolean over_angle = 0;

double accXangle, accYangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter

float distance = 0;

float gyro_offset = 0, angle_offset = 0;

int LPWM = 2; // H-bridge leg 1 ->LPWM
int enL = 8; // H-bridge enable pin 1 -> L_EN

int RPWM = 12; // H-bridge leg 2 ->RPWM
int enR = 7; // H-bridge enable pin 2 -> R_EN

int pwm = 0;

uint32_t timer;

void setup() {

  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(enL, OUTPUT);
  pinMode(enR, OUTPUT);

  digitalWrite(enL, HIGH);
  digitalWrite(enR, HIGH);
  digitalWrite(LPWM, LOW);
  digitalWrite(RPWM, LOW);

  /* LIST OF TIMERS FOR MEGA
  timer 0 (controls pin 13, 4)
  timer 1 (controls pin 12, 11)
  timer 2 (controls pin 10, 9)
  timer 3 (controls pin 5, 3, 2)
  timer 4 (controls pin 8, 7, 6)
  */

  // REGISTERS FOR TCCR1A and TCCR1B, same for others
  // TCCR1A: COM1A1 COM2A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
  // TCCR1B: ICNC1  ICES1    -    WGM13  WGM12  CS12   CS11  CS10

  TCCR1A = B01100001; // Fast PWM
  TCCR1B = B10001; //no prescalering

  OCR1A = 639; //count to 639 (16MHz/(640-1)=25 kHz)

  TCCR3A = B01100001; // Fast PWM // PHASE CORRECT (with second bit of B register)
  TCCR3B = B10001; //no prescalering

  OCR3A = 639; //count to 639 (16MHz/(640-1)=25 kHz)

    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);

    // initialize device
    //Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // configure Arduino LED for
    pinMode(LED_PIN, OUTPUT);
    if(accelgyro.testConnection())
      digitalWrite(LED_PIN,HIGH);

    gyro_offset = 0;
    angle_offset = 0;

    // GET OFFSET FOR INITIALISATION
    for(int z = 0;z<300;z++) {
      //calculatei();
      accelgyro.getAcceleration(&ax, &ay, &az);
      angle_offset += ax;
    }

    angle_offset=angle_offset/300;

}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    ax -= angle_offset;
    gx -= gyro_offset;

    accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG;
    accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG;

    if(accXangle > 180)
      accXangle = accXangle-360;

    double gyroXrate = (double)gx/131.0;
    double gyroYrate = -((double)gy/131.0);

    compAngleX = (0.98*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.02*accXangle); // Calculate the angle using a Complimentary filter
    compAngleY = (0.98*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.02*accYangle);

    float motor = 45*compAngleX + 2.2*gyroXrate;

    // MAX PWM for BTS is 25kHz
    // Max arduino write = 255...

    if(motor > 250) {
     L_pwmL = 1;
     L_pwmR = 250;
     R_pwmL = 1;
     R_pwmR = 250;

    analogWrite(RPWM,1);
    OCR1B = 639;
    digitalWrite(LPWM, LOW);

    } else if(motor < -250) {
     L_pwmL = 250;
     L_pwmR = 1;
     R_pwmL = 250;
     R_pwmR = 1;

    analogWrite(LPWM,1);
    OCR3B = 639;
    digitalWrite(RPWM, LOW);

    } else if(motor > 0) {
     L_pwmL = 1;
     L_pwmR = (int) (((motor)/255)*639);
     R_pwmL = 1;
     R_pwmR = (int) (((motor)/255)*639);

    analogWrite(RPWM,1);
    OCR1B = R_pwmR;
    digitalWrite(LPWM, LOW);

    } else {
     L_pwmL = (int) ((abs((motor))/255)*639);
     L_pwmR = 1;
     R_pwmL = (int) ((abs((motor))/255)*639);
     R_pwmR = 1;

    analogWrite(LPWM,1);
    OCR3B = L_pwmL;
    digitalWrite(RPWM, LOW);

    }


    /* POTENTAIALL REACH A MAX ANGLE???
    if(compAngleX > MAX_ANGLE) {
     Serial.println("Reached max angle FORWARD");
    } else if(compAngleX < MAX_ANGLE*-1) {
     Serial.println("Reached max angle BACKWARD");
    }
    */

    gyroXangle += gyroXrate*((double)(micros()-timer)/1000000);

    timer = micros();


    Serial.print(accXangle);    Serial.print("\t");
    Serial.print(gyroXangle);    Serial.print("\t");
    Serial.print(gyroXrate);    Serial.print("\t");
    Serial.print(compAngleX);   Serial.print("\t\t");
    Serial.print(motor);   Serial.print("\t\t");

    Serial.print(L_pwmL);   Serial.print("\t");
    Serial.print(L_pwmR);   Serial.print("\t");

    Serial.println();

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}