Self Balancing Robot – V2


DSC06380

This is a quick post to dump some pictures and lessons learneth the hard way, when I tried to build a nicer / better self balancing robot.

All this dates back from October 2014, so more than 1 year ago, but I finally gave up on trying to make this work and wanted to keep all this info/details for my own reference.

In a nutshell, the problem is that either the (quite expensive) Turnigy gimbal motors, bought specifically as they have lower speed and higher torque than normal brushless ones, are not powerful enough for what I need or that the ESC are to slow to move from forward to backward direction. Probably the latter…

It all started with some nice, minimalist design for the body modeled in OpenSCAD:

Body

rMotor=14;
rOuterShell = rMotor + 4;
lMotor=23;
fn=180;

motorMount(rOuterShell, lMotor);
translate([0,0,30]){
rotate([0,180,0]){
motorMount(rOuterShell, lMotor);
}
}
translate([0,rOuterShell-3,15]){
cube([2*rOuterShell, 6, 30], center=true);
}

module motorMount(rOuterShell, lMotor) {
difference(){
cylinder(r=rOuterShell-2, h=6, center=true, $fn=fn);

// hole for the axle
cylinder(r=4.5, h=100, center=true, $fn=fn);

rotate([0,0,-45]){
motorScrews(6.1, 3);
}

// cut the bottom of the disk at the back of the motor
translate([0,-rOuterShell, 0]){
cube([100, 6, 100], center=true);
}
}

difference(){
translate([0,0,-(lMotor-6)/2]){
difference(){
cylinder(r=rOuterShell, h=lMotor, center=true, $fn=fn);
cylinder(r=rOuterShell-2, h=lMotor, center=true, $fn=fn);
translate([0,-rOuterShell/2, 0]){
cube([2*rOuterShell, rOuterShell, lMotor], center=true);
}
}

difference(){
translate([0,rOuterShell/2, 0]){
cube([2*rOuterShell, rOuterShell, lMotor], center=true);
}
cylinder(r=rOuterShell, h=lMotor, center=true, $fn=fn);
}
}

// wires hole
translate([-4.5,0,-(lMotor-9)/2]){
cube([9,100,4]);
}
}
}

module motorScrews(height, recess){
translate([8,0,0]){
motorScrew(height, recess);
}
translate([-8,0,0]){
motorScrew(height, recess);
}
translate([0,9.5,0]){
motorScrew(height, recess);
}
translate([0,-9.5,0]){
motorScrew(height, recess);
}
}

module motorScrew(height, recess){
cylinder(r=1.7, h=height, center=true, $fn=fn);
translate([0,0,(height-recess)/2]){
cylinder(r=2.8, h=recess, center=true, $fn=fn);
}
}

and a great way of fitting the Meccano wheels directly to the motors:
WheelAdapter.png

OpenSCAD is really great !

rMain=18.4;
hMain=4;
fn=180;
wheelHolesDistance=13;

difference() {
cylinder(h=hMain, r=rMain, center=true, $fn=fn);

motorScrews(hMain+0.1, 2);
wheelScrews(hMain+0.1, 2);
}

translate([wheelHolesDistance,0,hMain/2]){
wheelScrewExtra();
}
rotate([0,0,120]){
translate([wheelHolesDistance,0,hMain/2]){
wheelScrewExtra();
}
}
rotate([0,0,-120]){
translate([wheelHolesDistance,0,hMain/2]){
wheelScrewExtra();
}
}

module wheelScrewExtra(){
difference(){
cylinder(r=1.9, h=3, center=true, $fn=fn);
cylinder(r=1.3, h=3, center=true, $fn=fn);
}
}

module wheelScrews(height, recess) {
translate([wheelHolesDistance,0,0]){
wheelScrew(height, recess);
}
rotate([0,0,120]){
translate([wheelHolesDistance,0,0]){
wheelScrew(height, recess);
}
}
rotate([0,0,-120]){
translate([wheelHolesDistance,0,0]){
wheelScrew(height, recess);
}
}
}

module wheelScrew(height, recess){
cylinder(r=1.3, h=height, center=true, $fn=fn);
translate([0,0,-(height-recess)/2]){
cylinder(r=3, h=recess, center=true, $fn=fn);
}
}

module motorScrews(height, recess){
translate([6,0,0]){
motorScrew(height, recess);
}
translate([-6,0,0]){
motorScrew(height, recess);
}
translate([0,6,0]){
motorScrew(height, recess);
}
translate([0,-6,0]){
motorScrew(height, recess);
}
}

module motorScrew(height, recess) {
cylinder(r=1.1, h=height, center=true, $fn=fn);
translate([0,0,(height-recess)/2]){
cylinder(r=1.95, h=recess, center=true, $fn=fn);
}
}

Like so:

DSC06201

Which then gets mounted on the body with 3 screws at the back of the motor:

DSC06203

 

And here’s the view of the electronics, basically the same as in the previous version:

DSC06204.JPG

 

The code is mostly the same, but I paste it below for reference:

SelfBalancingRobot2.ino


#include "BButil.h"
#include <Servo.h>

#define MOTOR_LEFT_PIN 4
#define MOTOR_RIGHT_PIN 5
#define LED_PIN 13

// angle is in thousandth's of radians
// gyro is in radians per sec, doesn't normally go beyond 1000
// the 2 have opposite signs
AngleGyroData _angleData = {0, 0};

// motors are connected using RC hobby car ESCs
Servo escL;
Servo escR;

int16_t _initAngle;
double BASE_Kp = 0, BASE_Ki = 0, BASE_Kd = -10;
double Kp = BASE_Kp, Ki = BASE_Ki, Kd = BASE_Kd;

void setup(){
//Serial.begin(9600);

mpu_setup();

escL.attach(MOTOR_LEFT_PIN);
escL.writeMicroseconds(1500); // I've configured both ESCs to have a range from 1000 to 2000 microseconds
escR.attach(MOTOR_RIGHT_PIN);
escR.writeMicroseconds(1500);

pinMode(LED_PIN, OUTPUT);

// wait for the ESCs and the MPU to initialise
delay(5000);

// configure the initial angle, which is assumed to be the "stable" position
while(mpu_getData(&_angleData));//wait to get valid data
_initAngle = _angleData.angle;
}

long currentTime, lastTime = 0;
int16_t integralErr = 0, INTEGRAL_ERR_MAX = 20000, actualAngle;

void loop(){
currentTime = millis();

if(currentTime - lastTime > 1){
// get the MPU data if available
int8_t res = mpu_getData(&_angleData);

// if res != 0 the data is not yet ready or there were errors, so ignore and keep trying
if(res == 0){
actualAngle = _angleData.angle - _initAngle;

// update the speed, apply PID algo (for the D, we don't need to do it manually as the sensor already gives us the gyro value which is the derivative of the angle)
double speed = Kp * actualAngle + Ki * integralErr + Kd * _angleData.gyro;
//Serial.println(actualAngle, DEC);
setSpeed(speed);

integralErr = constrain(integralErr + actualAngle, -INTEGRAL_ERR_MAX, INTEGRAL_ERR_MAX);

// log the actual angle and gyro values
//Serial.write(lowByte(_angleData.angle));Serial.write(highByte(_angleData.angle)); Serial.write(lowByte(_angleData.gyro));Serial.write(highByte(_angleData.gyro));Serial.print('#');

// keep track of the timings so that we do the update at regular intervals
lastTime = currentTime;
}
}
/*
// check if we've received new values for the PID constants. Each byte is a percentage variation of the BASE konstant
if(Serial.available() >= 4) {
if(Serial.read() == '#'){
Kp = BASE_Kp + (Serial.read() - 52) * BASE_Kp / 5;
Ki = BASE_Ki + (Serial.read() - 52) * BASE_Ki / 5;
Kd = BASE_Kd + (Serial.read() - 52) * BASE_Kd / 5;
}

// now simply empty the buffer
while(Serial.available())Serial.read();
}
*/
}

void setSpeed(int s){
int speed = constrain(s, -500, 500);
escL.writeMicroseconds(1500 - speed);
escR.writeMicroseconds(1500 + speed);
}

 

MPU6050.ino


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

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69 This is the ONLY way I can make my cheap banggood.com board work, wile also connectiong AD0 to VCC
MPU6050 mpu(0x69);

/* Connect VCC, GND, SDA, SCL and the MPU-6050's INT pin to Arduino's external interrupt #0.
On the Arduino Uno and Mega 2560, this is digital I/O pin 2. */

// MPU control/status vars
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint8_t fifoBuffer[64];          // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container

VectorFloat gravity;    // [x, y, z]            gravity vector
float yawPitchRoll[3];  // [z, y, x]   yaw/pitch/roll container
int16_t gyros[3];      // [x, y, z] gyros container

bool mpu_setup() {
Wire.begin(); // join I2C bus (I2Cdev library doesn't do this automatically)

Serial.print(F("Init I2C "));
mpu.initialize();

Serial.print(F("ID")); Serial.println(mpu.getDeviceID());

// verify connection
Serial.println(F("Test conns"));
Serial.println(mpu.testConnection() ? F("Conn success") : F("Conn failed"));

// load and configure the DMP
Serial.println(F("Init DMP"));
uint8_t devStatus = mpu.dmpInitialize();

// 0 = success, !0 = error
if (devStatus == 0) {
Serial.println(F("Enable DMP"));
mpu.setDMPEnabled(true);

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();

// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("Set INTrrpts"));

return true;
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed (if it's going to break, usually the code will be 1)
Serial.print(F("DMP ERR ")); Serial.println(devStatus);

return false;
}
}

int8_t mpu_getData(AngleGyroData* data){
uint8_t mpuIntStatus = mpu.getIntStatus();

// get current FIFO count (all bytes currently in the FIFO)
uint16_t fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount >= 1024) {
mpu.resetFIFO(); // reset so we can continue cleanly
return -1; //FIFO overflow
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
if (fifoCount >= packetSize && fifoCount >= packetSize) {
// read 1st packet from FIFO, don't bother with the rest
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.resetFIFO(); // reset so we can continue cleanly
}

// in case there were plenty of packets in the FIFO, only transform the last one, to avoid wasting CPU for nothing
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(yawPitchRoll, &q, &gravity);
mpu.dmpGetGyro(gyros, fifoBuffer);

// all we care is 1 axis data
data->angle = yawPitchRoll[2] * 1000;
data->gyro =  gyros[2];

return 0; // all good
}else {
return -2; //Wrong Status
}
}

// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet: 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 - (340 * 35) = -12412
int8_t mpu_getTemp(){
return (mpu.getTemperature() + 12412.0) / 340.0;
}

 

BButil.h


#ifndef BBUTIL_h
#define BBUTIL_h

#include <Arduino.h>

struct AngleGyroData {
int16_t angle;
int16_t gyro;
};

#endif

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: