Bulding first balancing robot

Status
Not open for further replies.

Black_Stork

Member
I need guidance with building(programing) my first balancing robot.

Tasks that I want to accomplish :
- program robot so it can stand on its own.
- then add remote control over bluetooth so it go forward/back, left/right

Initialy i want to do this with already avaible libraries, then maybe I'll try to to imlement PID from scratch.

------------------------------------------------------------------------------------------------------------------------------------------------------------

PARTS:
-Teensy 3.2
-Pololu DRV8835 Dual Motor Driver Carrier --> https://www.pololu.com/product/2135
-HC 05v2 x2 bluetooth modue --> https://botland.com.pl/moduly-bluet...05-v2.html?search_query=bluetooth&results=279
-SparkFun 9DoF Sensor Stick LSM9DS1 --> https://www.sparkfun.com/products/13944 --> LIBRARY: https://github.com/sparkfun/SparkFun_LSM9DS1_Arduino_Library
-DC Motors 120RPM x2 --> https://www.pololu.com/product/992
-wheels --> https://www.pololu.com/product/1420
-2AA battery holder + 2AA batteries,
-4AA battery holder +4AA batteries, or 9V battery
-breadbord
-2 plexi plates
-4 tapped rods
-16 nuts

PHOTOS:
https://drive.google.com/file/d/0B0iA-Iu_GinvQUFacGFuZ2VDcU0/view?usp=sharing
https://drive.google.com/file/d/0B0iA-Iu_GinvRXc1M2J4RWNiSWs/view?usp=sharing
https://drive.google.com/file/d/0B0iA-Iu_GinvdmU2SWVLOXAyRVE/view?usp=sharing
https://drive.google.com/file/d/0B0iA-Iu_Ginvd1RfQmw2T0ZFalk/view?usp=sharing ,(Orange cables on the photo are disconected form cicuit)
https://drive.google.com/file/d/0B0iA-Iu_GinvcmlkZ3FVSnRmWW8/view?usp=sharing
https://drive.google.com/file/d/0B0iA-Iu_GinvTDAxclNpRWZ4YmM/view?usp=sharing

WIRING:
pololdriver.pngPololudriver1.jpg

--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

1. I want to mount IMU on top of robot, would it be better to mount it on th bottom?
2. How to calibrate IMU? How to analyze data from IMU?
3. What PID libraries do you recomend?
4. And how to use them?

I've already checked if if driver works correctly, i managed to spin engine back and forward with INPUT_PULLUP command. I was able to connect with slave module from my phone.
With IMU I uploaded I2C example and it reacted to pitch, and roll correctly, but yank acted weirdly, I guess that it needs proper calibration.
 
