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.

PPM encoder

;=====================================================
;======== HARDWARE ===================================
;=====================================================
;MPU is AVR ATMEGA328P AT 8MHz internal clock.
;2KB Data_Ram, 1KB EEPROM, 32KB Flash_program_memory.
;
;
;
;=====================================================
.include "m328pdef.inc"
;
;================================================================
;
.def AL=R16
.def AH=R17
.def temp=R18
.def char=R19
.def BL=R20
.def BH=R21
.def CL=R22
.def CH=R23
;
;--------------------------
;------ MACROS ------------
;--------------------------
.macro cpiw
 cpi @0L,low(@1)
 ldi r16,high(@1)
 cpc @0H,r16
.endm
;
;----------------
.macro ldiw
 ldi @0L,low(@1)
 ldi @0H,high(@1)
.endm
;
;----------------
.macro outi
 ldi r16,@1
 out @0,r16
.endm
;
;============================================
;==== CODE START   ==========================
;
.cseg
.org $0000
;
;============================================
;=====  INTERRUPT VECTORS ATMEGA328   =========
;
 jmp RESET   ;Reset Handler
 jmp NOTUSEDHERE ;EXT_INT0 ;IRQ0 Handler
 jmp NOTUSEDHERE ;EXT_INT1 ;IRQ1 Handler
 jmp PCINT0i   ;PCINT0 Handler for portB
 jmp NOTUSEDHERE ;PCINT1i ;PCINT1 Handler for portC
 jmp NOTUSEDHERE ;PCINT2i ;PCINT2 Handler for port D
 jmp NOTUSEDHERE ;WDT ; Watchdog Timer Handler
 jmp NOTUSEDHERE ;TIM2_COMPA ; Timer2 Compare A Handler
 jmp NOTUSEDHERE ;TIM2_COMPB ; Timer2 Compare B Handler
 jmp TIM2_OVF  ; Timer2 Overflow Handler
 jmp NOTUSEDHERE ;TIM1_CAPT ; Timer1 Capture Handler
 jmp NOTUSEDHERE ;TIM1_COMPA ; Timer1 Compare A Handler
 jmp NOTUSEDHERE ;TIM1_COMPB ; Timer1 Compare B Handler
 jmp NOTUSEDHERE ;TIM1_OVF ; Timer1 Overflow Handler
 jmp NOTUSEDHERE ;TIM0_COMPA ; Timer0 Compare A Handler
 jmp NOTUSEDHERE ;TIM0_COMPB ; Timer0 Compare B Handler
 jmp NOTUSEDHERE ;TIM0_OVF ; Timer0 Overflow Handler
 jmp NOTUSEDHERE ;SPI_STC ; SPI Transfer Complete Handler
 jmp NOTUSEDHERE  ; USART, RX Complete Handler
 jmp NOTUSEDHERE ;USART_UDRE ; USART, UDR Empty Handler
 jmp NOTUSEDHERE ;USART_TXC ; USART, TX Complete Handler
 jmp NOTUSEDHERE ;ADC ; ADC Conversion Complete Handler
 jmp NOTUSEDHERE ;EE_RDY ; EEPROM Ready Handler
 jmp NOTUSEDHERE ;ANA_COMP ; Analog Comparator Handler
 jmp NOTUSEDHERE ;TWI ; 2-wire Serial Interface Handler
 jmp NOTUSEDHERE ;SPM_RDY ; Store Program Memory Ready Handler
;
NOTUSEDHERE: reti
;
;============================================================
;=== MAIN PROGRAM STARTS HERE ===============================
;============================================================
RESET:
;initialize Stack_Pointer
 ldi  temp,low(RAMEND)
 out  SPL,temp
 ldi  temp,high(RAMEND)
 out  SPH,temp
;
;============================================================
;================== Clear RAM ===============================
;============================================================
 ldiw Z, RAMEND+1
 clr temp
loop_clr_RAM:   
 st -Z, temp  
 cpiw Z, SRAM_START   
 brne loop_clr_RAM
;
;============================================================
;================= Ports setup ==============================
;============================================================
 outi PORTB,0b00000000 ;Initialize Port B as input
 outi DDRB, 0b00000000 ;      
 outi PORTD, 0b00000000 ;Initialize Port D as output=0    
     ;Pind,7=PPM_OUTPUT   
     ;Pind,5=Led output
 outi DDRD,  0b11111111 

;
;============================================================
;============ Pin change interrupt setup ====================
;============================================================
 ;               76543210
 ldi temp, 0b00000001 ;All pins of portB active for Pin-Change-Interrupt function.
 sts  pcicr,temp
 ;               76543210
 ldi  temp, 0b11111111 ;All PortB pins will react at Pin-Change-Interrupt.
 sts  pcmsk0,temp
;
;============================================================
;===================== Timer2 init ==========================
;============================================================
 ; Set prescale and start Timer/Counter1
 ldi r16,(1<<CS12)|(1<<CS10) ;Set prscaler=128 (8MHz/128)
 sts TCCR2B,r16  ;Cycle_period=16us (0,000016)
 ldi r16,(1<<TOIE1) 
 sts TIMSK2, R16

;      
;----------------------------------------------
;-----------------Init vars -------------------
;
 cbi portd,7   ;PPM_OUTPUT
 cbi portd,5   ;Led
 set     ;t bit used as flag

 ser AH
 clr BL
 sbi Portd,5   ;Turn LED ON. (I am alive!)
 sei     ;Enable interrupts
; 
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;@@@@@@@@@@@@ MAIN LOOP @@@@@@@@@@@@@@@@@@@@@
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;
main:   ;loop forever.
rjmp main
;
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;@@@@@@@@@@@@ END OF MAIN LOOP @@@@@@@@@@@@@@
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;@@@@@@@@ Interrupt service routines @@@@@@@@
;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
;
PCINT0i:  ; PCINT0 Handler. Got an interrupt from PortB, start the channel pulse.
 push  AL
 in  AL,SREG
 push  AL

 brtc tisnotone
 in AL,PINB
 andi AL, 0b00111111
 breq startpulse
 clt
startpulse:
 sbi portd,7
 ldi temp, 0b00000101 ;Enable timer2
 sts TCCR2B,temp
 ldi AL,-25   ;TCNT=255-25=230 for 400usec pulse
 sts TCNT2,AL
 rjmp finishinterrupt
tisnotone:
 in AL,PINB
 andi AL, 0b00111111
 breq startpulse
finishinterrupt:
 pop  AL
 out  SREG,AL
 pop  AL
 reti
;
;================================= TIMER2 OVF
;
TIM2_OVF:    ;Finish the channel pulse
 push  AL
 in  AL,SREG
 push AL
 ldi temp, 0b00000000 ;Disable timer2
 sts TCCR2B,temp
 cbi portd,7
 pop  AL
 out  SREG,AL
 pop  AL
 reti
; -------------------------------------------------------

HEX  file of above listing.

:020000020000FC
:100000000C9435000C9434000C9434000C945D0076
:100010000C9434000C9434000C9434000C94340090
:100020000C9434000C9474000C9434000C94340040
:100030000C9434000C9434000C9434000C94340070
:100040000C9434000C9434000C9434000C94340060
:100050000C9434000C9434000C9434000C94340050
:100060000C9434000C94340018952FEF2DBF28E029
:100070002EBFE0E0F9E022272293E03001E0F00714
:10008000D9F700E005B900E004B900E00BB90FEFC3
:100090000AB921E0209368002FEF20936B0005E060
:1000A0000093B10001E0009370005F985D98689440
:1000B0001FEF44275D9A7894FFCF0F930FB70F93EC
:1000C00066F403B10F7309F0E8945F9A25E020937A
:1000D000B10007EE0093B20003C003B10F73A9F3A0
:1000E0000F910FBF0F9118950F930FB70F9320E04B
:0E00F0002093B1005F980F910FBF0F911895EC
:00000001FF

Use an AVR programmer to write the above hex-file and enjoy an absolutely reliable

PPM-Encoder.