AXONS - Homebrew firmware for AX12+ and CM510

[Fraser]

edit: thread title changed

Gentlemen,

I plan on implementing a fast control loop for AX12 servos:

* Main controller (hopefuly PC) sends desired power and direction to servos.

* Servos return current position.

I think 1KHz for 18 servos may be achievable....

EDIT: LATEST SPECS AND RELEASE CAN BE FOUND HERE:
http://www.ledset.com/bioloid/axons

[limor]

Hi Fraser
We are working towards the same goal.
So I suggest we join forces and get a better result in shorter time.

See the latest posts by Ricardo on http://actuated.wordpress.com

The code is still very experimental but you can see the code and a document describing stuff here.

What I suggest if you would like is that we set up a google code project (or github or whatever) and work on this together.

[Fraser]

Hi Limor,

Many thanks for the invite but I work very poorly in shared codespace for embedded stuff because I often rip my stuff right back and rework in order to simplify and optimise. Doing this I annoy the hell out of myself sometimes.

I will gladly share the finished firmware with you and will be happy to chat with Ricardo in private space about protocol ideas etc.

I am aware of the ActuatedCharacter blog, very nice, the simulation side of things that you are working out is fascinating, really great and essential work.

I have just read Ricardos nice ideas on feedback loop. I will check out the code this evening.

I think all that is needed is: velocity TX, position RX as fast and robust as possible, nothing else on the bus during robot control.

Provided that 1KHz feedback frequency is achieved then everything else; PID, torque calcs, position smoothing & extrapolation etc. can be carried out on the PC.

I am now working on protocol for embedded servo safety:
limits for torque, temp and position. These will be set once using a management command and stored in EEPROM. To prevent damage the servo will check these limits in spare time between comms.

[limor]

sharing code and ideas will help too

I'm not sure 1khz is achievable for say 12 servos but even if we manage to get it to 200 control cycles/sec from the PC it is good enough for even the most complicated stuff like self balancing and dynamic walking.

The protocol that we propose assumes the servos listen to the bus and know when it is their turn to send their sensor data packet, without the master telling each one to send out data one at a time.
This protocol has 2 advantages:
1) you can't get any faster than that
2) we want to synchronize the size of all the servo data packets.. with the buffer size of the FTDI. this way the 1.5ms latency of the USB bus controller does not provide significant latency. (with SYNC_READ method, the biggest bottleneck is the USB bus controller)

[Fraser]

Yes, the protocol in my PDF is very similar in concept with calibration in the time domain to make sure the servos all send out their replies as close together as possible without overlapping.

With UART to UART I'm pretty sure 1KHz is easily doable for 18 AX12s with my proposed protocol.

At the moment I Have not considered FTDI latency and I don't know how to synch to FTDI buffer to minimise it.

1.5ms latency is awful, adding up to 3ms per transaction it would drop the speed to around 250Hz for me.

The controller I will eventually be using will have bios level serial UART on it so MUCH smaller latency.

I have shared source with people in the past and they have used it for commercial gain without sharing said gains, I'm not saying that you would do that and I'm not saying I won't share source with you eventually. Besides all that I have no source to share at the moment , just the initial protocol and time calibration idea.

[Fraser]

Hi Limor,

Having looked at Ricardos source I will be happy to open source my code for this project, not that I have any source to share right now

For your project it would be very good to get rid of the USB latency. I think for a modest price you can get a plugin bus card for PC that gives a low latency serial port, I realise it's not essential but with low level UART you have a much faster loop that is also much more rigid in the time domain.

edit, I thought wrong, those days have gone!

[i-Bot]

I have a numberi of suggestions for improvements and problems, but could you first clarify the objective for me ?

It has become clearer as you changed the text, that the intention is to have the AX12 firmware as dumb, receiving only PWM values and returning position values. Is this correct ? I was confused by the terms position, velocity and power being previously used in some places when I think you meant simply PWM.

In the text you refer to the CM510 as the controller, but in your posts you say the intention is for the PC to perform the servo functions. If the PC, why is the CM510 used ? Doesn't it just add delay and jitter ?

Did you already identify a suitable PC high speed serial port ?

Will send suggestions based on your clarification, but agree with Limor that your timing approach may not work and is slower than the AX12 listening to the bus and counting the bytes till its turn to reply.

Back later..

[Fraser]

Hi i-Bot,

Correct, AX12 will be fairly dumb, only RX pwm and TX posn. With safety limit checking as background AX12 task for less drivetrain damage.

Initially I will have a closed loop of CM510 and AX12s. Then I will move over to a more powerful controller and take the CM510 out of the loop.

The CM510 will only be used to test timings and develop code and perhaps perform some simple control demonstration with gyro ala segway.

Limors idea you mention of each AX12 listening for the AX12 below is more robust without the need for calibration. I will upgrade the spec to this better method. Any more improvements are qreatly appreciated before I start implementing.

In time I want to experiment with adaptive control, for this I would like a fast and very low level channel to the servos. If the channel is fast enough then using pwm and posn the main controller can implement any control scheme that can be imagined, development should be quicker without having to constantly dig into the AX12s.

I have not decided on what controller to upgrade to. My laptop USB will not be able to close the loop as fast as I would like. I am thinking of getting something like this:

http://www.friendlyarm.net/products/mini2440

write code on laptop and debug through JTAG, also in future a PC could WIFI to the board and be a higher level controller for behaviour, vision etc. which does not require low latency like fine motor control does.

[i-Bot]

I must first declare that my own approach is very conventional. Servo loop in the servo, real time functions relating to multiple servos in the controller, and higher level functions in non real time processor. However I have come to realise the many barriers to entry and innovation of this approach.

I now see more people wanting to experiment with tuning servo parameters who often come from a Labview or Matlab background, they want to offload even low level functions to a very powerful non real time processor. This is what lead to trying to improve to 100Hz or even 1KHz performance for these interfaces. These interfaces usually give no problem on throughput, but major issues on latency and jitter in the end to end case.

For your first configuration when you have a controller (CM-510) perform the servo function for one or more servos over your protocol would be a really exciting start. I doubt the controller can do many servo functions simultaneously though. The ARM controller is a move forward, but the grail for many is probably to get data to and from a Labview or Matlab program on a PC.

Because of this, I would refine the timing of your protocol to allow for much greater latency and jitter.

So when the checksum of the command packet is received:
1) all AX12 update their PWM
2) trigger the position ADC conversion for all servos
3) log the time in a 32 bit 1microsec timer.

The response packet has your header followed by the 32 timer value from above, then each servo sends the position according to your protocol. This enables the PC or other processor to perform accurate time domain processing of the values.

5 bits of checksum seems generous, I would sacrifice one for an error bit to show a position, temperature error. You can't do a torque error, because you don't know the torque, but a |PWM| * duration is a good indicator of motor and FET driver stress.

I would only have one interrupt from the USART, the timer for a protocol timeout can interrupt too, but not in normal flow, The rest can be normal program flow.

Latency and jitter in the PC interface is still an issue. USB has latency, but I have yet to find a PCI or similar serial port with 1M, large enough FIFO and TTL bus capable. I have custom USB interface which gets it down to a latency of around 2 millisec round trip. The latency and jitter in my Windows PC itself is average 120 microsec, with often 200 microsec, and max of over 1 millisec. Since 1 millisec is about 1 position bit at full speed, it would be interesting to see if that works.

[Fraser]

i-Bot,

Many thanks for your thoughts.

Yes the UART interrupt should have highest priority. The only other interrupt I would use is ADC data ready.

The timestamp in packet idea is very nice for straightening out jitter at the PC end. Maybe could get away with 16bit 1usec timeval. This would wrap every 65ms, as long as lowest packet frequency was always above 15Hz the wraps could be untangled easily. Or maybe 24bit to play it safe.

I wonder if with the timestamp idea it would also be possible to magic away the latency by extrapolating servo position into now time at the PC end, after all the position signals are quite low frequency and deterministic thanks to the robot and drivetrain mass/inertia. Perhaps this could even be doable wirelessly!?

I too would really prefer closing the loop with my PC. Not for matlab but for visual studio with c/c++, that's what I'm used to, I can work quickly in it and I have built up quite a library of code. As you say, it is the holy grail and would open up lots of possibilities to anyone with a PC and a bioloid. It would be even nicer to make a superset of the existing dynamixe firmware, with these proposed new commands on top, I realise that's alot of work, but doable I think.

edit:
You just made me realise I made a mistake about latency slowing throughput in a previous post to Limor, I now realise it will only give later readings.

[Fraser]

Just thinking some more on extrapolating the position signal into now time:

Extrapolating using just previous known positions would be fairly good for lower latencies, but higher latencies might also be possible by adding the known PWM levels for the missing time window into the mix:

A perfect candidate for this extrapolation would be a fairly modest neural network. Not that computationaly expensive to run once trained.

First the known good position datapoints need quantising to discrete time, we will be doing this anyway to remove jitter.

From my playing with predicting chaotic signals, a suitable starting size for network experimentation is:

20 input nodes: 10 previous known positions, 10 latest known PWM
8 neurons in layer 1
4 neurons in layer 2
1 output neuron gives us the extrapolated position now sample.

The previous known positions can be the latest 10, or to become more aware of lower frequency components without adding nodes they can be spaced unevenly in time, the freshest samples being more important are closer in time, the older samples used for low freq get further apart in time (exponential distribution).

The latest known PWM should be latest consecutive values.

You can train output neuron to look as far ahead in time as you like, obviously less accurate as you look further ahead.

To run the above network once trained (20:8:4:1) we need to do this for each servo per packet:

20*8*4 = 640 float adds
8*4*1 = 32 float mults

For 18 servos per packet:

640*18 = 11520 float adds
32*18 = 576 float mults

This is negligible calculations per packet for a modernish PC.

It would be so nice if everyone could close the loop over a good wifi link.

[i-Bot]

The time of the timestamp in servo time was proposed to be a shared event by all servos (receipt of valid CRC) where the PWM is updated and the ADC sampled. This was to make interpolation in the PC software servo code possible.

The PC then knows when the pwm is updated in servo time, and the position at that given in servo time. The is not yet able to schedule events in servo time though and it will be interesting to see if this matters.

The timestamp in packet idea is very nice for straightening out jitter at the PC end. Maybe could get away with 16bit 1usec timeval. This would wrap every 65ms, as long as lowest packet frequency was always above 15Hz the wraps could be untangled easily. Or maybe 24bit to play it safe.

Because the ADC is triggered at the valid CRC and takes around 70 microsec I think, then there is time to send 32 bits and the header. Also 32 bits is forever !

Yes the UART interrupt should have highest priority. The only other interrupt I would use is ADC data ready.

I wouldn't even use the ADC interrupt or maybe no interrupts at all, they suck up a lot of cycles. I would code the interrupts in asm instead of C at least.

It would be even nicer to make a superset of the existing dynamixe firmware, with these proposed new commands on top, I realise that's alot of work, but doable I think.

If you do this I would make you close coupled code a seperate sub mode of the normal command set to reduce overhead. The normal code running the full protocol and the servo engine is strained and Robotis code does already fail at times. Stuff like EEPROM takes an age.
There is no published existing firmware source at this time. RandomMatt has a version but source not published. There is c code from John Hylands for Dynamixel I/O. Maybe Limor or Ricardo will mate the Openservo and the Dynamixel protocol.

[Fraser]

OK great, thanks a lot, and noted:
32bit t-stamp, just sent out by servo 1 as soon as cmd is parsed.

superset of original robotis firmware is just a dream at the moment, but yes it makes sense to have a whole new fast mode of operation for the close coupled stuff.

If I manage to get something that I think would be useful to others then I'd probably have a go at hacking through the dynamixel specs. The close coupled stuff will take up very little progmem so it should all fit. I will have to write cmds to set safety limits anyway so I will write them as per dynamixel specs from the get go.

Time to think about writing now I have some AX12s in hand.

[limor]

Robotis firmware for AX12 has timing issues. It seems that they went overboard with all these variables and their maintenance overheads. I don't see any reason to copy their entire framework of variables. For PC closed-loop control really you need a very degenerate subset of peek/poke variables.

having the following AX12 bus control options from a PC at reliable 100+ frames/sec (18 servos) is plenty more than what you can do with the original firmware:
- speed control + max voltage
- position control + spring effect
- voltage control


btw: We already have published code that uses the CM5 as an intermediary between the PC (RoBoard) and the AX12 bus (see here) and it works very well. 12 servos at 250 sync_read cycles/sec.

btw2: there has been talk on the forum before regarding lantronix wiport (wifi/serial) module used with bioloid and one can not ignore the BifferBoard+wifi-dongle (especially when mentioning the FriendlyArm9 micro-2440).

btw3: there is a real problem in getting any PC to react reliably at high frame rates and compute whatever control magic it needs to compute and reply in time etc. 100 frames/sec is manageable but not 1000. maybe on a real-time operating system. certainly not on windows while it is going about its mysterious hidden services work.

btw4: great in-depth discussion about the subject Fraser and i-Bot

btw5: any reason not to use Kalman to predict the current position and speed? it is much less costly than ANN once the prefixes are identified for each AX12.

[Fraser]

Thanks for the input Limor I really appreciate you bringing me up to speed, I hope I can contribute some small part in time toward the first adaptive smooth moving hobby robot.

Robotis firmware for AX12 has timing issues. It seems that they went overboard with all these variables and their maintenance overheads. I don't see any reason to copy their entire framework of variables. For PC closed-loop control really you need a very degenerate subset of peek/poke variables.

If I could shoehorn it all in then it would encourage lots of people to play with the realtime conrol, lots of people would not like to reprogram all their servos and lose all the robotis stuff. It would be ideal to have the closed loop stuff as an added cmd that puts the servo into a much leaner operating mode. But I will not work on this until I can achieve something that is useful fo realtime control.
[/quote]

having the following AX12 bus control options from a PC at reliable 100+ frames/sec (18 servos) is plenty more than what you can do with the original firmware:
- speed control + max voltage
- position control + spring effect
- voltage control

I will add these in as extra commands. First for my experiments I only need set:pwm read:postition with as fast throughput as possible.

btw: We already have published code that uses the CM5 as an intermediary between the PC (RoBoard) and the AX12 bus (see here) and it works very well. 12 servos at 250 sync_read cycles/sec.

That is nice and fast, faster than I thought possible with standard AX12. I will look at the code.

btw2: there has been talk on the forum before regarding lantronix wiport (wifi/serial) module used with bioloid and one can not ignore the BifferBoard+wifi-dongle (especially when mentioning the FriendlyArm9 micro-2440).

thanks for the info, I will read about these.

btw3: there is a real problem in getting any PC to react reliably at high frame rates and compute whatever control magic it needs to compute and reply in time etc. 100 frames/sec is manageable but not 1000. maybe on a real-time operating system. certainly not on windows while it is going about its mysterious hidden services work.

Yes it seems operating systems have got further and further removed from the hardware as the years have progressed. It's terrible that a $5 16MHz microcontroller can thrash a 2GHz+ $1000 PC when it comes to writing ones and zeros to the outside world realtime. Comms would have been no problem on a machine from the 90s.

My current thoughts on getting something decent using PC are:
[1]Jitter in packets from robot to pc can be removed using timestamps and interpolation.

[2]Latencies (TX and RX) can be (hopefuly) predicted away on PC.

[3]The one thing that will be a permanent stain is jitter in packets on the PC to robot channel hence why you guys like to synch to the buffers.

btw4: great in-depth discussion about the subject Fraser and i-Bot
btw5: any reason not to use Kalman to predict the current position and speed? it is much less costly than ANN once the prefixes are identified for each AX12.

I use ANNs because I'm quite familiar with them. I'm not yet familiar with kalman filter, have heard talk of it in predicting for control like balancers etc. I will wiki it. What I like about ANNs is that you can just throw in extra inputs (even from different times) and they can help prediction, the ANN can pull out any correlations and useful information if present. But if the kalman can do it much faster it will be the way to go. I will probably try both in time.

Good to have this forum to discuss on! I will use this place as a kind of log book, show and tell, for my tinkering. I will also change the title a little.