[UPDATE]
Here is some code that i wrote for motor control and for IMU (imu code was based on code in Joop Broking's video about MPU 6050):

Motors.hpp

Code:
#include "Arduino.h"
enum action {BACK = LOW, FORWARD = HIGH};
///////// DC_Motor ////////////////////////////////////////////////////////////
class DC_Motor
{
    int __rpm, __drct;
    int __ENABLE_PIN, __PHASE_PIN;

public:
    DC_Motor(int, int);
    void move(int, int=255);
};
///////// MotorControler //////////////////////////////////////////////////////
class MotorControler
{
    DC_Motor* __LeftMotor;
    DC_Motor* __RightMotor;

public:
    MotorControler(DC_Motor &, DC_Motor &);
    void moveForward(int=255);
    void moveBack(int=255);
    void spinLeft(int=255);
    void spinRight(int=255);
    void turnLeft(int=255);
    void turnRight(int=255);
};

Motors.cpp

Code:
#include "Motors.hpp"

///////////////// DC_Motor ////////////////////////////////////////////////.
DC_Motor::DC_Motor(int E_PIN, int P_PIN) : __ENABLE_PIN(E_PIN),
__PHASE_PIN(P_PIN), __rpm(0), __drct(0)
{
    pinMode(__ENABLE_PIN, OUTPUT);
    pinMode(__PHASE_PIN, OUTPUT);
}

void DC_Motor::move(int direction, int pwm)
{
    __rpm = pwm;
    __drct = direction;
    digitalWrite(__PHASE_PIN, direction);
    analogWrite(__ENABLE_PIN, pwm);
}

////////////////// MotorControler ///////////////////////////////////////////

MotorControler::MotorControler(DC_Motor &left, DC_Motor &right)
{
    pinMode(7, OUTPUT);
    digitalWrite(7, HIGH); //enables mode 1 in DRV8335
    __LeftMotor = &left;
    __RightMotor = &right;
}

void MotorControler::moveForward(int pwm)
{
    //__RightMotor->move(FORWARD, pwm);
    __LeftMotor->move(FORWARD, pwm);
    __RightMotor->move(FORWARD, pwm);
}

void MotorControler::moveBack(int pwm)
{
    __LeftMotor->move(BACK, pwm);
    __RightMotor->move(BACK, pwm);
}

void MotorControler::spinLeft(int pwm)
{
    __LeftMotor->move(BACK, pwm);
    __RightMotor->move(FORWARD, pwm);
}

void MotorControler::spinRight(int pwm)
{
    __LeftMotor->move(FORWARD, pwm);
    __RightMotor->move(BACK, pwm);
}

void MotorControler::turnLeft(int pwm)
{
    __LeftMotor->move(BACK, 0);
    __RightMotor->move(FORWARD, pwm);
}

void MotorControler::turnRight(int pwm)
{
    __LeftMotor->move(FORWARD, 0);
    __RightMotor->move(FORWARD, pwm);
}

Sensors.hpp

Code:
#include "Wire.h"
#include "SparkFunLSM9DS1.h"
#include "SPI.h"

class Sensor :public LSM9DS1
{
public:
    int gyroX, gyroY, gyroZ;
    float accX, accY, accZ, accVector;

    long gyroXcal, gyroYcal, gyroZcal;

    float angleRoll_G, anglePitch_G;
    float angleRoll_A, anglePitch_A;

    float angleRoll_Sum, anglePitch_Sum;
public:
    Sensor();
    void sensorCheck();
    void readData();
    void calibrateManual(int=2000);
};

Sensors.cpp

Code:
#include "Sensors.hpp"

Sensor::Sensor()
{
    settings.device.commInterface = IMU_MODE_I2C;
    settings.device.mAddress = 0x1E;
    settings.device.agAddress = 0x6B;
}

void Sensor::sensorCheck()
{
    if (!begin())
    {
    Serial.println("Failed to communicate with LSM9DS1.");
    Serial.println("Double-check wiring.");
    Serial.println("Default settings in this sketch will " \
                  "work for an out of the box LSM9DS1 " \
                  "Breakout, but may need to be modified " \
                  "if the board jumpers are.");
    while (1)
      ;
    }
    else { Serial.print("Communication checked"); }
}

void Sensor::readData()
{
    readGyro();
    gx -= gyroXcal;
    gy -= gyroYcal;
    gz -= gyroZcal;

    gyroX = calcGyro(gx);
    gyroY = calcGyro(gy);
    gyroZ = calcGyro(gz);

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

    readAccel();
    accX = calcAccel(ax);
    accY = calcAccel(ay);
    accZ = calcAccel(az) - 0.5;

    accVector = sqrt((accX*accX) + (accY*accY) + (accZ*accZ));
}

void Sensor::calibrateManual(int loops)
{
    for(int i = 0; i < loops; i++)
    {
        readGyro();
        gyroXcal += gx;
        gyroYcal += gy;
        gyroZcal += gz;
        delay(3);
    }
    gyroXcal /= loops;
    gyroYcal /= loops;
    gyroZcal /= loops;
}

and main.cpp

Code:
#include "Motors.hpp"
#include "Sensors.hpp"

const int LED = 13;

DC_Motor leftMotor(5, 6);
DC_Motor rightMotor(3, 4);
MotorControler motors(leftMotor, rightMotor);

Sensor Imu;
bool IsItSet = false;
long loop_timer;
void setup()
{
    Wire.begin();
    Serial.begin(38400);

    pinMode(LED, OUTPUT);
    digitalWrite(LED, HIGH);
    Imu.sensorCheck();
    Imu.calibrateManual(2000);

    delay(300);
    digitalWrite(LED, LOW);

    loop_timer = micros();
}

void loop()
{
    Imu.readData();


    Imu.angleRoll_G += Imu.gyroX / 125.0;
    Imu.anglePitch_G += Imu.gyroY / 125.0;

    Imu.angleRoll_G += Imu.anglePitch_G * sin(Imu.gyroZ / 125 * (3.142/180));
    Imu.anglePitch_G -= Imu.angleRoll_G * sin(Imu.gyroZ / 125 * (3.142/180));
    //--------------------------------------------------------------------------
    Imu.angleRoll_A = asin(Imu.accY/Imu.accVector)* 57.296;
    Imu.anglePitch_A = asin(Imu.accX/Imu.accVector)* -57.296;

    if(IsItSet)
    {
        Imu.angleRoll_G = Imu.angleRoll_G * 0.9 + Imu.angleRoll_A * 0.1;
        Imu.anglePitch_G = Imu.anglePitch_G * 0.9 + Imu.anglePitch_A * 0.1;
    }
    else
    {
        Imu.anglePitch_G = Imu.anglePitch_A;
        Imu.angleRoll_G = Imu.angleRoll_A;
        IsItSet = true;
    }

    Serial.print(Imu.angleRoll_G);
    Serial.print(", ");
    Serial.println(Imu.anglePitch_G);

    while(micros() - loop_timer < 8000);
    loop_timer = micros();
}

Some lines in main.cpp I cann pass as Sensors method, but before I do that, I want to ask if should I change something?
It seems that that gyro has really big drift, so 0.1 ooutput is data from accelerometer. I can change sampling rate to higher, but arduino serial plotter can't keep up with it so I do not know if it actualy will improve gyro drift or not.

Now the pitch angle would be the one indicating the angle of a balancing robot.
As far as I understand now i will need kalman filter to get rid off unnecessary data, and then pass the remaining one to PID controler?
 
I’m sure you’ve had a good look at what others have done in the past; here is a couple of examples.

https://www.pololu.com/blog/657/balboa-is-here consider the mass distribution around the axle (batteries) and the torque/gearing of the motors. The heavier you make it the more powerful motors and speedy acceleration is required when dealing with external forces.

http://robotshop.com/letsmakerobots/falling-up-robot-project-with-teensy-36
This is taking it to a new level, sky high. But he is using Teensy.

I myself was in the process of converting my own balancing robot to Teensy control with stepper motors because I didn’t want to build an oscillator, a symptom of gear train backlash. Stepper motors provide for a greater degree of control but they also have their own drawbacks.
https://www.youtube.com/playlist?list=PLy4ktWU6lWqQ5huyFAXizUVoSSatXnIoL

Added:

Did you hear the story of the balancing robot and the breadcrumb?

Balancing robot:

I am mighty and strong a heavy weight, I will crush you under wheel the moment I encounter you.

Breadcrumb:

I am old and stale, I will resist you and throw you off kilter.

Balancing robot:

I have adaptive control, I will sense your presence apply measured force and run over the top of you as if you did not exist.
 
Last edited:
[UPDATE 2]

I have almost completed code to have standing robot.
Because I have problem with calibration and I do not know if I should add something to PID I post code and ask for help.

Code:
#include "Sensors.hpp"

Sensor::Sensor(int mag, int acgy)
{
    settings.device.commInterface = IMU_MODE_I2C;
    settings.device.mAddress = mag;
    settings.device.agAddress = acgy;
}

void Sensor::sensorCheck()
{
    if (!begin())
    {
    Serial.println("Failed to communicate with LSM9DS1.");
    Serial.println("Double-check wiring.");
    Serial.println("Default settings in this sketch will " \
                  "work for an out of the box LSM9DS1 " \
                  "Breakout, but may need to be modified " \
                  "if the board jumpers are.");
    while (1)
      ;
    }
    else { Serial.print("Communication checked"); }
}

void Sensor::readData()
{
    readGyro();
    gx -= gyroXcal;
    gy -= gyroYcal;
    gz -= gyroZcal;

    gyroX = calcGyro(gx);
    gyroY = calcGyro(gy);
    gyroZ = calcGyro(gz);

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

    readAccel();
    accX = calcAccel(ax);
    accY = calcAccel(ay);
    accZ = calcAccel(az) - 0.5;

    accVector = sqrt((accX*accX) + (accY*accY) + (accZ*accZ));
}

void Sensor::calibrateManual(int loops)
{
    for(int i = 0; i < loops; i++)
    {
        readGyro();
        gyroXcal += gx;
        gyroYcal += gy;
        gyroZcal += gz;
        delay(3);
    }
    gyroXcal /= loops;
    gyroYcal /= loops;
    gyroZcal /= loops;
}

void Sensor::initializeKalman()
{
    angleRoll_K.setQangle(0.01);      // 0.001
    angleRoll_K.setQbias(0.003);    // 0.003
    angleRoll_K.setRmeasure(0.015);

    angleRoll_K.setAngle(angleRoll_A);

    anglePitch_K.setQangle(0.01);      // 0.001
    anglePitch_K.setQbias(0.003);    // 0.003
    anglePitch_K.setRmeasure(0.015);

    anglePitch_K.setAngle(anglePitch_A);
}

void Sensor::calculateAngles(float freq)
{
    angleRoll_G += (float)gyroX / freq;
    anglePitch_G += (float)gyroY / freq;

    angleRoll_G += anglePitch_G * sin(gyroZ / freq * (3.142/180));
    anglePitch_G -= angleRoll_G * sin(gyroZ / freq * (3.142/180));
    //--------------------------------------------------------------------------
    angleRoll_A = asin(accY/accVector)* 57.296;
    anglePitch_A = asin(accX/accVector)* -57.296;

    angleRoll_A += 0.75;
    anglePitch_A += 0.75;

    angleRoll_G = angleRoll_G * 0.99 + angleRoll_A * 0.01;            //complementary filter
    anglePitch_G = anglePitch_G * 0.99 + anglePitch_A * 0.01;

    angleRoll_Sum = angleRoll_K.getAngle(angleRoll_A, angleRoll_G, 1.0/(float)freq);           //additional kalman filter, dont know if this should be changed.
    anglePitch_Sum = anglePitch_K.getAngle(anglePitch_A, angleRoll_G, 1.0/(float)freq);
}

void Sensor::calculateAngSum(bool isSet)
{
    if(isSet)
    {
        //angleRoll_G = angleRoll_G * 0.998 + angleRoll_A * 0.002;
        //anglePitch_G = anglePitch_G * 0.998 + anglePitch_A * 0.002;
    }
    else
    {
        anglePitch_G = anglePitch_A;
        angleRoll_G = angleRoll_A;
        isSet = true;
    }
}

sensors hpp

Code:
#include "Wire.h"
#include "SparkFunLSM9DS1.h"
#include "SPI.h"
#include "Kalman.h"

class Sensor :public LSM9DS1
{
public:
    int gyroX, gyroY, gyroZ;
    float accX, accY, accZ, accVector;

    long gyroXcal, gyroYcal, gyroZcal;

    float angleRoll_G, anglePitch_G;
    float angleRoll_A, anglePitch_A;

    Kalman angleRoll_K, anglePitch_K;

    float angleRoll_Sum, anglePitch_Sum;
public:
    Sensor(int=0x1E, int=0x6B);
    void sensorCheck();
    void readData();
    void calibrateManual(int=2000);
    void initializeKalman();
    void calculateAngles(float=125.0);
    void calculateAngSum(bool=true);
};

driver hpp

Code:
////// PID_Controler /////////////////////////////////////////////////////
class PID_Controler
{
    const float KP, KI, KD;
    const int MAX_OUTPUT, MIN_OUTPUT;
    int lastInput;
    float desiredValue;
    float iterative;
public:
    PID_Controler(double=0, double=0, double=0);
    float computeOutput(float);
    float getDesiredValue();
    void setDesiredValue(float);
};

cpp

Code:
#include "Drivers.hpp"

PID_Controler::PID_Controler(double prop, double integ, double deriv) :KP(prop), KI(integ), KD(deriv), MAX_OUTPUT(255), MIN_OUTPUT(-255), lastInput(0), desiredValue(0) {}

float PID_Controler::computeOutput(float inputAngle)
{
    float error = desiredValue - inputAngle;
    iterative += KI * error;

    //if(error < 1 && error > -1) return 0;

    if(iterative > MAX_OUTPUT) iterative = MAX_OUTPUT;
    else if(iterative < MIN_OUTPUT) iterative = MIN_OUTPUT;

    float derivative = inputAngle - lastInput;

    float motorVelocity = KP*error + iterative + KD*derivative;

    if(motorVelocity > MAX_OUTPUT) motorVelocity = MAX_OUTPUT;
    else if(motorVelocity < MIN_OUTPUT) motorVelocity = MIN_OUTPUT;

    lastInput = inputAngle;

    return motorVelocity;
}

float PID_Controler::getDesiredValue() { return desiredValue; }

void PID_Controler::setDesiredValue(float dAngle) { desiredValue = dAngle; }

and main.cpp

Code:
#include "Motors.hpp"
#include "Sensors.hpp"
#include "Drivers.hpp"

const int LED = 13;

DC_Motor leftMotor(5, 6);
DC_Motor rightMotor(3, 4);
MotorControler motors(leftMotor, rightMotor);

Sensor IMU;
bool IsItSet = false;
const float FREQ = 250;
const float PERIOD = 1 / FREQ * 1000000;

PID_Controler PID(180, 0, 0);
char rCommand = '0';

long loop_timer;
void setup()
{
    Wire.begin();
    Serial.begin(38400);
    Serial1.begin(38400);

    pinMode(LED, OUTPUT);
    digitalWrite(LED, HIGH);
    IMU.sensorCheck();
    IMU.calibrateManual(2000);

    delay(10);
    digitalWrite(LED, LOW);

    IMU.readData();
    IMU.initializeKalman();
    IMU.calculateAngles(FREQ);
    IMU.calculateAngSum(IsItSet);

    loop_timer = micros();
}

void loop()
{
    rCommand = '0';//Serial1.read();
    IMU.readData();
    IMU.calculateAngles(FREQ);
    IMU.calculateAngSum(IsItSet);

    if(rCommand == 'w') PID.setDesiredValue(2);
    else if(rCommand == 's') PID.setDesiredValue(-2);

    float velocity = PID.computeOutput(IMU.anglePitch_Sum);

    //Serial.print(velocity);
    //Serial.print(", ");

    if(velocity > 0) motors.moveForward(abs(velocity));
    else  motors.moveBack(abs(velocity));
    //motors.moveForward();
    Serial.print(IMU.anglePitch_Sum, 4);
    Serial.print(", ");
    Serial.println(IMU.anglePitch_G, 4);


    //Serial.print(micros() - loop_timer);
    while(micros() - loop_timer < PERIOD);
    loop_timer = micros();
}
 
[UPDATE 2]

I have almost completed code to have standing robot.
Because I have problem with calibration and I do not know if I should add something to PID I post code and ask for help.

That's the most challenging part. I built a balancing robot several years ago (video somwhere on my youtube) - with a Arduino Nano & Raspberry - The Nano did all the hard work of balancing, the PI only the higer level things - For the balancing the PI was'nt needed. Don' remember much of the code, sry.. but yes, i used a simple PID too. Calibration was very challenging and took a long time - was try & error. It's easier if the centery of gravity is near the top (try to balance a brom on your hand and you'll see what I mean)
 
Status
Not open for further replies.
Back
Top