// This is the code we use to test the motors of the keyesstudio Self-balancing car kit.
// Source: https://wiki.keyestudio.com/Ks0193_keyestudio_Self-balancing_Car
// Code Modified by Addison Sears-Collins at https://automaticaddison.com

// Digital Pins 8 and 12 control the right motor's direction.
// Speed is controlled by Digital Pin 10.
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;

// Digital Pins 7 and 6 control the left motor's direction.
// Speed is controlled by Digital Pin 9.
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;


void setup() 
{
  //set the baud rate to 9600
  Serial.begin(9600);          

  // Set all the pins to OUTPUT
  // The motors' speed and direction is controlled by 
  // the output of these pins
  pinMode(right_R1,OUTPUT);     
  pinMode(right_R2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_L,OUTPUT);
}

void loop() 
{
  // Go forward for 3 seconds
  digitalWrite(right_R1,HIGH); 
  digitalWrite(right_R2,LOW);
  digitalWrite(left_L1,LOW);
  digitalWrite(left_L2,HIGH);
  analogWrite(PWM_R,100);   // write into PWM value 0~255（speed）
  analogWrite(PWM_L,100);

  delay(3000);

   // Stop for 3 seconds
  digitalWrite(right_R1,HIGH); 
  digitalWrite(right_R2,HIGH);
  digitalWrite(left_L1,HIGH);
  digitalWrite(left_L2,HIGH);
  analogWrite(PWM_R,100);   // write into PWM value 0~255（speed）
  analogWrite(PWM_L,100);

  delay(3000);

   // Reverse for 3 seconds
  digitalWrite(right_R1,LOW); 
  digitalWrite(right_R2,HIGH);
  digitalWrite(left_L1,HIGH);
  digitalWrite(left_L2,LOW);
  analogWrite(PWM_R,100);   // write into PWM value 0~255（speed）
  analogWrite(PWM_L,100);   // The higher the PWM value, the faster the speed

  delay(3000);

  // Stop for 3 seconds
  // Notice that we can do all LOW or all HIGH to stop the robot.
  digitalWrite(right_R1,LOW); 
  digitalWrite(right_R2,LOW);
  digitalWrite(left_L1,LOW);
  digitalWrite(left_L2,LOW);
  analogWrite(PWM_R,100);   // write into PWM value 0~255（speed）
  analogWrite(PWM_L,100);

  delay(3000);
  
}

// This is the code we use to test the hall encoder on the Keyesstudio self-balancing car
// Speed is based on the number of pulses generated by the Hall encoder within 100ms
// Source: https://wiki.keyestudio.com/Ks0193_keyestudio_Self-balancing_Car
// Code Modified by Addison Sears-Collins at https://automaticaddison.com

// Digital Pins 8 and 12 control the right motor.
// Speed is controlled by Digital Pin 10.
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;

// Digital Pins 7 and 6 control the left motor's direction.
// Speed is controlled by Digital Pin 9.
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

const int PinA_left = 1;    // set the left motor’s pulse pin to Digital Pin 1
const int PinA_right = 4;   // set the right motor’s pulse pin to Digital Pin 4

int times = 0;    // Time
int newtime = 0;  // New Time
int d_time = 100; // Time Interval set to 100ms

int valA = 0; // Number of pulses for left motor
int flagA = 0;

int valB = 0; // Number of pulses for right motor
int flagB = 0;

void setup() {
  
  //set the baud rate to 9600
  Serial.begin(9600);          

  // Set all the pins to OUTPUT
  // The motors' speed and direction is controlled by 
  // the output of these pins
  pinMode(right_R1,OUTPUT);     
  pinMode(right_R2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  // The pulse pins are set to INPUT
  pinMode(PinA_right,INPUT);
  pinMode(PinA_left,INPUT);   

}

void loop() {
  
  // Go forward 
  digitalWrite(right_R1,HIGH); 
  digitalWrite(right_R2,LOW);
  digitalWrite(left_L1,LOW);
  digitalWrite(left_L2,HIGH);

  // write into PWM value 0~255（speed）
  analogWrite(PWM_R,100);  // Right wheel 
  analogWrite(PWM_L,200);  // Set Left wheel to be faster than the right wheel

  // Set newtime and times to Returns the number of 
  // milliseconds passed since the Arduino board began 
  // running the current program
  newtime = times = millis(); 

  while ((newtime-times) < d_time) {   // If we are less than 100ms, keep looping
    if(digitalRead(PinA_left) == HIGH && flagA == 0) { // If HIGH is detected
      valA++;      // valA plus 1
      flagA = 1;
    }
    if(digitalRead(PinA_left) == LOW && flagA == 1) {  // if LOW is detected
      valA++;     // valA plus 1
      flagA = 0;
    }    
    if(digitalRead(PinA_right) == HIGH && flagB == 0) { // if HIGH is detected
      valB++;     // valB plus 1
      flagB = 1;
    }
    if(digitalRead(PinA_right) == LOW && flagB == 1) {  // if LOW is detected
      valB++;
      flagB = 0;
    }
    
    newtime = millis();        // Update the new time
  }

  // Print out the number of pulses in 100ms for the left motor
  Serial.println(String("Left Motor: ") + valA);

  // Print out the number of pulses in 100ms for the right motor
  // valA (left motor) should be greater than valB (right motor) 
  Serial.println(String("Right Motor: ") + valB);

  valA = valB = 0; // Reset the values to 0  
 
}

// This is the code to test the built-in internal timer in the Arduino
// Speed is based on the number of pulses generated by the Hall encoder within 100ms
// Source: https://wiki.keyestudio.com/Ks0193_keyestudio_Self-balancing_Car
// Code Modified by Addison Sears-Collins at https://automaticaddison.com

#include <MsTimer2.h>

// Digital Pins 8 and 12 control the right motor.
// Speed is controlled by Digital Pin 10.
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;

// Digital Pins 7 and 6 control the left motor's direction.
// Speed is controlled by Digital Pin 9.
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

const int PinA_left = 1;    // set the left motor’s pulse pin to Digital Pin 1
const int PinA_right = 4;   // set the right motor’s pulse pin to Digital Pin 4

int times = 0;    // Time
int newtime = 0;  // New Time
int d_time = 100; // Time Interval set to 100ms

int valA = 0; // Number of pulses for left motor
int flagA = 0;

int valB = 0; // Number of pulses for right motor
int flagB = 0;

void setup() {
  
  //set the baud rate to 9600
  Serial.begin(9600);    

  // Set all the pins to OUTPUT
  // The motors' speed and direction is controlled by 
  // the output of these pins
  pinMode(right_R1,OUTPUT);     
  pinMode(right_R2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  // The pulse pins are set to INPUT
  pinMode(PinA_right,INPUT);
  pinMode(PinA_left,INPUT);   

  MsTimer2::set(100, inter); // trigger an interrupt every 100ms 
  MsTimer2::start();    // start interrupt  

}

void loop() {
  
  // Go forward 
  digitalWrite(right_R1,HIGH); 
  digitalWrite(right_R2,LOW);
  digitalWrite(left_L1,LOW);
  digitalWrite(left_L2,HIGH);

  // write into PWM value 0~255（speed）
  analogWrite(PWM_R,100);  // Right wheel 
  analogWrite(PWM_L,200);  // Set Left wheel to be faster than the right wheel

  if(digitalRead(PinA_left) == HIGH && flagA == 0) { // If HIGH is detected
    valA++;      // valA plus 1
    flagA = 1;
  }
  if(digitalRead(PinA_left) == LOW && flagA == 1) {  // if LOW is detected
    valA++;     // valA plus 1
    flagA = 0;
  }    
  if(digitalRead(PinA_right) == HIGH && flagB == 0) { // if HIGH is detected
    valB++;     // valB plus 1
    flagB = 1;
  }
  if(digitalRead(PinA_right) == LOW && flagB == 1) {  // if LOW is detected
    valB++;
    flagB = 0;
  }
}

//interrupt function
void inter() {
    sei();    //allow whole interrupts
    Serial.print("Left Motor = ");   //print out the pulse value on serial monitor
    Serial.println(valA);
    Serial.print("Right Motor: = ");
    Serial.println(valB);
    valA = valB = 0;
}

/**
* Addison Sears-Collins
* June 11, 2020
* This code is used to test the MPU6050 IMU sensor chip that is built-in
* to the Keyestudio Balance Shield V3.
* This 3-axis gyroscope and the 3-axis accelerometer enable
* us to measure the angular velocity (in degrees/second)
* and the linear acceleration (in G-forces, g) of the
* self-balancing robot car.
* 
**/

// Arduino's standard library for I2C communication
#include <Wire.h>

// Used to store accelerometer data
long accelX, accelY, accelZ;     
float gForceX, gForceY, gForceZ;

// Used to store the gyroscope data 
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ; // Angular velocity around those axes
 
void setup() {

  // Baud rate
  Serial.begin(9600);

  // Initialize I2C communication
  Wire.begin();

  setupMPU();
}
 
void loop() {
  recordAccelRegisters();
  recordGyroRegisters();
  printData();
  delay(100); // Delay of 100ms
}

/** 
 *  Establish communication between Arduino and the MPU6050.
 *  Set up all registers we'll be using to read MPU6050 data
 */
void setupMPU(){
  // REGISTER 0x6B/REGISTER 107:Power Management 1
  
  // This is the I2C address of the MPU 
  //(b1101000/b1101001 for AC0 low/high datasheet Sec. 9.2)
  // You can do a Google search to find the MPU6050 datasheet.
  Wire.beginTransmission(0b1101000); 

  //Accessing the register 6B/107 - Power Management (Sec. 4.28) 
  Wire.write(0x6B); 

  //Setting SLEEP register to 0, using the internal 8 Mhz oscillator
  Wire.write(0b00000000); 
  Wire.endTransmission();
 
  // REGISTER 0x1b/REGISTER 27:Gyroscope Configuration

  //I2C address of the MPU
  Wire.beginTransmission(0b1101000); 

  //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  Wire.write(0x1B);  

  // Setting the gyro to full scale +/- 250deg./s
  // (250/360 * 60 = 41.67rpm) 
  // The highest can be converted to 2000deg./s (333.3 rpm), but 
  // gyro sensitivity will be reduced.
  Wire.write(0x00000000); 
  Wire.endTransmission();
  
  // REGISTER 0x1C/REGISTER 28:ACCELEROMETER CONFIGURATION
  //I2C address of the MPU
  Wire.beginTransmission(0b1101000); 

  // Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  Wire.write(0x1C); 

  // Setting the accel to +/- 2g（if choose +/- 16g，
  // the value would be 0b00011000）
  Wire.write(0b00000000); 
  Wire.endTransmission(); 
}
 
void recordAccelRegisters() {
  // REGISTER 0x3B~0x40/REGISTER 59~64
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accelerometer Readings (see datasheet)
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
 
  // Use left shift << and bit operations |  Wire.read() read once 1bytes，and automatically read the data of the next address on the next call.
  while(Wire.available() < 6);  // Waiting for all the 6 bytes data to be sent from the slave machine （Must wait for all data to be stored in the buffer before reading） 
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX （Automatically stored as a defined long value）
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}

 // Divide the raw register reading to get a value that is meaningful for us
void processAccelData(){
  gForceX = accelX / 16384.0;     //float = long / float
  gForceY = accelY / 16384.0; 
  gForceZ = accelZ / 16384.0;
}
 
void recordGyroRegisters() {
  // REGISTER 0x43~0x48/REGISTER 67~72
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 ~ 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}

// The 131.0 comes from the MPU6050 datasheet. It helps us convert
// the raw data into a meaningful value.
void processGyroData() {
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0; 
  rotZ = gyroZ / 131.0;
}

// Print out 3-axis gyroscope data and 3-axis accelerometer data
void printData() {
  Serial.print("Gyro (deg)");
  Serial.print(" X=");
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print(" Accel (g)");
  Serial.print(" X=");
  Serial.print(gForceX);
  Serial.print(" Y=");
  Serial.print(gForceY);
  Serial.print(" Z=");
  Serial.println(gForceZ);
}

/**
* Addison Sears-Collins
* June 13, 2020
* This code is uses the MPU6050 IMU sensor chip that 
* is built-in to the Keyestudio Balance Shield V3 to 
* calculate the angle that the vehicle is tilting.
**/

// MPU6050 library
#include <MPU6050.h> 

// Arduino's standard library for I2C communication
#include <Wire.h> 

// Create an MPU6050 object named mpu6050
MPU6050 mpu6050; 

// Define three-axis acceleration and 
// three-axis gyroscope variables
// 16-bit signed integer is the data type
int16_t ax, ay, az, gx, gy, gz;     

// Variable that will hold the tilt angle
float Angle;   

// Variable that will hold the angle velocity
int16_t Gyro_x;   

void setup() {
  // Initialize I2C communication
  Wire.begin(); 

  // Set the baud rate
  Serial.begin(9600);   

  // Don't do anything for 1500 milliseconds (1.5 seconds)                    
  delay(1500);

  // Initialize the MPU6050
  mpu6050.initialize();                       

  // Do nothing for 2 milliseconds
  delay(2);
}

void loop() {

  // I2C to get MPU6050 six-axis data  ax ay az gx gy gz
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Radial rotation angle calculation formula; 
  // The negative sign indicates the direction.
  // Convert radians to degrees.
  Angle = -atan2(ay , az) * (180/ PI);     

  // The x-axis angular velocity calculated by the gyroscope; 
  // The negative sign indicates the direction. The 131 value comes
  // from the MPU6050 datasheet.
  Gyro_x = -gx / 131;  

  // Print the tilt angle in degrees
  Serial.print("Angle = ");
  Serial.print(Angle);

  // Print the angular velocity in degrees per second
  Serial.print("   Gyro_x = ");
  Serial.println(Gyro_x);
}

/**
* Addison Sears-Collins
* June 14, 2020
* This code uses a Kalman Filter on data from the MPU6050 
* IMU sensor chip (built-in to the Keyestudio Balance Shield V3) 
* to calculate the tilt angle and the angular velocity.
* Reference: https://wiki.keyestudio.com/Ks0193_keyestudio_Self-balancing_Car
**/

// MPU6050 library
#include <MPU6050.h>   

// I2C communication library
#include <Wire.h>        

// Create an MPU6050 object named mpu60500
MPU6050 mpu6050;     

//Define three-axis acceleration, three-axis gyroscope variables
int16_t ax, ay, az, gx, gy, gz; 

// The tilt angle
float Angle;

// Angular velocity along each axis as measured by the gyroscope
// The units are degrees per second.
float Gyro_x,Gyro_y,Gyro_z;  

///////////////////////Kalman_Filter////////////////////////////
// Covariance of gyroscope noise
float Q_angle = 0.001;  

// Covariance of gyroscope drift noise
float Q_gyro = 0.003;  

// Covariance of accelerometer
float R_angle = 0.5;    
char C_0 = 1;

// The filter sampling time.
float dt = 0.005;

// a function containing the Kalman gain is used to 
// calculate the deviation of the optimal estimate.
float K1 = 0.05; 
float K_0,K_1,t_0,t_1;
float angle_err;

// Gyroscope drift 
float q_bias;    

float accelz = 0;
float angle;
float angle_speed;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////

void setup() 
{
  // Join the I2C bus 
  Wire.begin();    

  // Set the baud rate
  Serial.begin(9600); 

  // 1.5 second delay
  delay(1500);

  // Initialize the MPU6050 sensor
  mpu6050.initialize();   

  // Delay 2 milliseconds
  delay(2);  
}

void loop() 
{
  Serial.print("Angle = ");
  Serial.print(Angle);
  Serial.print("  K_angle = ");
  Serial.println(angle);
  Serial.print("Gyro_x = ");
  Serial.print(Gyro_x);
  Serial.print("  K_Gyro_x = ");
  Serial.println(angle_speed);

  // I2C to get MPU6050 six-axis ax ay az gx gy gz
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     

  // Obtain angle and Kalman Filter 
  angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      
}

/////////////////////////////angle calculate///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
  // Radial rotation angle calculation formula; negative sign is direction processing
  Angle = -atan2(ay , az) * (180/ PI); 

  // The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
  Gyro_x = -gx / 131;      

  // KalmanFilter 
  Kalman_Filter(Angle, Gyro_x);            
}
////////////////////////////////////////////////////////////////

///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
  // Prior estimate
  angle += (gyro_m - q_bias) * dt;          
  angle_err = angle_m - angle;

  // Differential of azimuth error covariance
  Pdot[0] = Q_angle - P[0][1] - P[1][0];    
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;

  // The integral of the covariance differential of the prior estimate error
  P[0][0] += Pdot[0] * dt;    
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  
  // Intermediate variable of matrix multiplication
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  
  // Denominator
  E = R_angle + C_0 * PCt_0;
  
  // Gain value
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;

  // Intermediate variable of matrix multiplication
  t_0 = PCt_0;  
  t_1 = C_0 * P[0][1];

  // Posterior estimation error covariance
  P[0][0] -= K_0 * t_0;     
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;

  // Posterior estimation
  q_bias += K_1 * angle_err;    

  // The differential value of the output value; work out the optimal angular velocity
  angle_speed = gyro_m - q_bias;   

  ////Posterior estimation; work out the optimal angle
  angle += K_0 * angle_err; 
}

#include <MsTimer2.h>        //internal timer 2
#include <PinChangeInt.h>    //This library file can make all pins on the REV4 board as external interrupts.
#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC library

MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz;     // Define three-axis acceleration, three-axis gyroscope variables

//TB6612 pins
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

///////////////////////angle parameters//////////////////////////////
float angle_X; //Calculate the tilt angle variable about the X axis from the acceleration
float angle_Y; //Calculate the tilt angle variable about the Y axis from the acceleration
float angle0 = 1; //Actual measured angle (ideally 0 degrees)
float Gyro_x,Gyro_y,Gyro_z;  //Angular angular velocity by gyroscope calculation
///////////////////////angle parameters//////////////////////////////

///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001;  //Covariance of gyroscope noise
float Q_gyro = 0.003;    //Covariance of gyroscope drift noise
float R_angle = 0.5;    //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; // The value of dt is the filter sampling time. 
float K1 = 0.05; //a function containing the Kalman gain is used to calculate the deviation of the optimal estimate 
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;    //Gyro drift

float accelz = 0;
float angle;
float angleY_one;
float angle_speed;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////

//////////////////////PID parameters///////////////////////////////
double kp = 34, ki = 0, kd = 0.62;                   //angle loop parameters
double kp_speed = 3.6, ki_speed = 0.080, kd_speed = 0;   // speed loop parameters
double setp0 = 0; //angle balance point
int PD_pwm;  //angle output
float pwm1=0,pwm2=0;

//////////////////Interrupt speed measurement/////////////////////////////
#define PinA_left 5  //external interrupts
#define PinA_right 4   //external interrupts
volatile long count_right = 0;//Used to calculate the pulse value calculated by the Hall encoder (the volatile long type is to ensure the value is valid)
volatile long count_left = 0;
int speedcc = 0;
//////////////////////pulse calculation/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////PI variable parameters//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;

void setup() 
{
  // set the pins of motor to OUTPUT
  pinMode(right_R1,OUTPUT);       
  pinMode(right_R2,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  //assign initial state value
  digitalWrite(right_R1,1);
  digitalWrite(right_R2,0);
  digitalWrite(left_L1,0);
  digitalWrite(left_L2,1);
  analogWrite(PWM_R,0);
  analogWrite(PWM_L,0);

  pinMode(PinA_left, INPUT);  //speed code wheel input
  pinMode(PinA_right, INPUT);

  // join I2C bus
  Wire.begin();                            //join I2C bus sequence
  Serial.begin(9600);                       //open the serial monitor to set the baud rate to 9600
  delay(1500);
  mpu6050.initialize();                       //initialize MPU6050
  delay(2);

  //5ms; use timer2 to set timer interruption (note：using timer2 will affect the PWM output of pin3 pin11)
  MsTimer2::set(5, Interrupt_Service_Routine);    //5ms ; execute the function Interrupt_Service_Routine once
  MsTimer2::start();    //start interrupt
}

void loop() 
{
  Serial.println(angle);
  delay(100);
  //Serial.println(PD_pwm);
  //Serial.println(pwm1);
  //Serial.println(pwm2);
  //Serial.print("pulseright = ");
  //Serial.println(pulseright);
  //Serial.print("pulseleft = ");
  //Serial.println(pulseleft);
  //Serial.println(PI_pwm);
  //Serial.println(speeds_filter);
  //Serial.println (positions);
  
  //External interrupt for calculating wheel speed 
  attachPinChangeInterrupt(PinA_left, Code_left, CHANGE);          //PinA_left Level change triggers external interrupt; execute subfunction Code_left
  attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);       //PinA_right Level change triggers external interrupt; execute subfunction Code_right
}

