Inverted pendulum robot: Complementary filters

Alexandre_PV

Hello,

I'm studing Robotics at The University of Plymouth and I'm currently in my final year. My final year project is a robot that balances itself on a ball and uses the Propeller Chip as its main processor. These are two pictures of the base of the robot, not yet finished:

http://img508.imageshack.us/my.php?image=1002654smallwp0.jpg

http://img444.imageshack.us/my.php?image=1002655smallxx2.jpg

The robot uses stepper motors as main actuators, with encoders for stall detection, one MCP3208 12bit ADC and two IMU 2Dof boards (one for X and one for Y). I have modified an assembly program called "MCP3208_fast.spin" to sample all channels at 1.5Khz each and loop forever without never returning from the assembly code, it writes the values to the main memory directly. I'm now trying to figure out a way to fuse the accelerometers and the gyroscopes to get a true angle posivion and angular velocity for each axis. I bet you are gonna say "Kalman Filter!" but no, I would like to use a simple complementary filter aproach. The kalman filter source codes I've found so far available for download either use too many cogs (and still require further modification for my aplication) or use an FPU which I don't have. I know that the complementary filter approach works fine because I've seen other projects that tried it out and succeded. I would like to ask if anyone here knows how to code a low/high pass filter in propeller assembly. That is all I need to know in order to move on to the main control phase of my project. Any comments about my projects are welcome.

Regards,

Alex

SRLM

I reproduced the images inline:






Edit: Nevermind. They need a link:

Photo 1
Photo 2

Alexandre_PV

Many thanks. Not exactly the kind of help I was looking for but I definitely appreciate ^_^.

Lawson

I assume you can express a low pass and high pass filter as a differential equation? Differential equations are valid in continuous time but they can be translated to equivalent representations in discrete time. (ie. sampled time or what a computer calculates) Difference Equations as always Wikipedia has something, I bet you'll get farther if you ask your Prof. for a good text book on difference equations. (Discretization may also be useful, too much math'eze for my taste though)

In a nut shell replace dx/dt with (X{n} - X{n-1})/T where T is the time between samples, X{n} is the current value of X and X{n-1} is the last sampled value of X. (do the same for "y" or any other variable) Now solve for the most recent value of the output variable. (usually Y{n} or Y{n+1})

Marty

P.S. the structure you end up with is also know as an IIR filter.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Lunch cures all problems! have you had lunch?

Post Edited (Lawson) : 1/7/2009 4:47:28 AM GMT

heater

No idea how to help you here but I have to say that is one beautifully engineered robot.

Somewhere I have an algorithm in C that gives low pass and high pass outputs from three stages of integration each with feed back to the input. I don't have much of a clue about digital filtering I just wrote the algorithm as a simulation of an op amp circuit I found. A musically inclined friend of mine has been using it as an active crossover for his loudspeaker set up on a PC for years now and loves it. Not sure if it would count as a complimentary filter though. I can try to dig it out if you like.

Actually I'd love to get that algorithm working on a Prop as a stand alone digital crossover but that's another story...

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
For me, the past is not over yet.

heater

Actually re-reading you post I'm not sure what the question is. Is it:

a) How to define, mathematically, algorithmically what the filter should be ?. How it works in general.
b) I have my filter algorithm, how can I implement it in Prop assembler ?

Perhaps you could define for us exactly what your inputs and expected outputs are.

Excuse my ignorance but if we had a high pass filter and subtracted its output from the input wouldn't the result be a low pass filter. The sums sure always add to one so it's complementary, right. Or is there more to a complementary filter ?

I'm sure floating point is not required. There are plenty of 32 bit integer multiply/divide routines knocking around here somewhere. I've done Mandlebrot sets in integer arithmetic in the past.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
For me, the past is not over yet.

Bob Lawrence (VE1RLL)

If it's a IIR filter you require you can use "Implementing IIR Digital Filters" from Microchip as a reference

www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=1824&appnote=en011082

I can also generate filter code in Micro C (for a dsPIC) if you want to try and convert it to prop Spin or Asm.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Aka: CosmicBob

heater

Here is a guy who has done exactly what you want. Balancing bot project including C code (under GPL) just needs converting to integer and Spin or Pasm.

www.mtoussaint.de/tobb/index.html

It's hard to find this stuff as so many confuse compliment and complement (Myself included).

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
For me, the past is not over yet.

Alexandre_PV

Thank you all for you replies and thank you 'heater' for your compliment about my design that is the result of two and a half months of hard work. The T.O.B.B. robot uses exactly what I want to use for sensor fusion, I just hope mine will be a lot more stable. The IIR .pdf paper is also very helpful and I'll definitely use it for reference. I'll write a simple program in C++ to capture the accelerometer and gyro data and run a low-pass-filter just to make sure it works and try different cut-off frequencies before implementing in my Propeller chip. My goal is to design to implement my ADC assembly program to sample, filter and combine the gyro + accelerometer data using only one cog. If I succeed in it, I'll make sure it will be available in the Propeller Object Exchange page. If any of you have any other .pdf files or links about discreet low-pass filter, please fell free to post them here. One more, thank you all for your help, I really appreciate it.

Regards,

Alex