Self Balancing Bot - Arduino

6
Jun
2018

Here is the code for Self Balancing Bot using Arduino UNO, MPU6050, L298N motor driver. I've used Jeff Rowberg's MPU6050 library to calculate Yaw, Pitch and Roll. In my build only Pitch is of importance.

For the next update, I'm going to move the bot using Blynk app and Bluetooth module.

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
#include <PID_v1.h>

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  #include "Wire.h"
#endif

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

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

//POTENTIOMETERS FOR CHANGING Kp, Kd, Ki in real time
static int pot1Pin = A0;
static int pot2Pin = A1;
static int pot3Pin = A2;

double Kp=0 , Ki=0, Kd=0;
double prevKp=0, prevKi=0, prevKd=0;

double input=0, output;

PID pid(&input, &output, 0, Kp, Ki, Kd, DIRECT); //<-------PID defined here!!

// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

// ================================================================
// === INITIAL SETUP ===
// ================================================================

void setup() {
    Serial.begin(9600);    
    
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
            Wire.begin();
            TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
            Fastwire::setup(400, true);
    #endif
    
    while (!Serial); 
    
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
    if (devStatus == 0) {
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();

        //setup PID
        
        pid.SetMode(AUTOMATIC); //AUTOMATIC or MANUAL --> defaults to MANUAL
        pid.SetSampleTime(10); //how often PID is computed
        pid.SetOutputLimits(-255, 255); //defaults to 0 to 255
        
    } else {
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    pinMode(6,OUTPUT);
    pinMode(7,OUTPUT);
    pinMode(9, OUTPUT); //motor pwm
    pinMode(10, OUTPUT); //motor pwm
    pinMode(pot1Pin, INPUT);
    pinMode(pot2Pin, INPUT);
    pinMode(pot3Pin, INPUT);
}

// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================

void loop() {
    if (!dmpReady) return;
    while (!mpuInterrupt && fifoCount < packetSize) {
      
    }
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);mpu.resetFIFO();
        fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            input = (ypr[1] * 180/M_PI)/2;
#endif
       
        pid.Compute();
        Drive_Motor(output);
        setPIDTuningValues();
    }
}

double Drive_Motor(double torque)  {
  if (torque >= 0)  {    
    // drive motors forward
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);   
  }  else {   
    // drive motors backward    
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);    
  }
  torque = abs(torque);
  analogWrite(9, torque);
  analogWrite(10, torque);
}

void setPIDTuningValues()
{
    readPIDTuningValues();    
    if (Kp != prevKp || Ki != prevKi || Kd != prevKd)
    {
      Serial.print(Kp);Serial.print(", ");Serial.print(Ki);Serial.print(", ");Serial.println(Kd);
      prevKp = Kp; prevKi = Ki; prevKd = Kd;
    }
    pid.SetTunings(Kp, Ki, Kd);
}
void readPIDTuningValues()
{
    int potKp = analogRead(A0);
    int potKi = analogRead(A1);
    int potKd = analogRead(A2);
        
    Kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
    Ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
    Kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}

By administrator at 06:55:45 PM 0 Comment(s)

Comments

There are no comments yet. Be the first one to add a comment!

Add a Comment

Please enter the email address.Invalid format. (Won't be Displayed)
Notify me of followup comments
Enter the displayed Code: captcha