Self-balancing.ino

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>

#define leftMotorPWMPin   6
#define leftMotorDirPin   7
#define rightMotorPWMPin  5
#define rightMotorDirPin  4

#define TRIGGER_PIN 9
#define ECHO_PIN 8
#define MAX_DISTANCE 1

#define Kp  20
#define Kd  0.05
#define Ki  20
#define sampleTime  0.005
#define targetAngle 10

MPU6050 mpu;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;
int distanceCm;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
  if(leftMotorSpeed >= 0) {
    analogWrite(leftMotorPWMPin, leftMotorSpeed);
    digitalWrite(leftMotorDirPin, LOW);
  }
  else {
    analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
    digitalWrite(leftMotorDirPin, HIGH);
  }
  if(rightMotorSpeed >= 0) {
    analogWrite(rightMotorPWMPin, rightMotorSpeed);
    digitalWrite(rightMotorDirPin, LOW);
  }
  else {
    analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
    digitalWrite(rightMotorDirPin, HIGH);
  }
}

void init_PID() {  
  /* Initialize Timer1 */
  cli();          // Disable global interrupts
  TCCR1A = 0;     // Reset TCCR1A register
  TCCR1B = 0;     // Reset TCCR1B register
    
  /* Set compare match register to set sample time 5ms */
  OCR1A = 9999;
    
  /* Turn on CTC mode */
  TCCR1B |= (1 << WGM12);

  /* Set CS11 bit for prescaling by 8 */
  TCCR1B |= (1 << CS11);

  /* Enable timer compare interrupt */
  TIMSK1 |= (1 << OCIE1A);

  sei();          // Enable global interrupts
}

void setup() {
  /* Set the motor control and PWM pins to output mode */
  pinMode(leftMotorPWMPin, OUTPUT);
  pinMode(leftMotorDirPin, OUTPUT);
  pinMode(rightMotorPWMPin, OUTPUT);
  pinMode(rightMotorDirPin, OUTPUT);

  /* Set the status LED to output mode 
  pinMode(13, OUTPUT);

  /* Initialize the MPU6050 and set offset values
  mpu.initialize();

  /* Initialize PID sampling loop
  init_PID();
}

void loop() {
  /* Read acceleration and gyroscope values */
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();  
  gyroX = mpu.getRotationX();

  /* Set motor power after constraining it
  motorPower = constrain(motorPower, -255, 255);
  setMotors(motorPower, -motorPower);

  /* Measure distance every 100 milliseconds
  if((count%20) == 0){
    distanceCm = sonar.ping_cm();
  }
  if((distanceCm < 20) && (distanceCm != 0)) {
    setMotors(-motorPower, motorPower);
  }
}


/* The ISR will be called every 5 milliseconds */
ISR(TIMER1_COMPA_vect)
{
  /* Calculate the angle of inclination */
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate*sampleTime;  
  currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
  
  error = currentAngle - targetAngle;
  errorSum = errorSum + error;  
  errorSum = constrain(errorSum, -300, 300);

  /* Calculate output from P, I and D values */
  motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
  prevAngle = currentAngle;

  /* Toggle the led on pin13 every second */
  count++;
  if(count == 200)  {
    count = 0;
    digitalWrite(13, !digitalRead(13));
  }
}