/////////////////////Hall calculation/////////////////////////
//left speed code wheel count
void Code_left() 
{
  count_left ++;
} 
//Right speed code wheel count
void Code_right() 
{
  count_right ++;
} 
////////////////////pulse calculation///////////////////////
void countpluse()
{
  lz = count_left;     //Assign the value counted by the code wheel to lz
  rz = count_right;

  count_left = 0;     //Clear the code counter count
  count_right = 0;

  lpluse = lz;
  rpluse = rz;

  if ((pwm1 < 0) && (pwm2 < 0))                     //judge the moving direction; if backwards（PWM, namely motor voltage is negative）, pulse number is a negative number
  {
    rpluse = -rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 > 0))                 // if backwards（PWM, namely motor voltage is positive）, pulse number is a positive number
  {
    rpluse = rpluse;
    lpluse = lpluse;
  }
  else if ((pwm1 < 0) && (pwm2 > 0))                 //Judge turning direction of the car;  turn left; Right pulse number is a positive number; Left pulse number is a negative number.
  {
    rpluse = rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 < 0))               //Judge turning direction of the car;  turn right; Right pulse number is a negative number; Left pulse number is a positive number.
  {
    rpluse = -rpluse;
    lpluse = lpluse;
  }

  //enter interrupts per 5ms; pulse number superposes
  pulseright += rpluse;
  pulseleft += lpluse;
}

/////////////////////////////////interrupts////////////////////////////
void Interrupt_Service_Routine()
{
  sei();  //Allow global interrupts
  countpluse();        //Pulse superposition subfunction
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data  ax ay az gx gy gz
  angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      //get angle and Kalman filtering
  PD();         //angle loop PD control
  anglePWM();

  cc++;
  if(cc>=8)     //5*8=40，40ms entering once speed PI algorithm  
  {
    speedpiout();   
    cc=0;  //Clear
  }
}
///////////////////////////////////////////////////////////

/////////////////////////////angle calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
  float Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; negative sign is direction processing
  Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
  Kalman_Filter(Angle, Gyro_x);            //Kalman Filtering
  //Rotation angle Z-axis parameter
  Gyro_z = -gz / 131;                      //Z-axis angular velocity
  //accelz = az / 16.4;

  float angleAx = -atan2(ax, az) * (180 / PI); //Calculate the angle with the x-axis
  Gyro_y = -gy / 131.00; //Y-axis angular velocity
  Yiorderfilter(angleAx, Gyro_y); //first-order filtering
}
////////////////////////////////////////////////////////////////

///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
  angle += (gyro_m - q_bias) * dt;          //Prior estimate
  angle_err = angle_m - angle;
  
  Pdot[0] = Q_angle - P[0][1] - P[1][0];    //Differential of azimuth error covariance
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
  
  P[0][0] += Pdot[0] * dt;    //A priori estimation error covariance differential integral
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  
  //Intermediate variable of matrix multiplication 
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  //Denominator
  E = R_angle + C_0 * PCt_0;
  //gain value
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  
  t_0 = PCt_0;  //Intermediate variable of matrix multiplication
  t_1 = C_0 * P[0][1];
  
  P[0][0] -= K_0 * t_0;    //Posterior estimation error covariance
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  
  q_bias += K_1 * angle_err;    //Posterior estimate 
  angle_speed = gyro_m - q_bias;   //The differential of the output value gives the optimal angular velocity
  angle += K_0 * angle_err; ////Posterior estimation to get the optimal angle
}

/////////////////////first-order filtering/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
  angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}

//////////////////angle PD////////////////////
void PD()
{
  PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control
}

//////////////////speed PI////////////////////
void speedpiout()
{
  float speeds = (pulseleft + pulseright) * 1.0;      //Vehicle speed  pulse value
  pulseright = pulseleft = 0;      //Clear
  speeds_filterold *= 0.7;         //first-order complementary filtering
  speeds_filter = speeds_filterold + speeds * 0.3;
  speeds_filterold = speeds_filter;
  positions += speeds_filter;
  positions = constrain(positions, -3550,3550);    //Anti-integral saturation
  PI_pwm = ki_speed * (setp0 - positions) + kp_speed * (setp0 - speeds_filter);      //speed loop control PI
}
//////////////////speed PI////////////////////


////////////////////////////PWM end value/////////////////////////////
void anglePWM()
{
  pwm2=-PD_pwm - PI_pwm ;           //assign the final value of PWM to motor 
  pwm1=-PD_pwm - PI_pwm ;
  
  if(pwm1>255)             //limit PWM value not greater than 255
  {
    pwm1=255;
  }
  if(pwm1<-255) 
  {
    pwm1=-255;
  }
  if(pwm2>255)
  {
    pwm2=255;
  }
  if(pwm2<-255)
  {
    pwm2=-255;
  }

  // When the self-balancing trolley’s tilt angle gets less than a certain angle, 
  // the motor will stop.
  
  if(angle>25 || angle<-25)      
  {
    pwm1=pwm2=0;
  }

  // Determine the motor’s steering and speed by the positive and negative of PWM 
  if(pwm2>=0)         
  {
    digitalWrite(left_L1,LOW);
    digitalWrite(left_L2,HIGH);
    analogWrite(PWM_L,pwm2);
  }
  else
  {
    digitalWrite(left_L1,HIGH);
    digitalWrite(left_L2,LOW);
    analogWrite(PWM_L,-pwm2);
  }

  if(pwm1>=0)
  {
    digitalWrite(right_R1,HIGH);
    digitalWrite(right_R2,LOW);
    analogWrite(PWM_R,pwm1);
  }
  else
  {
    digitalWrite(right_R1,LOW);
    digitalWrite(right_R2,HIGH);
    analogWrite(PWM_R,-pwm1);
  }
}

  // When the self-balancing trolley’s tilt angle gets less than 
  // a certain angle, the motor will stop.
  
  if(angle>25 || angle<-25)      
  {
    pwm1=pwm2=0;
  }

#include <MsTimer2.h>        //internal timer 2
#include <PinChangeInt.h>    //this library can make all pins of arduino REV4 as external interrupt
#include <MPU6050.h>      //MPU6050 library 
#include <Wire.h>        //IIC communication library 

MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz;     //Define three-axis acceleration, three-axis gyroscope variables

//TB6612 pins
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

///////////////////////angle parameters//////////////////////////////
float angle_X; //calculate the inclined angle variable of X-axis by accelerometer  
float angle_Y; //calculate the inclined angle variable of Y-axis by accelerometer
float angle0 = 1; //Actual measured angle (ideally 0 degrees) 
float Gyro_x,Gyro_y,Gyro_z;  //Angular angular velocity for gyroscope calculation 
///////////////////////angle parameters//////////////////////////////

///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001;  //Covariance of gyroscope noise
float Q_gyro = 0.003;    //Covariance of gyroscope drift noise
float R_angle = 0.5;    //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; //The value of dt is the filter sampling time
float K1 = 0.05; //  a function containing the Kalman gain; used to calculate the deviation of the optimal estimate
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;    //gyroscope drift

float accelz = 0;
float angle;
float angleY_one;
float angle_speed;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////

//////////////////////PID parameters///////////////////////////////
double kp = 34, ki = 0, kd = 0.62;                   //angle loop parameters
double kp_speed = 3.6, ki_speed = 0.080, kd_speed = 0;   // speed loop parameters
double kp_turn = 24, ki_turn = 0, kd_turn = 0.08;       // steering loop parameters
double setp0 = 0; //Angle balance point 
int PD_pwm;  //angle output
float pwm1=0,pwm2=0;

//////////////////Interrupt speed count/////////////////////////////
#define PinA_left 5  //external interrupt
#define PinA_right 4   //external interrupt 
volatile long count_right = 0;//Used to calculate the pulse value calculated by the Hall encoder (the volatile long type is to ensure the value is valid)
volatile long count_left = 0;
int speedcc = 0;
//////////////////////pulse count/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////PI variable parameter//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;

//////////////////////////////steering PD///////////////////
int turnmax,turnmin,turnout; 
float Turn_pwm = 0;
int zz=0;
int turncc=0;

//Remote control or Bluetooth//
int front = 0;//go front variable
int back = 0;//go back variable
int left = 0;//turn left
int right = 0;//turn right
char val;

void setup() 
{
  //set the motor control pins to OUTPUT
  pinMode(right_R1,OUTPUT);       
  pinMode(right_R2,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  //assign initial state value
  digitalWrite(right_R1,1);
  digitalWrite(right_R2,0);
  digitalWrite(left_L1,0);
  digitalWrite(left_L2,1);
  analogWrite(PWM_R,0);
  analogWrite(PWM_L,0);

  pinMode(PinA_left, INPUT);  //Speed encoder input
  pinMode(PinA_right, INPUT);

  // join I2C bus
  Wire.begin();                            //join I2C bus sequence 
  Serial.begin(9600);                       //open the serial monitor and set the baud rate to 9600
  delay(1500);
  mpu6050.initialize();                       //initialize MPU6050
  delay(2);

  //5ms; use timer2 to set timer interruption (note：using timer2 will affect the PWM output of pin3 pin11)
  MsTimer2::set(5, Interrupt_Service_Routine);    //5ms ; execute the function Interrupt_Service_Routine once
  MsTimer2::start();    //start interrupt
}

void loop() 
{
  Serial.print(angle_speed);
  Serial.print("     ");
  Serial.println(Gyro_x);
  
  //Serial.println(angle);
  //delay(100);
  //Serial.println(PD_pwm);
  //Serial.println(pwm1);
  //Serial.println(pwm2);
  //Serial.print("pulseright = ");
  //Serial.println(pulseright);
  //Serial.print("pulseleft = ");
  //Serial.println(pulseleft);
  //Serial.println(PI_pwm);
  //Serial.println(speeds_filter);
  //Serial.println (positions);
  //Serial.println(Turn_pwm);
  //Serial.println(Gyro_z);
  //Serial.println(Turn_pwm);

  // Used for Bluetooth control
  // if(Serial.available()) {
  // val = Serial.read();      //assign the value read from serial port to val
  // Serial.println(val);
  val = 'R';
  switch(val)             //switch statement
  {
    case 'F': front=250; break;       //if val equals to F，front=250，balance robot goes front.
    case 'B': back=-250; break;       //go back
    case 'L': left=1; break;    //turn left
    case 'R': right=1; break;                         // turn right
    case 'S': front=0,back=0,left=0,right=0;break;    //stop
      case 'D': Serial.print(angle);break;    //when receiving ‘D’，send value of angle to APP
   }


  //}
  
  //external interrupt; used to calculate the wheel speed
  attachPinChangeInterrupt(PinA_left, Code_left, CHANGE);          //PinA_left  Level change triggers external interrupt;  execute subfunction Code_left
  attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);       //PinA_right Level change triggers external interrupt;  execute subfunction Code_right
}

/////////////////////Hall count/////////////////////////
//left speed encoder count
void Code_left() 
{
  count_left ++;
} 
//right speed encoder count
void Code_right() 
{
  count_right ++;
} 
////////////////////pulse count///////////////////////
void countpluse()
{
  lz = count_left;     //assign the value counted by encoder to lz
  rz = count_right;

  count_left = 0;     //clear the count quantity
  count_right = 0;

  lpluse = lz;
  rpluse = rz;

  if ((pwm1 < 0) && (pwm2 < 0))                     //judge the moving direction of balance robot; if go back (PWM the motor voltage is negative), the number of pulses is negative.
  {
    rpluse = -rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 > 0))                 // if go back (PWM the motor voltage is positive) , the number of pulses is positive.
  {
    rpluse = rpluse;
    lpluse = lpluse;
  }
  else if ((pwm1 < 0) && (pwm2 > 0))                 //judge the moving direction of balance robot; if turn left, right pulse is positive but left pulse is negative.
  {
    rpluse = rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 < 0))               //judge the moving direction of balance robot; if turn right , right pulse is negative but left pulse is positive.
  {
    rpluse = -rpluse;
    lpluse = lpluse;
  }

  //entering interrupt per 5ms，pulse will plus
  pulseright += rpluse;
  pulseleft += lpluse;
}

/////////////////////////////////interrupt ////////////////////////////
void Interrupt_Service_Routine()
{
  sei();  //allow overall interrupt
  countpluse();        //pulse count subfunction
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data ax ay az gx gy gz
  angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      //get the angle and Kalman filtering
  PD();         //PD control of angle loop 
  anglePWM();

  cc++;
  if(cc>=8)     //5*8=40，40ms entering PI count of speed once
  {
    speedpiout();   
    cc=0;  // Clear
  }
  turncc++;         
  if(turncc>4)       //20ms entering PI count of steering once
  {
    turnspin();
    turncc=0;     //Clear
  }
}
///////////////////////////////////////////////////////////

/////////////////////////////tilt angle calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
  float Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; the negative sign is direction processing.
  Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope ; the negative sign is the direction processing.
  Kalman_Filter(Angle, Gyro_x);            // Kalman Filter
  // Z-axis angular velocity
  Gyro_z = -gz / 131;                      //speed of Z-axis 
  //accelz = az / 16.4;

  float angleAx = -atan2(ax, az) * (180 / PI); //calculate the included angle of X-axis  
  Gyro_y = -gy / 131.00; //angle speed of Y-axis 
  Yiorderfilter(angleAx, Gyro_y); //first-order filtering
}
////////////////////////////////////////////////////////////////

///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
  angle += (gyro_m - q_bias) * dt;          //prior estimate
  angle_err = angle_m - angle;
  
  Pdot[0] = Q_angle - P[0][1] - P[1][0];    //The differential of the covariance of the prior estimate error 
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
  
  P[0][0] += Pdot[0] * dt;    //The integral of the covariance differential of the prior estimate error
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  
  //Intermediate variables in matrix multiplication
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  //denominator
  E = R_angle + C_0 * PCt_0;
  //gain value
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  
  t_0 = PCt_0;  //Intermediate variables in matrix multiplication 
  t_1 = C_0 * P[0][1];
  
  P[0][0] -= K_0 * t_0;    //the covariance of the prior estimate error 
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  
  q_bias += K_1 * angle_err;    //posterior estimate
  angle_speed = gyro_m - q_bias;   //The differential of the output value; get the optimal angular velocity
  angle += K_0 * angle_err; ////posterior estimate; get the optimal angular velocity
}

