Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 5 of 5

Thread: Bulding first balancing robot

  1. #1

    Bulding first balancing robot

    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-blueto...th&results=279
    -SparkFun 9DoF Sensor Stick LSM9DS1 --> https://www.sparkfun.com/products/13944 --> LIBRARY: https://github.com/sparkfun/SparkFun...rduino_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/0B0i...ew?usp=sharing
    https://drive.google.com/file/d/0B0i...ew?usp=sharing
    https://drive.google.com/file/d/0B0i...ew?usp=sharing
    https://drive.google.com/file/d/0B0i...ew?usp=sharing ,(Orange cables on the photo are disconected form cicuit)
    https://drive.google.com/file/d/0B0i...ew?usp=sharing
    https://drive.google.com/file/d/0B0i...ew?usp=sharing

    WIRING:
    Click image for larger version. 

Name:	pololdriver.png 
Views:	152 
Size:	68.3 KB 
ID:	11145Click image for larger version. 

Name:	Pololudriver1.jpg 
Views:	386 
Size:	96.5 KB 
ID:	11146

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

    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.

  2. #2
    [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?

  3. #3
    Junior Member
    Join Date
    May 2017
    Posts
    8
    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/...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?lis...izUVoSSatXnIoL

    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 by tarzan; 08-13-2017 at 12:20 AM.

  4. #4
    [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();
    }

  5. #5
    Senior Member+ Frank B's Avatar
    Join Date
    Apr 2014
    Location
    Germany NRW
    Posts
    6,920
    Quote Originally Posted by Black_Stork View Post
    [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)

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •