My balancing robot

This project is developed with Arduino language. The material needed is the following.



A- Arduino NANO board with ATMega328 at 16 MHz.

B- MPU6050  GY-521 (3-axis Gyro + 3-axis accelerometer)

C- L298 (Dual DC motor controller)

D- Two 100Rpm DC motors (Very low cost motors with with reduction gearedbox)

E- 11.1 Volts LiPo battery.



For motion commands I use the following:



F- SPECTRUM  DX-7000 receiver. (Used with SPECTRUM 2.4 GHz Transmitter)

G- PPM signal encoder. (Developed with AVR ATMega328 at 8 MHz in AVR assembly)





It is the usual 'inverted pendulum' model that governs the function of the project, and thus:



When power is turned on, an eternal loop calculates a PID amplifier.

The commanded input is zero degrees of inclination to maintain the robot in equilibrium.

The calculated input is the inclination angle composed from the fusion of the integral of Gyro angular velocity and the angle obtained from the gravity acceleration of the accelerometer.

The fusion  is calculated within the MPU6050 from the internal DMP unit.

The Output of the PID amplifier returns the proper PWM needed for the two motors to maintain equilibrium of the unstable robot system.

Last segment of the program scales the pwm of each DC motor to maintain equilibrium while moving according to the PMM motion commands.



My program uses known libraries for the MPU6050 and the PID control. It is free to use as is. I assume no responsibility of any kind.



Pls keep in mind that the PID control algorithm is based the proper selection on 3 gain parameters. The proportional Pk, the integral Ik and the derivative Dk.



I did my best to select values that are not very critical. So I think that keeping the Dk factor as is you can use Pk and Ik factors in a wide range of values (+ - 10) with good results. Unfortunately it is a trial and error procedure but believe me my program is very flexible.



If you do not want to use motion commands, just comment out the program lines that refer to the  joypitchw and joyrollw variables.



If you need the PPM encoder project you can find it at my blog page:

Iannis-page.blogspot.com 

This is the code:

//---------------------------------------------------

//---------- TWO_WHEELS_BALANCING -------------------

//

// Program developed by Iannis Iliopoulos (20-11-2015)

//---------------------------------------------------

// This program runs on Arduino NANO with ATMEGA328P at 16MHz

// 2KB Ram, 32KB Flash-Rom, 512 Bytes EEPROM.

//

// An MPU6050 (3-axis accelerometer and Gyro) with I2C comms

// and int1 (pin3), detects X inclination.

// 

// Reception of 7-channels PPM signal at Int0 (pin2) is 

// performed through a second ATMEGA328 at 8MHz internal clock,

// which receives up to 8 PWM signals from a 2,4MHz RX

// (Spectrum DX-7 in this case)and transforms it to PPM signal.

//

// 2 PWM outputs control two 100RPM DC motors

// through an L298 PWM-controller.

//

// Serial port output permits comms with HOST

// for programming and generic I/O

//

//---------------------------------------------------

//-------------- PROJECT STARTS HERE ----------------

//

//---------------- INCLUDED LIBRARIES ---------------

 

#include "Wire.h"

#include <PID_v1.h>

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

//--------------------------------------------------

//               Motors controller L298

//--------------------------------------------------

#define ENA 9       //Arduino digital pin 9 = (PB1-OC1A)

#define ENB 10      //Arduino digital pin 10 = (PB2-OC1B)

#define IN1 12      //Arduino digital pin 12 = (PB4) 

#define IN2 5       //Arduino digital pin 5 = (PD5)

#define IN3 4       //Arduino digital pin 4 = (PD4)

#define IN4 6       //Arduino digital pin 6 = (PD6)

int reladi = 30;

//--------------------------------------------------

//                     PID

//--------------------------------------------------

double targetbalanceangle = 180;

double setpoint = targetbalanceangle;

double input, output, Loutput, Routput;

double kp=45, ki=350, kd=1.5;

PID powercontrol(&input, &output, &setpoint, kp , ki, kd, DIRECT);

//--------------------------------------------------

//                    LEDS

//--------------------------------------------------

#define led_red 7

#define led_green 8

#define led_yellow 11

#define led_front A2

#define led_rear A3

#define LED_PIN 13 // Arduino Led

bool blinkState1 = false;

bool blinkState2 = false;

bool blinkState3 = false;

bool blinkState4 = false;

bool blinkState5 = false;

//--------------------------------------------------

//                     MPU6050

//--------------------------------------------------

MPU6050 mpu;    //create MPU object.

// MPU control/status vars

bool dmpReady = false;  // set true if DMP init was successful

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU

uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount;     // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q;           // [w, x, y, z]         quaternion container

VectorInt16 aa;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements

VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements

VectorFloat gravity;    // [x, y, z]            gravity vector

float euler[3];         // [psi, theta, phi]    Euler angle container

float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

float yprOffset[] = {0, 0, 0};

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

#define INT1 3

//----------------------------------------------------------------------

//      Receive 7-Channels from Spectrum DX7 receiver plus PPM-encoder

//----------------------------------------------------------------------

#define PPM_int 2           //PPM signal present as Int0 at pin D2

int ppm[8], pr_ppm[8];      //arrays for storing 8 servo signals

byte channel=0;

float joypitch = 0, joypitchw=0;

float joyyaw = 0, joyyaww=0;

float joyroll = 0, joyrollw=0;

//--------------------------------------------------

//                  FUNCTIONS and ISR's

//--------------------------------------------------

void dmpDataReady() {

  mpuInterrupt = true;

}

//-----------------------------------------------------------------

ISR(TIMER2_OVF_vect)   // Timer2 Overflow interrupt service routine (4ms period)

 {                     // Detects the period between the end of 7channels (max 14ms) and

 TCNT2 = 0;            // the total PPM-signal-period = 20ms

 channel = 0;          // So Timer2 counter is reset and channel is restarted for a new 20ms period.

 //blinkState3 = !blinkState3;

 //digitalWrite(led_red, blinkState3);

 }

//-----------------------------------------------------------------

void read_ppm(){  //receives all 7 joystick channels (Max channel-period = 2ms  => Total_period = max 14ms)

  //-- SPECTRUM DX7 Receiver --------------------------------------

  // Channel 1 = Roll

  // Channel 2 = Throttle

  // Channel 3 = Pitch

  // Channel 4 = Yaw

  // Channel 5 = Throttle (same as Channel 2)

  // Channel 6 = 3pos Switch

  // Channel 7 = Aux2 switch

  if (channel > 0) {     

  ppm[channel] = TCNT2;    //Store timer2 counter in ppm[channel]

  pr_ppm[channel]=TCNT2; //ppm[channel];

  }

  channel++;              // RISING value of channel=0 will be discarded (Valid channels are 1 ~ 7)

  TCNT2=0;                // Restart Timer2 counter

}

// ================================================================

// ================================================================

// ================================================================

// ================================================================

// ================================================================

// ===                           SETUP                          ===

// ================================================================

void setup() {

 

  // configure Serial comms

  Serial.begin(115200);    

  //configure all motor controller L298 pins

  pinMode(ENA, OUTPUT);

  pinMode(ENB, OUTPUT);

  pinMode(IN1, OUTPUT);

  pinMode(IN2, OUTPUT);

  pinMode(IN3, OUTPUT);

  pinMode(IN4, OUTPUT);

  //-------------------------------------------

  // configure all LED's for output

  pinMode(led_red, OUTPUT);

  pinMode(led_green, OUTPUT);

  pinMode(led_yellow, OUTPUT);

  pinMode(led_front, OUTPUT);

  pinMode(led_rear, OUTPUT);

  pinMode(LED_PIN, OUTPUT);       //Arduino Led

  //-------------------------------------------  

  // configure PPM input at interrupt0 pin 2

  pinMode(PPM_int, INPUT);        //Int0

  //-------------------------------------------

  // configure MPU input at interrupt1 pin 3

  pinMode(INT1, INPUT);           //Int1

  //-------------------------------------------

  // join I2C bus (I2Cdev library doesn't do this automatically) 

  Wire.begin();

  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

  //-------------------------------------------

  // initialize device

  mpu.initialize();

  //-------------------------------------------

 

  // verify connection

  //Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  devStatus = mpu.dmpInitialize();

  

  // supply own gyro offsets here extrapolated from any callibration program.

  mpu.setXGyroOffset(92); 

  mpu.setYGyroOffset(-22);

  mpu.setZGyroOffset(14);

   

  mpu.setXAccelOffset(-69);  

  mpu.setYAccelOffset(-215);

  mpu.setZAccelOffset(1259);

  if (devStatus == 0) {

    // turn on the DMP, now that it's ready. (devStatus == 0)

    mpu.setDMPEnabled(true);

    // enable Arduino interrupt-1 detection

    attachInterrupt(1, dmpDataReady, RISING);

    mpuIntStatus = mpu.getIntStatus();

    dmpReady = true;

    packetSize = mpu.dmpGetFIFOPacketSize();

    //-------------setup PID ----------------------

        

    powercontrol.SetMode(AUTOMATIC);

    powercontrol.SetSampleTime(10);                 //looptime is 10ms

    powercontrol.SetOutputLimits(-255, 255);     

    //--------------------------------------------

       

  } else {      // (devStatus != 0)

    // ERROR!

    // ERROR codes are:

    // 1 = initial memory load failed

    // 2 = DMP configuration updates failed

    //Serial.print(F("DMP Initialization failed (code "));

    //Serial.print(devStatus);

    //Serial.println(F(")"));

  }

 

  //---- initialize PPM-SIGNAL-INPUT (Interrupt0)

  attachInterrupt(0, read_ppm, RISING); //calls function read_ppm at rising interrupt0 (pin D2)

  // initialize Timer2 to interrupt every ~4ms. [1/(16MHz / prescaler=256 / TCNT=256)]

  TCCR2B = 0;                   // reset timer2 by setting prescaler = 0

  TCNT2 = 0;                    // reset timer2 counter

  TCCR2A = 0;                   // normal CTC operation

  TIMSK2 = 0x01;  //|= (1 << TOIE2);            // enable timer overflow interrupt

  TCCR2B = 0x06;  //|= (1 << CS22)|(1 << CS21); // prescaler = 256

 

  sei();                        // enable all interrupts

  blinkState1 = !blinkState1;

  digitalWrite(led_yellow, blinkState1);

}  

// ================================================================

// ================================================================

// ================================================================

// ================================================================

// ================================================================

// ===                    MAIN PROGRAM LOOP                     ===

// ================================================================

void loop() {

 

  if (!dmpReady) return;

 

  // wait for MPU interrupt or extra packet(s) available

  while (!mpuInterrupt && fifoCount < packetSize) {

   

    //MPU not ready yet, so lets perform some tasks

    //---------------------------------------  

//  

//   

//     

    //---------------------------------------              

  }

  // reset interrupt flag and get INT_STATUS byte

  mpuInterrupt = false;

  mpuIntStatus = mpu.getIntStatus();

 

  // get current FIFO count

  fifoCount = mpu.getFIFOCount();

 

  // check for overflow

  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {  //fifo overflow. Reset fifo.

    // reset so we can continue cleanly

    mpu.resetFIFO();

   

    // otherwise, check for DMP data ready interrupt

  } else if (mpuIntStatus & 0x02) { // OK DMP data interrupt present.

    // wait for full packetsize from FIFO.

    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // OK. so get the complete packet from FIFO.

    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available

    // (this lets us immediately read more without waiting for an interrupt)

    fifoCount -= packetSize;

    // display Euler angles in degrees

    mpu.dmpGetQuaternion(&q, fifoBuffer);

    mpu.dmpGetGravity(&gravity, &q);

    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

   

    //--------------------------

    //Get the MPU-Pitch angle as PID input

    float mpupitch = ypr[1] * 180/M_PI + 180;

    input = mpupitch;

    //------------ Scale the joystick values ----------------------

    joyroll = pr_ppm[1]-99;               //range 74 ~ 124 -> -25 to +25 new range   

    joypitch = pr_ppm[3]-99;               //range 74 ~ 124 -> -25 to +25 new range

    //joypitchw=map(joypitch, -25, 25, -2, 2);

   

    joypitchw = 0;  

    if (joypitch>4) joypitchw=1;

    if (joypitch<-4) joypitchw=-1;

    targetbalanceangle = 180.2 + joypitchw;

    setpoint = targetbalanceangle;

   //  Serial.print(input);

   //  Serial.println("   ");

  

    powercontrol.Compute(); //Performs PID calculations

    joyrollw = 0;  

    if (joyroll>4) joyrollw=100;

    if (joyroll<-4) joyrollw=-100;

   

    Loutput = output-joyrollw;

    Routput = output+joyrollw;

    //------- prepare PWM for both LEFT an RIGHT motors --------

    if (Loutput < 0) {

      digitalWrite(IN1, HIGH);

      digitalWrite(IN2, LOW);

    } else if (Loutput > 0) {

      digitalWrite(IN1, LOW);

      digitalWrite(IN2, HIGH);     

    } else {

      digitalWrite(IN1, HIGH);

      digitalWrite(IN2, HIGH);     

    }

    if (Routput < 0) {

      digitalWrite(IN3, LOW);

      digitalWrite(IN4, HIGH);

    } else if (Loutput > 0) {

      digitalWrite(IN3, HIGH);

      digitalWrite(IN4, LOW);      

    } else {

      digitalWrite(IN3, HIGH);

      digitalWrite(IN4, HIGH);     

    }

    int tempLspeed = constrain(abs(Loutput), reladi, 255);   

    int tempRspeed = constrain(abs(Routput), reladi, 255);

    analogWrite(ENA, tempLspeed);

    analogWrite(ENB, tempRspeed);

  }

}

// ================================================================

// ======================== END OF PROGRAM ========================

// ================================================================

// ======= LEGEND =================================================

// No of channel received from the Spectrum DX-7 receiver

// Channel 1 = Roll

// Channel 2 = Throttle

// Channel 3 = Pitch

// Channel 4 = Yaw

// Channel 5 = Throttle (same as Channel 2)

// Channel 6 = 3pos Switch

// Channel 7 = Aux2 switch

//------------------------------------------------------------------

Photos of the hardware components:

L298 controller

Arduino NANO

 MPU6050

 DC motor with gearbox (Left)

DC motor with gearbox (Right)

 11,1 volts LIPO battery

I hope you enjoy your own project.