/////////////////////first-order filtering/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
  angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}

//////////////////angle PD////////////////////
void PD()
{
  PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control 
}

//////////////////speed PI////////////////////
void speedpiout()
{
  float speeds = (pulseleft + pulseright) * 1.0;      //pulse value of speed  
  pulseright = pulseleft = 0;      //Clear 
  speeds_filterold *= 0.7;         //first-order complementary filtering
  speeds_filter = speeds_filterold + speeds * 0.3;
  speeds_filterold = speeds_filter;
  positions += speeds_filter;
  positions += front;             //Forward control fusion
  positions += back;              //backward control fusion
  positions = constrain(positions, -3550,3550);    //Anti-integral saturation
  PI_pwm = ki_speed * (setp0 - positions) + kp_speed * (setp0 - speeds_filter);      //speed loop controlling PI
}
//////////////////speed PI////////////////////

///////////////////////////steering/////////////////////////////////
void turnspin()
{
  int flag = 0;      //
  float turnspeed = 0;
  float rotationratio = 0;
  
  if (left == 1 || right == 1)
  {
    if (flag == 0)                             //judge the speed before turning; increase the car’s flexibility.
    {
      turnspeed = ( pulseright + pulseleft);                      //current speed of car; pulse expression
      flag=1;
    }
    if (turnspeed < 0)                                 //Absolute value of the car's current speed
    {
      turnspeed = -turnspeed;
    }
    if(left==1||right==1)         //if press left key or right key
    {
     turnmax=3;          //maximum value of turning
     turnmin=-3;         // minimum value of turning
    }
    rotationratio = 5 / turnspeed;          //set the value by speed
    if (rotationratio < 0.5)
    {
      rotationratio = 0.5;
    }
     
    if (rotationratio > 5)
    {
      rotationratio = 5;
    }
  }
  else
  {
    rotationratio = 0.5;
    flag = 0;
    turnspeed = 0;
  }
  if (left ==1)//plus by direction parameter
  {
    turnout += rotationratio;
  }
  else if (right == 1 )//plus by direction parameter
  {
    turnout -= rotationratio;
  }
  else turnout = 0;
  if (turnout > turnmax)   turnout = turnmax;//the max value setting of amplitude
  if (turnout < turnmin)   turnout = turnmin;//the min value setting of amplitude 

  Turn_pwm = -turnout * kp_turn - Gyro_z * kd_turn;//The rotation PD algorithm controls the fusion speed and Z axis rotation positioning
}
///////////////////////////turning/////////////////////////////////

////////////////////////////PWM end value/////////////////////////////
void anglePWM()
{
  pwm2=-PD_pwm - PI_pwm + Turn_pwm;           //assign the end value of PWM to motor
  pwm1=-PD_pwm - PI_pwm - Turn_pwm;
  
  if(pwm1>255)             //limit the PWM value not more than 255
  {
    pwm1=255;
  }
  if(pwm1<-255) 
  {
    pwm1=-255;
  }
  if(pwm2>255)
  {
    pwm2=255;
  }
  if(pwm2<-255)
  {
    pwm2=-255;
  }

  if(angle>25 || angle<-25)      //if tilt angle is greater than 25° , motor will stop.
  {
    pwm1=pwm2=0;
  }

  // Determine the motor’s steering and speed by the positive and negative of PWM 
  if(pwm2>=0)         
  {
    digitalWrite(left_L1,LOW);
    digitalWrite(left_L2,HIGH);
    analogWrite(PWM_L,pwm2);
  }
  else
  {
    digitalWrite(left_L1,HIGH);
    digitalWrite(left_L2,LOW);
    analogWrite(PWM_L,-pwm2);
  }

  if(pwm1>=0)
  {
    digitalWrite(right_R1,HIGH);
    digitalWrite(right_R2,LOW);
    analogWrite(PWM_R,pwm1);
  }
  else
  {
    digitalWrite(right_R1,LOW);
    digitalWrite(right_R2,HIGH);
    analogWrite(PWM_R,-pwm1);
  }
}

  // Used for Bluetooth control
  // if(Serial.available()) {
  // val = Serial.read(); //assign the value read from serial port to val
  // Serial.println(val);
  val = 'R';
  switch(val)             //switch statement
  {
    case 'F': front=250; break; //if val=F，front=250, bot goes forward.
    case 'B': back=-250; break; //go back
    case 'L': left=1; break;  //turn left
    case 'R': right=1; break; // turn right
    case 'S': front=0,back=0,left=0,right=0;break;  //stop
    case 'D': Serial.print(angle);break;  //when receiving ‘D’，send value of angle to APP
   }