Self Balancing Robot

07.11.17

Well finally the robot is finished, for now, and I have also managed to get the website up and running. There is quite a bit of information related to this project so i have broken it down into sections below, however please take a look at the YouTube videos as they explain the process. If you have any questions, please get in contact.

 

3D Printed Frame

All the design work was completed in Fusion 360 and I have included the design files below:

Robot Wheel

Robot Frame Side

Robot Frame Top

Robot Frame Bottom

Robot Frame Back

 

Schematic Diagram

The Schematic Diagram is below, I have also included a link to the YouTube video that runs through the schematic in detail.

Robot Schematic

This YouTube video provides a detailed discussion on the schematic diagram.

YouTube Video for Self Balancing Robot Schematic

 

Arduino Code 

The Arduino Code has been broken out into two classes and the main Module. Please follow the instructions below to import the code.

YouTube link below gives a run through as well:

Self Balancing Code

Main Module

The following code can be imported into the main code module (SkaterBot.ino):

//
// Terms of use
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.


//
//  SkaterBot
//

#include "MPU.h"
#include "PID.h"

// Pins
#define batteryLowIndicator 13
#define leftPulse           2
#define leftDirection       3
#define rightPulse          4
#define rightDirection      5

// PID Paramaters
#define pidP      15
#define pidI      1.5
#define pidD      2                                                                                                                                                                        
#define pidDB     5
#define pidOPMin  -400
#define pidOPMax  400

// Dynamic Paramaters
float turnSpeed = 30;
float maxSpeed = 150;

// State Engine
int robotState;
#define Starting  0
#define Balancing 1

// Battery Monitoring
int   batteryVoltage;
bool  lowBattery;

// Bluetooth Comms
byte  receivedByte;
int   receivedCounter;

// Loop Timing
unsigned long loopTimer;
unsigned long reportCounter;

// Control Variables
float angle;
float outputLeft;
float outputRight;

int leftMotor;
int throttleLeftMotor;
int throttleCounterLeftMotor;
int throttleLeftMotorMemory;

int rightMotor;
int throttleRightMotor;
int throttleCounterRightMotor;
int throttleRightMotorMemory;

// Class Instances
MPU mpu(0x68);          // MPU-6050 I2C address (0x68 or 0x69)
PID pid;

//******************
// SETUP
//******************

void setup() {

  Serial.begin(9600);           //Start the serial port at 9600 bps

    pinMode(batteryLowIndicator, OUTPUT);
    pinMode(leftPulse, OUTPUT);
    pinMode(leftDirection, OUTPUT);
    pinMode(rightPulse, OUTPUT);
    pinMode(rightDirection, OUTPUT);
  
  //To create a variable pulse for controlling the stepper motors a timer is created that will execute a piece of code (subroutine) every 20us
  //This subroutine is called TIMER2_COMPA_vect
  TCCR2A = 0;                                                               //Make sure that the TCCR2A register is set to zero
  TCCR2B = 0;                                                               //Make sure that the TCCR2A register is set to zero
  TIMSK2 |= (1 << OCIE2A);                                                  //Set the interupt enable bit OCIE2A in the TIMSK2 register
  TCCR2B |= (1 << CS21);                                                    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
  OCR2A = 39;                                                               //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1
  TCCR2A |= (1 << WGM21);                                                   //Set counter 2 to CTC (clear timer on compare) mode

  // Setup PID
  pid.reset();
  pid.setTuningParameters(pidP, pidI, pidD);
  pid.setDeadband(pidDB);
  pid.setOutputLimited(pidOPMin, pidOPMax);

  // Initialise the MPU
  mpu.initialise();

  // get calibration values
  mpu.getGyroCalibrationValues();
 
  robotState = Starting;
  loopTimer = micros() + 4000;
}


//******************
// LOOP
//******************

