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.

Let’s start with the body, again clean and minimalist :

SelfBalancingRobot_Body.png

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:

DSC06375.JPG

A match made in heaven… sorry, OpenSCAD :

DSC06376.JPG

Then add a nice transparent box for the torso, to house the electronics:

DSC06378.JPG

Arghhh… that old lunch box looks awful !

Let’s try again…

DSC06381.JPG

Much better, though the acrylic is harder to work with than the lunch box’s plastic.

DSC06384.JPG

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 !

wheel.png

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:

Wheels_v3.png

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:

MotorToMeccanoWheel_Adapter.png

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])
								polygon(points=[[0,0],[0,baseFilletRadius],[baseFilletRadius,0]]);
					}
			
					// Chamfer the top...
					rotate_extrude() 
						translate([diameter/2,height/2,0])				
							polygon(points=[[0.5,0.5],[-topFilletRadius-0.5,0.5],[0.5, -topFilletRadius-0.5]]);
			
					// Carve the bottom fillet from the chamfer
					if ( !chamferOnly ) { 
						rotate_extrude() {
							translate([(diameter/2)+baseFilletRadius,
								-(height-(2*baseFilletRadius))/2-hubZOffset,0]) {
								circle(r=baseFilletRadius);
							}
						}
					}
				}

				// Add the fillet back on top of the top chamfer 
				if (!chamferOnly) {
					rotate_extrude() {
						translate([
							(diameter/2)-topFilletRadius,
							(height-(2*topFilletRadius))/2,
							0])				
							circle(r=topFilletRadius);
					}
				}
			}
	
			// 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
									+(boreDiameter-shaftFlatDiameter), holeLengthBottom=hubWidth+baseFilletRadius-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
									+(boreDiameter-shaftFlatDiameter), holeLengthBottom=hubWidth+baseFilletRadius-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[0], nutSize[1]], center=true );
	
		translate([0,0,-(nutSize[1]/2)-holeLengthBottom]) 
			cylinder(r=setScrewHoleDiameter/2, h=nutSize[1]+holeLengthTop+holeLengthBottom, $fn=15);
	}
}

// nutSize = [inDiameter,thickness]
module nut( nutSize ) { 
	side = nutSize[0] * tan( 180/6 );
	if ( nutSize[0] * nutSize[1] != 0 ) {
		for ( i = [0 : 2] ) {
			rotate( i*120, [0, 0, 1]) 
				cube( [side, nutSize[0], nutSize[1]], 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()) {
        char instruction = Serial.read();
        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 --;
//    }
//}

Advertisements

One Response to Self Balancing Robot – V3

  1. Pingback: Self Balancing Robot – V4 | Robotics / Electronics / Physical Computing

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: