# Self Balancing Robot – V3

Same as the previous post, this is an old project that still doesn’t work properly, but will probably never have the time to fix, so I’ve decided to post the details here, in case I need to refer to them later.

It’s not completely crap though, it can quite easily find its balance, it’s just that it’s far too jittery !  My latest theory (after having moved the IMU closer to the main body and used smaller wheels without much success) is that it’s due to the backslash of the motors/gears. The plan would be to replace them with some over sized servos, connected directly to the wheels, like Ytai here. ```module fixationScrew(height, recess){
cylinder(r=1.5, h=height, center=true, \$fn=18);
translate([0,0,-(height-recess)/2]){
cylinder(r=3, h=recess, center=true, \$fn=36);
}
}

module motorSupport(thickness){
difference(){
cylinder(r=20, h=thickness, center=true, \$fn=180);

// remove big hole for the axle
translate([7, 0, 0])
cylinder(r=7, h=thickness, center=true, \$fn=90);

// fixation screws holes
rotate([0, 0, 30])
translate([15.5, 0, 0])
fixationScrew(thickness, 2);

rotate([0, 0, 150])
translate([15.5, 0, 0])
fixationScrew(thickness, 2);

rotate([0, 0, 270])
translate([15.5, 0, 0])
fixationScrew(thickness, 2);
}
}

// main body
difference(){
translate([-11.5, 0, 0])
cube([23, 40, 150], center=true);

// using r "almost" 20 allows to get rid of the "Object isn't a valid 2-manifold!" exception when exporting to STL
cylinder(r=19.999, h=150, center=true, \$fn=180);

// support screws for mounting a box on top
rotate([0, -90, 0]){
for(x = [-50, -25, 0, 25, 50]){
for(y = [-10, 0, 10]){
translate([x, y, 19])
fixationScrew(10, 7);
}
}

// big hole for motor power cables
cylinder(r=6, h=100, center=true, \$fn=90);
}
}

// motor supports on the sides
translate([0, 0, -72.5])
motorSupport(5);
translate([0, 0, 72.5])
rotate([180, 0, 0])
motorSupport(5);
```

Built around the new motors, 12V, 350RPM (should be enough, haven’t done any math 🙂 ) bought for quite cheap from China:  Then add a nice transparent box for the torso, to house the electronics: Arghhh… that old lunch box looks awful !

Let’s try again… Much better, though the acrylic is harder to work with than the lunch box’s plastic. Now let’s focus on the wheels. I want them to be big, as I have a feeling the motors are very powerful, but slightly slow at 350RPM…

I big thank you to this guy, as I’ve used his OpenSCAD (have you noticed yet what a big fan I am !? ) script to generate these fairly complex wheels ! Those tire knobs while impressive, are completely useless, even on carpet, not to mention on hard floor.

So let’s try a design that can take a rubber tire: The bottom looks good: The above also shows the thin additional wheel that together with a couple of IR sensors from an old printer, serve as rotary encoders.

None of this made the jitter go away, so I tried using smaller wheels to minimize its effect: ```rMain=18.4;
hMain=6;
dHub = 20;
hHub = 14;
wheelHolesDistance=13;
\$fn=180;

difference() {
union(){
cylinder(h=hMain, r=rMain, center=true);
rotate([180, 0, 60]){
translate([0, 0, (hHub + hMain)/2]){
hub(hHub, dHub, 6.5, 5.8, 1, [6, 3], 3.2, -2, 0, 4, 4);
}
}
}

wheelScrews(hMain+0.1, 2);
}

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.7, h=height, center=true);
translate([0,0,-height/2 - 100]){
rotate([0, 0, 30]){
cylinder(r=3.3, h=2*(recess + 100), center=true, \$fn=6);
}
}
}

// The hub (the part that holds the wheel onto the motor
module hub( height,
diameter,
boreDiameter, // The diameter of the motor shaft
shaftFlatDiameter, // The diameter of the motor shaft at the flat, or shaftDiameter for no flat.
nuts, // The number of set screws/nuts to render, spaced evenly around the shaft
nutSize, // Size [indiameter, thickness] of set screw nut. The depth is set automatically.
setScrewDiameter, // The diameter of the set screw. 3 is the default for an M3 screw.
setScrewNutOffset=0,	// The distance to offset the nut from the center of the material. -/+ = in/out
hubZOffset=0,
baseFilletRadius=0, // The radius of the fillet (rounded part) between the hub and wheel.
topFilletRadius=0, // The radius of the fillet (rounded part) at the top of the hub.
chamferOnly=false, // Set to true to use chamfers (straight 45-degree angles) instead of fillets.
hexbore=false // Make the bore a hex shaft vs a circle
)
{

hubWidth=(diameter-boreDiameter)/2;

union() {
difference() {

// Main hub shape
union() {
difference() {
union() {
cylinder( h=height, r=diameter/2, center=true );

// First chamfer the base...
rotate_extrude()
translate([diameter/2,-(height/2)-hubZOffset,0])
}

// Chamfer the top...
rotate_extrude()
translate([diameter/2,height/2,0])

// Carve the bottom fillet from the chamfer
if ( !chamferOnly ) {
rotate_extrude() {
}
}
}
}

// Add the fillet back on top of the top chamfer
if (!chamferOnly) {
rotate_extrude() {
translate([
0])
}
}
}

// Remove the bore
difference() {
if (hexbore) {
cylinder(r=boreDiameter/2/ cos(180/6),h=height+1,\$fn=6, center=true);
} else {
difference(){
cylinder( h=height+1, r=boreDiameter/2, center=true );
translate([(boreDiameter-shaftFlatDiameter+1)/2 + (boreDiameter/2)
- (boreDiameter - shaftFlatDiameter),0,0])
cube( [boreDiameter-shaftFlatDiameter+1,boreDiameter,height+2], center=true );
}
}
}

// Remove the captive nut
for( i=[0:nuts-1] ) {
if (hexbore) {
rotate([ 0,0, (360/nuts)*i+30 ])
translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset, 0, height/2 - (height+hubZOffset)/2]) {
rotate([0,-90,0]) {
captiveNut( nutSize, setScrewDiameter, depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
}
}
} else {
rotate([ 0,0, (360/nuts)*i ])
translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset,	0, height/2 - (height+hubZOffset)/2]) {
rotate([0,-90,0]) {
captiveNut( nutSize, setScrewDiameter, depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
}
}
}
}
}
}
}

// This is the captive nut module
module captiveNut( nutSize, setScrewHoleDiameter=3,
depth=10, holeLengthTop=5, holeLengthBottom=5 )
{
render()
union() {
nut( nutSize );

if ( depth > 0 )
translate([depth/2,0,0])
cube( [depth, nutSize, nutSize], center=true );

translate([0,0,-(nutSize/2)-holeLengthBottom])
cylinder(r=setScrewHoleDiameter/2, h=nutSize+holeLengthTop+holeLengthBottom, \$fn=15);
}
}

// nutSize = [inDiameter,thickness]
module nut( nutSize ) {
side = nutSize * tan( 180/6 );
if ( nutSize * nutSize != 0 ) {
for ( i = [0 : 2] ) {
rotate( i*120, [0, 0, 1])
cube( [side, nutSize, nutSize], center=true );
}
}
}
```

But without much success, as mentioned at the very beginning…

So went back to the original wheels, did a couple of movies and left it there.

Here’s the code (plus the MPU6050.ino and BButil.h files, but you already have those in the 2 previous posts):

```#include "BButil.h"

#define MA_ENABLE_PIN 6
#define MA_IN1_PIN 9
#define MA_IN2_PIN 8
#define MA_ENCODER_PIN 2  //has to be a hardware interrupt enabled pin

#define MB_ENABLE_PIN 5
#define MB_IN3_PIN 7
#define MB_IN4_PIN 4
#define MB_ENCODER_PIN 3  //has to be a hardware interrupt enabled pin

#define LED_PIN 13

#define ANGLE_SETPOINT_CENTRE 10
#define ANGLE_SETPOINT_MAXDELTA 40

// 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};

float BASE_Kp = 4.0, BASE_Ki = 0.05, BASE_Kd = 5.0;
float Kp = BASE_Kp, Ki = BASE_Ki, Kd = BASE_Kd;

// coefficient for staying in the same position (only Proportional for now)
volatile int KPos = 0;

int speedA = 0, speedB = 0;
volatile int encoderCountA = 0, encoderCountB = 0;

void setup(){
pinMode(MA_ENABLE_PIN, OUTPUT);
pinMode(MA_IN1_PIN, OUTPUT);
pinMode(MA_IN2_PIN, OUTPUT);
//    // enable PULL-UP resistor for encoder pins
//    pinMode(MA_ENCODER_PIN, INPUT_PULLUP);

pinMode(MB_ENABLE_PIN, OUTPUT);
pinMode(MB_IN3_PIN, OUTPUT);
pinMode(MB_IN4_PIN, OUTPUT);
//    // enable PULL-UP resistor for encoder pins
//    pinMode(MB_ENCODER_PIN, INPUT_PULLUP);

// stop the motors ASAP
analogWrite(MA_ENABLE_PIN, 0);
analogWrite(MB_ENABLE_PIN, 0);

pinMode(LED_PIN, OUTPUT);

//    // we use INTERRUPTS for the optical sensors/encoders used to cound how much each wheel has actually moved
//    attachInterrupt(MA_ENCODER_PIN - 2, encoderEventA, RISING);
//    attachInterrupt(MB_ENCODER_PIN - 2, encoderEventB, RISING);

Serial.begin(9600);

mpu_setup();
}

long currentTime, lastTime = 0, deltaTime;
int16_t integralErr = 0, INTEGRAL_ERR_MAX = 1000, angleErr;
volatile int16_t angleSetpoint = ANGLE_SETPOINT_CENTRE;

void loop(){
currentTime = millis();
deltaTime = currentTime - lastTime;

if(deltaTime > 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){
angleErr = _angleData.angle - angleSetpoint;

// 1. 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)
float speed = Kp * angleErr + Ki * integralErr + Kd * _angleData.gyro;

// 20 1000th of radians ~ 1degrees
//if(abs(angleErr) > 20){
integralErr = constrain(integralErr + angleErr * deltaTime, -INTEGRAL_ERR_MAX, INTEGRAL_ERR_MAX);
//}else if (abs(speed) < 10){
//integralErr = 0;
//}

// 2. figure out which DIRECTION to go
updateMotorSpeed(speed);

// log the actual angle and gyro values
//Serial.println(actualAngle);

// 3. 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
// 'q' / 'a' increase/decrease Kp and 'w' / 's' Kd
if(Serial.available()) {
if(instruction == 'q'){
Kp = Kp + 0.1;
Serial.print("Kp: "); Serial.println(Kp);
}else if(instruction == 'a'){
Kp = Kp - 0.1;
Serial.print("Kp: "); Serial.println(Kp);
}else if(instruction == 'w'){
Kd = Kd + 0.1;
Serial.print("Kd: "); Serial.println(Kd);
}else if(instruction == 's'){
Kd = Kd - 0.1;
Serial.print("Kd: "); Serial.println(Kd);
}else if(instruction == 'e'){
Ki = Ki + 0.01;
Serial.print("Ki: "); Serial.println(Ki, 3);
}else if(instruction == 'd'){
Ki = Ki - 0.01;
Serial.print("Ki: "); Serial.println(Ki, 3);
}else if(instruction == 'r'){
angleSetpoint = angleSetpoint + 1;
Serial.print("AS: "); Serial.println(angleSetpoint);
}else if(instruction == 'f'){
angleSetpoint = angleSetpoint - 1;
Serial.print("AS: "); Serial.println(angleSetpoint);
}else if(instruction == 't'){
KPos = KPos + 1;
Serial.print("KPos: "); Serial.println(KPos);
}else if(instruction == 'g'){
KPos = KPos - 1;
Serial.print("KPos: "); Serial.println(KPos);
}else if(instruction == 'y'){
encoderCountA = encoderCountA + 10;
Serial.print("eCtA: "); Serial.println(encoderCountA);
}else if(instruction == 'h'){
encoderCountA = encoderCountA - 10;
Serial.print("eCtA: "); Serial.println(encoderCountA);
}else if(instruction == 'u'){
encoderCountB = encoderCountB + 10;
Serial.print("eCtB: "); Serial.println(encoderCountB);
}else if(instruction == 'j'){
encoderCountB = encoderCountB - 10;
Serial.print("eCtB: "); Serial.println(encoderCountB);
}
}
}

void updateMotorSpeed(float speed) {
// use a global variables so that we can read the in the ISR for the direction
speedA = speed;// - KPos * encoderCountA;
speedB = speed;// - KPos * encoderCountB;

if(speedA > 0){
digitalWrite(MA_IN1_PIN, HIGH);
digitalWrite(MA_IN2_PIN, LOW);
}else{
digitalWrite(MA_IN1_PIN, LOW);
digitalWrite(MA_IN2_PIN, HIGH);
}

if(speedB > 0){
digitalWrite(MB_IN3_PIN, LOW);
digitalWrite(MB_IN4_PIN, HIGH);
}else{
digitalWrite(MB_IN3_PIN, HIGH);
digitalWrite(MB_IN4_PIN, LOW);
}

analogWrite(MA_ENABLE_PIN, speedToPWM(speedA));
analogWrite(MB_ENABLE_PIN, speedToPWM(speedB));
}

uint8_t speedToPWM(int s){
int pwm = abs(s);
// ignore small fluctuations around 0 to avoid annoying noise
if(pwm < 10) pwm = 0;
// with a 4cells battery (~15V) the 12V motors don't start turning before 75 for B & 85 for A out of 255...
// the 1000 is more or less random, the Ks of the PID will have to be adjusted in sync, as well as INTEGRAL_ERR_MAX
else pwm = map(pwm, 10, 1000, 90, 255);

return pwm;
}

//// ISR routines that keep count of optical encoder events
//void encoderEventA(){
//    // we can't know from the even itself the direction, so use the speed of the motor
//    // this is NOT necessarily reliable as at low speeds, the wheel can actually be spinning in the opposite direction...
//    if(speedA > 0){
//        //encoderCountA ++;
//        angleSetpoint = constrain(angleSetpoint + KPos, ANGLE_SETPOINT_CENTRE - ANGLE_SETPOINT_MAXDELTA, ANGLE_SETPOINT_CENTRE + ANGLE_SETPOINT_MAXDELTA);
//    }else {
//        //encoderCountA --;
//        angleSetpoint = constrain(angleSetpoint - KPos, ANGLE_SETPOINT_CENTRE - ANGLE_SETPOINT_MAXDELTA, ANGLE_SETPOINT_CENTRE + ANGLE_SETPOINT_MAXDELTA);;
//    }
//}
//
//void encoderEventB(){
//    // we can't know from the even itself the direction, so use the speed of the motor
//    // this is NOT necessarily reliable as at low speeds, the wheel can actually be spinning in the opposite direction...
//    if(speedB > 0){
//        encoderCountB ++;
//    }else {
//        encoderCountB --;
//    }
//}

```