void loop() {

  if (Serial.available()) {
    receivedByte = Serial.read();
    receivedCounter = 0;
  }

  if (receivedCounter <= 25) {
    receivedCounter++;
  }
  else {
    receivedByte = 0x00;
  }
  
  batteryVoltage = map(analogRead(0),0,1023,0,1250);

  if (batteryVoltage < 1070 && batteryVoltage > 1050) {
    Serial.print("Battery Warning");
  }
  
  if (batteryVoltage < 1050 && batteryVoltage > 800) {
    digitalWrite(batteryLowIndicator, HIGH);
    lowBattery = true;  
    Serial.print("Battery Low");
  }
 
  // State Machine
  switch (robotState) {
    case Starting:
    {
      angle = mpu.getAccelAngle();
      if (angle > -0.5 && angle < 0.5) {
        mpu.setTiltAngle(angle);
        robotState = Balancing;
        
      }
      break;
    }
    case Balancing:
    {
      angle = mpu.getTiltAngle();
      if (angle > 30 || angle < -30 || lowBattery) {
        robotState = Starting;
        pid.reset();
      }
      else {
        // Process PID
        pid.processVariable = angle;
        pid.calculate();
      }
      break;
    }
  }

  // Calculate control signals
  outputLeft = pid.output;
  outputRight = pid.output;

  // Turn Left
  if (receivedByte & 0b00000010) {
    outputLeft += turnSpeed;
    outputRight -= turnSpeed;
  }

  // Turn Right
  if (receivedByte & 0b00000001) {
    outputLeft -= turnSpeed;
    outputRight += turnSpeed;
  }

  // Forward
  if (receivedByte & 0b00000100) {
    if (pid.setpoint > -2.5) {
      pid.setpoint -= 0.05;
    }
    if (pid.output > -maxSpeed) {
      pid.setpoint -= 0.005;
    }
  }
 
  // Reverse
  if (receivedByte & 0b00001000) {
    if (pid.setpoint < 2.5) {
      pid.setpoint += 0.05;
    }
    if (pid.output < maxSpeed) {
      pid.setpoint += 0.005;
    }
  }

  // Not forward or reverse
  if (!(receivedByte & B00001100)) {
    if (pid.setpoint > 0.5) {
      pid.setpoint -= 0.05; 
    }
    else if (pid.setpoint < -0.5) {
      pid.setpoint += 0.05;
    }
    else {
      pid.setpoint = 0;
    }
  }

  if (pid.setpoint == 0) {
    if (pid.output < 0) {
      pid.selfBalanceSetpoint += 0.0015;
    }
    if (pid.output > 0) {
      pid.selfBalanceSetpoint -= 0.0015;
    }
  }

  // Motor Calculations

  // Compensate for the non-linear behaviour of the stepper motors
  if (outputLeft > 0) {
    outputLeft = 405 - (1/(outputLeft + 9)) * 5500;
  }
  else if (outputLeft < 0) {
    outputLeft = -405 - (1/(outputLeft - 9)) * 5500;
  }
  
  if (outputRight > 0) {
    outputRight = 405 - (1/(outputRight + 9)) * 5500;
  }
  else if (outputRight < 0) {
    outputRight = -405 - (1/(outputRight - 9)) * 5500;
  }

  // Calculate the needed pulse time for the left and right stepper motor controllers
  if (outputLeft > 0) {
    leftMotor = 400 - outputLeft;
  }
  else if (outputLeft < 0) {
    leftMotor = -400 - outputLeft;
  }
  else {
    leftMotor = 0;
  }

  if (outputRight > 0) {
    rightMotor = 400 - outputRight;
  }
  else if (outputRight < 0) {
    rightMotor = -400 - outputRight;
  }
  else {
    rightMotor = 0;
  }

  // Copy for interrupt to use
  throttleLeftMotor = leftMotor;
  throttleRightMotor = rightMotor;

  if (reportCounter > 50) {
    reportCounter = 0;
  
//    Serial.print(angle);
//    Serial.print(",");
//    Serial.print(pid.selfBalanceSetpoint);
//    Serial.print(",");
//    Serial.print(pid.output);
//    Serial.print(",");
//    Serial.print(throttleLeftMotor);
//    Serial.print(",");
//    Serial.print(throttleRightMotor);
//    Serial.println("");
  }
 
  // Delay 4 milliseconds
  while(loopTimer > micros());
  loopTimer += 4000;
  reportCounter ++;
}

//******************
// INTERUPT HANDLERS
//******************

ISR(TIMER2_COMPA_vect) {
  
  // Left motor pulse calculations
  throttleCounterLeftMotor ++;                                //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
  if (throttleCounterLeftMotor > throttleLeftMotorMemory) {   //If the number of loops is larger then the throttle_left_motor_memory variable
    throttleCounterLeftMotor = 0;                             //Reset the throttle_counter_left_motor variable
    throttleLeftMotorMemory = throttleLeftMotor;              //Load the next throttle_left_motor variable
    if (throttleLeftMotorMemory < 0) {                        //If the throttle_left_motor_memory is negative
      PORTD &= 0b11110111;                                    //Set output 3 low to reverse the direction of the stepper controller
      throttleLeftMotorMemory *= -1;                          //Invert the throttle_left_motor_memory variable
    }
    else {
      PORTD |= 0b00001000;                                    // Set output 3 high for a forward direction of the stepper motor
    }
  }
  else if (throttleCounterLeftMotor == 1) {
    PORTD |= 0b00000100;                                      //Set output 2 high to create a pulse for the stepper controller
  }
  else if (throttleCounterLeftMotor == 2) {
    PORTD &= 0b11111011;                                      //Set output 2 low because the pulse only has to last for 20us 
  }

  // Right motor pulse calculations
  throttleCounterRightMotor ++;                               //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  if (throttleCounterRightMotor > throttleRightMotorMemory) { //If the number of loops is larger then the throttle_right_motor_memory variable
    throttleCounterRightMotor = 0;                            //Reset the throttle_counter_right_motor variable
    throttleRightMotorMemory = throttleRightMotor;            //Load the next throttle_right_motor variable
    if (throttleRightMotorMemory < 0) {                       //If the throttle_right_motor_memory is negative
      PORTD |= 0b00100000;                                    //Set output 5 low to reverse the direction of the stepper controller
      throttleRightMotorMemory *= -1;                         //Invert the throttle_right_motor_memory variable
    }
    else {
      PORTD &= 0b11011111;                                    //Set output 5 high for a forward direction of the stepper motor
    }
  }
  else if (throttleCounterRightMotor == 1) {
    PORTD |= 0b00010000;                                      //Set output 4 high to create a pulse for the stepper controller
  }
  else if (throttleCounterRightMotor == 2) {
    PORTD &= 0b11101111;                                      //Set output 4 low because the pulse only has to last for 20us
  }
}

 

MPU Class

Create a new Tab abd name the File MPU.h

Copy the following code to this new tab.

#define MPU_REG_PWR_MGMT_1       0x6B

#define RAD2DEGREE               57.296;

class MPU
{
  public:
    MPU(int address);
    void initialise();
    void getGyroCalibrationValues();
    float getAccelAngle();
    void setTiltAngle(float angle);
    float getTiltAngle();

  private:
    int   accelCalibrationValue = -187;   // Enter the accelerometer calibration value
    int   busAddress;
    long  gyroYawCalibrationValue;
    long  gyroPitchCalibrationValue;
    float tiltAngle;

  private:
    void reset();
    void wakeup();
    void setGyroFullScaleRange();
    void setAccelFullScaleRange();
    void setLowPassFilter();
    long getRotationX();
    long getRotationY();
    long getRotationZ();
    long getAccelerationX();
    long getAccelerationY();
    long getAccelerationZ();
};

#endif

Create a new Tab abd name the File MPU.cpp 

Copy the following code to this new tab.

//
// MPU Class
//

#include "Arduino.h"
#include "MPU.h"

MPU::MPU(int address) {

  busAddress = address; 

  Wire.begin();   //Start the I2C bus as master
  TWBR = 12;      //Set the I2C clock speed to 400kHz
}

void MPU::initialise() {

  //reset();
  wakeup();
  setGyroFullScaleRange();
  setAccelFullScaleRange();
  setLowPassFilter();
}

void MPU::getGyroCalibrationValues() {

  int loopCounter;
  
  for (loopCounter = 0; loopCounter < 500; loopCounter++) {

    gyroYawCalibrationValue += getRotationX();
    gyroPitchCalibrationValue += getRotationY();
    delayMicroseconds(4000);  
  }
  
  gyroYawCalibrationValue /= 500;
  gyroPitchCalibrationValue /= 500;
}

float MPU::getAccelAngle() {
  
  long accelRawData;
  float accelAngle;

  //Get the Accel Angle
  accelRawData = getAccelerationZ() + accelCalibrationValue;
  constrain(accelRawData, -8200, 8200);
  accelAngle = asin((float)accelRawData / 8200.0) * RAD2DEGREE;
  return accelAngle;
}

void MPU::setTiltAngle(float angle) {

  tiltAngle = angle;
}

float MPU::getTiltAngle() {

  long gyroYawRawData;
  long gyroPitchRawData;
  long accelRawData;
  float accelAngle;

  //Get the Accel Angle
  accelRawData = getAccelerationZ() + accelCalibrationValue;
  constrain(accelRawData, -8200, 8200);
  accelAngle = asin((float)accelRawData / 8200.0) * RAD2DEGREE;

  gyroYawRawData = getRotationX();
  gyroPitchRawData = getRotationY() - gyroPitchCalibrationValue;
  tiltAngle += gyroPitchRawData * 0.000031;  // update with this loops travelled value

  // Compensate for mounting try -0.0000003 or 0.0000003 if required
  gyroYawRawData -= gyroYawCalibrationValue;
  //tiltAngle -= gyroYawRawData * 0.0000003;  
  
  // Correct the drift of the gyro angle with the accelerometer angle
  tiltAngle = tiltAngle * 0.9996 + accelAngle * 0.0004; 
  return tiltAngle;
}

void MPU::reset() {
  
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_PWR_MGMT_1);
  Wire.write(0b10000000);
  Wire.endTransmission();
}

void MPU::wakeup() {
  
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_PWR_MGMT_1);
  Wire.write(0b00000000);
  Wire.endTransmission();
}

void MPU::setGyroFullScaleRange() {
  
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_GYRO_CONFIG);
  Wire.write(0b00000000);             // +/- 250 derees per second
  Wire.endTransmission();
}

void MPU::setAccelFullScaleRange() {
  
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_ACCEL_CONFIG);
  Wire.write(0b00001000);             // +/- 4G
  Wire.endTransmission();
}

void MPU::setLowPassFilter() {
  
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_CONFIG);
  Wire.write(0b00000011);             // 42/44 Hz
  Wire.endTransmission();
}

long MPU::getRotationX() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_GYRO_XOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

long MPU::getRotationY() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_GYRO_YOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

long MPU::getRotationZ() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_GYRO_ZOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

long MPU::getAccelerationX() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_ACCEL_XOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

long MPU::getAccelerationY() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_ACCEL_YOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

long MPU::getAccelerationZ() {

  long value;
  Wire.beginTransmission(busAddress);
  Wire.write(MPU_REG_ACCEL_ZOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(busAddress, 2);
  value = Wire.read() << 8 | Wire.read();
  return value;
}

 

PID Class

Create a new Tab abd name the File PID.h

Copy the following code to this new tab.

//
// PID Class
//

#ifndef PID_H
#define PID_H

#include "Arduino.h"

class PID
{
  public:
    PID();
    void reset();
    void setTuningParameters(float p, float i, float d);
    void setDeadband(float db);
    void setOutputLimited(float opMin, float opMax);
    float calculate();

  private:
    float pTerm;
    float iTerm;
    float dTerm;
    float iMemory;
    float lastError;
    float outputMin;
    float outputMax;
    float deadband;

  public:
    float setpoint;
    float processVariable;
    float output;
    float selfBalanceSetpoint;
};

#endif

 

Create a new Tab abd name the File PID.cpp

Copy the following code to this new tab.

//
// PID Class
//

#include "Arduino.h"
#include "PID.h"

PID::PID() {
  iMemory = 0;
  lastError = 0;
  outputMin = 0;
  outputMax = 100;
  deadband = 0;
}

void PID::reset() {
  output = 0;
  iMemory = 0;
  selfBalanceSetpoint = 0;
}

void PID::setTuningParameters(float p, float i, float d) {
  pTerm = p;
  iTerm = i;
  dTerm = d;
}
    
void PID::setDeadband(float db) {
  deadband = db;
}
    
void PID::setOutputLimited(float opMin, float opMax) {
  outputMin = opMin;
  outputMax = opMax;
  iMemory = constrain(output, outputMin, outputMax);
}

float PID::calculate() {
  
  float error;
  error = processVariable - setpoint - selfBalanceSetpoint;

  // Brake function
  if (output > 10 || output < -10) {
    error += output * 0.015;
  }

  iMemory += iTerm * error;
  iMemory = constrain(iMemory, outputMin, outputMax);

  output = pTerm * error + iMemory + dTerm * (error - lastError);
  output = constrain(output, outputMin, outputMax);
  
  lastError = error;

  if (output < deadband && output > -deadband) {
    output = 0;
  }
}

This completes the Arduino code import.

IMPORTANT

When you load the code to the Arduino its important to remove the Bluetooth module as it shares the Serial RX and TX with the USB programming port.

iOS Controller Code

I haven't created a GIT repository for this code yest so if you would like a copy please email me and I will forward it on to you.

The video below will give you a rundown on the code through.

iOS Controller code

 

Comments (4)

  • Toàn 10.05.18

    Can you sent me the code ? Thanks very much

  • Grumpy 17.05.18

    Its available from Github now https://github.com/Grumpy-Old-Tech/SkaterBot the new version 2 robot code is up there as well.

  • Goh PH 02.08.18

    I would appreciate if you can provide full schematics & all arduino codes & library for Self Balancing Robot version 1. I tried copying the code & library to arduino IDE & encountered some errors while compiling. For a start I will try to build the version 1 robot as I have most of the parts. If everything is fine, I will go for the version 2. Thanks & best regards.

  • Grumpy 18.08.18

    Please check comments above for a link to github where you can find the code, schematic is above. If you would like more information. Can you please contact me at grumpyoldtech@outlook.com.au


Leave a Comment