Bluetooth and the Physical Model

Being a mobile robot, radio communication is essential for both control and debug. 

It took me ages to integrate it (~ 4 hours). My first problem was not knowing about the protection for the non mask-able interrupt pin that is shared with the tx line for uart 2. The second was actually pairing the Bluetooth with my PC. I could pair, but it would immediately go offline. I did not realise that it would actually only pair correctly once I had opened the comm port. It did not help that there were a few weird errors in the windows event log either.

The model it self  has a training sled and is very top heavy, especially when the batteries are added. Essentially components have just been screwed onto a block of timber. The pcb on the bottom is the dc motor driver. It has a 5v dc regulator output so the logic components can either be powered from the usb jtag or battery with a flick of a switch.

The batteries will be NiMh and currently only 8 of 10 the battery slots will be used. The Lego motors will be powered by 9.6V! All good.

Kalman Filter

Kalman Filter

The Kalman filter is pretty straight forward filter that seeks to turn noisy measurements into not so noisy measurements. It does so by combining the observed measurement with a predicted measurement.

The blue line is the raw gyroscope measurement of the X axis. The red line is the Kalman filtered version.

This was just the sensor sitting on my bench, so the prediction model is trivial. That is the predicted gyroscope value is the previous gyroscope value.

Once the gyroscope is mounted on the robot the model will be based upon the model described in earlier post.

Stellaris and Gyroscopic Sensor

I've kind of settled on the Stellaris simply because of its 5 Volt tolerant pins.

A fair bit of effort was spent into integrating the gyroscopic sensor to the Stellaris. The problem ended up being the drive current from the micro controller was too low and was intermittently failing (i.e. the sensor was either not getting the command or getting the wrong one). On the Stellaris, you can set the drive current. The default was 2mA. 4mA seemed to work fine. So I am using 8mA to be sure to be sure.

Beaglebone with Accelerometer and Gyro

Beaglebone and I2C sensor.

The micro controller board below is called a Beaglebone. It has a very powerful arm chip running at 700Mhz. It is similar to the Raspberry Pi. The Stellaris launchpad is just 16Mhz (I think). Anyway in this small project a $6 i2c gyro and accelerometer was hooked up. The full blown SYS/BIOS was used. Linux could have been used, but the main issue there was the code size. The code size is important as it takes forever (~5 minutes) to download something the size of Linux with the slow built in jtag debugger.

SYS/BIOS is not officially supported on the Beaglebone. However, it was made to work using TI's own industrial SDK.

The output of the sensor makes its way through the Beaglebone, through a (usb) comm port to my PC to be displayed in a simple app written in C# in real time. Louka had a lot of fun hitting the table and making the graph bounce.

Launchpad and Stepper Motor

Stellaris Launchpad and Stepper Motor

The Texas Instruments Launchpad series is awesome. For around $15 delivered to AU you get a micro controller with jtag debugger. Together with a recently an Arduino kit that contained a heap of goodies, a mini project was born. You can see here the hooking up of a stepper motor via some open collector drivers (green board). The direction and speed is controlled using the buttons on the red pcb. It is a Stellaris Launchpad and it has a Stellaris LM4F120H5QR 32 bit micro controller.

This was not a bare metal job, the OS that was used was TI RTOS (very similar to SYS/BIOS). Of interest was the chip's PWM timers and it was hoped that they could be used to send the sequence to the stepper motor. This did not work too well when trying to synchronise them and change the speed. Eventually this was dropped in lieu of one timer configured to fire off an interrupt every time a signal needed to change.

Rather than polling the buttons (to deal with de-bounce), an interrupt was configured for each button. The interrupt routine along with setting the new target speed would disable the interrupt. In the background a timer task would go through and enable the interrupt once 100-200 ms had elapsed.

The micro controller runs @ 3 V, and fortunately this was enough to drive the open collector drivers so that 5V could be supplied to the stepper. The micro controller has some 5V tolerant pins and that is going to be useful.

Here is Louka playing with the buttons, she suggested that it could be used as a tail.

Lego Mindstorms NXT

Searching on the internet, I found a number of people that had successfully implemented a segway style robot using the Mindstorms kit. I also found that IAR were giving away free a special edition of their development environment.

Unfortunately, it (special edition of IAR) won't run on windows 8 so I had to make do with the 32kb free version. I already had a jlink ice, so all I needed to do was to do a bit of soldering.

I wanted to hack into the main i2c bus to add a gyro sensor and unfortunately I managed to totally destroy a couple of surface mount resisters (I'm still learning to solder). Fortunately I was able to correct the problem and connect to the slave amtel chip. In the lego guide they do recommend against this and they certainly do not make it easy - no test points on the i2c.

I did manage to write a custom program to get a motor spinning in IAR. It was quite good to be able to single step through the program using the ice.

However after all of that, I discovered another problem, the whole system runs on 5V and my sensor is 3.3V.

As you can see there is not much lego left on the circuit board.

The ice connected to the circuit with some oscilloscope leads. I had to make the cable up myself and solder on the jtag headers.

The glue there is to provide strain relief - so I don't rip the tracks off the circuit board.

I2C traffic, under sampled, but look at the amount of traffic! In the standard lego software, it polls the slave processor for sensor and motor data at a very fast rate.

Actually I have four problems with the nxt:
1) IAR development environment limited to 32KB - I could use open source of course!
2) system runs at 5V and I will have to wait for a 5V sensor
3) I really want to use a more powerful system.
4) having only 6 batteries, it does not have much power especially when running on NiMh

It has been fun, but time to investigate something else.

PID Controller

At uni, I did study fuzzy controllers and after a bit of searching I found my text book and lecture notes. However, fuzzy controllers suffer a bit from tuning. I experimented a bit, read a bit and did all those sorts of things until I kind of kept on stumbling on the internet phrases like "fuzzy controllers are a good option when standard controller don't work well". Hmm, look, these PID controllers look simpler and easier. Again lots of reading, experimenting etc, and then I discovered that in my situation I should break down the problem into two control problems. The first controller should focus on keeping it upright and then the second setting the speed.

First Controller - Keeping it Upright

In this controller the error term is angle speed with the objective of finding the best values that minimise energy and that the angle stays between -0.5 and 0.5 radians over a period of time. In effect this drives the solution to be making the angle as close to 0 as possible.

Using a "sophisticated" technique of iterating through a large number of permutations, I found the best co-efficient values were Kp = -64.9 Kd = 0, Ki = -846.3. The Kd = 0 is interesting because it seems controlling is not based at all on current angle acceleration. Hmm, I can't think with total confidence why this is. The only thing I can visualise is acceleration at different angle positions and speeds. Say would the controller do anything different if the acceleration is -ve or +ve when the angle is 0.5 and speed is -10 rad/s. I'm thinking from an energy point of view, maybe not.

Second Controller - Setting the Speed Point

In this controller the error term is speed with the objective of finding the best values that minimise speed over a period of time.

Again, using a "sophisticated" technique of iterating through a large number of permutations, I found the best co-efficient values were Kp = -434.5, Kd = 337.4, Ki = 0.0. The Ki = 0.0 is interesting because obviously the speed of the robot has nothing to do with its position and that makes sense.

Graph

The controller seems to work well, you can see that the angle and speed goes to zero.

You can see the robot is driven up until the blue line makes a sharp turn upwards. 

A New Request

Louka, my daughter wants a robot. Being an engineer, it is really a dream request. She wants the usual for a 7 year old, straight out of a sci fi movie, fly, playmate companion etc. Boy, I have a surprise for her!

The basic concept is a Segway style robot that can be remotely controlled. It is the kind of robot that has that wow factor and it is achievable. I'm thinking that this will take between 6 months and a year.

Mathematical Model

A mathematical model of the Segway is the first step.

Here it is in matlab...(I grabbed this from a UWA under graduate thesis)

syms Ip Mp l ttt km ke R r Va g t xxx Mw Iw xx  tt

f = ((Ip + (Mp*l*l))*ttt) - (2*km*ke*xx)/(R * r) + 2 * (km * Va / R) + (Mp * g * l * sin(t)) + (Mp * l * xxx * cos(t)) { equation a.1 }
ff = (-2*km*Va) / (R*r) + (2*Mw + (2*Iw/(r*r)) + Mp) * xxx + (2*km*ke*xx/(R*r*r)) + (Mp*l*ttt*cos(t)) - (Mp*l*tt*tt*sin(t)) { equation a.2 }

where
Ip = Inertia of body = 0.0041
Mp = Mass of body = 1.13
l = length to body centre of mass from wheel = 0.07
ttt = body angel acceleration (radians/second) = variable
km = torque constant Nm/Amp (from motor) = 0.006123
ke = back emf (V/radians) = 0.006087
R = terminal resistance (ohms) = 3
r = wheel radius = 0.051
Va = volts = variable used to control
g = gravity = 9.81
t = body angle (radians) = variable
xxx = robot acceleration = variable
Mw = wheel mass = 0.03
Iw = Inertia of wheel = 0.00039
xx = robot speed = variable
tt = body angle speed = variable

Of course
tt = ttt * time { equation a.3 }
t = tt * time { equation a.4 }

xx = xxx * time { equation a.5 }
x = xx * time { equation a.6 }

Bringing ttt and aaa to one side

Given the two equations { a.1 and a.2 }, we need to bring the ttt and aaa variables to one side. What we can then do is see the effects of Va upon ttt and xxx. Then given these we can see the effects upon tt, t and xx, and x over time (see equations a.2, a.4, a.5, a.6).

To do this, use matlab...


solve(subs(ff,xxx,solve(f,xxx)),ttt)

solve(subs(ff,ttt,solve(f,ttt)),xxx)

Adding Friction Component

To make the model more real, I added friction component...
pivotFrictionCoefficient = 0.5
wheelFrictionCoefficient = 0.05

ttt += pivotFrictionCoefficient  * tt
xxx += wheelFrictionCoefficient  * xx

Validation the Model

You can see what happens when it is pushed over quite hard. The segway is on a set of rails which allows it to spin a full 360 degrees. You can see that it spins three times before turning into a pendulum. The green line is tending to end up at 2.5 * Pi.

This is the effects without gravity and you can see it just keeps slowly spinning down.