AXONS - Homebrew firmware for AX12+ and CM510

[SK]

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.

Schedulers for COTS Operating Systems just aren't meant for this applications, so they indeed fail for these high update rate scenarios. I'd suggest using a Linux with RT PREEMPT Kernel. At least for the Ubuntu 10.04 I'm currently using this is available as a package (just do "sudo apt-get install linux-rt"). I'm not sure it is currently conveniently available for 10.10 though. With RT PREEMPT and a SCHED_FIFO thread, one can pretty easily have update rates of 10 kHz+ on current (Core2Duo) hardware. Depending on the hardware there can still be some jitter, but it is reduced orders of magnitude.
A pretty nice overview is available here:
http://people.mech.kuleuven.be/~psoeten ... derWal.pdf

There's also dual kernel approaches like Xenomai and LXRT/RTAI, but those are not quite as painless to use in my experience.

[Fraser]

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.

Schedulers for COTS Operating Systems just aren't meant for this applications, so they indeed fail for these high update rate scenarios. I'd suggest using a Linux with RT PREEMPT Kernel. At least for the Ubuntu 10.04 I'm currently using this is available as a package (just do "sudo apt-get install linux-rt"). I'm not sure it is currently conveniently available for 10.10 though. With RT PREEMPT and a SCHED_FIFO thread, one can pretty easily have update rates of 10 kHz+ on current (Core2Duo) hardware. Depending on the hardware there can still be some jitter, but it is reduced orders of magnitude.
A pretty nice overview is available here:
http://people.mech.kuleuven.be/~psoeten ... derWal.pdf

There's also dual kernel approaches like Xenomai and LXRT/RTAI, but those are not quite as painless to use in my experience.

Thanks, the VanderWal document describes a very nice OS. I have only a little experience in linux (writing CGI modules for linux server) so it would be a steep learning curve for me. But it may well be the path I end up taking.

For now I will experiment with writing small code for CM510 and AX12s just to see what packet density I can achieve with timestamped position data. This is necessary for me because I want a good understanding of the bioloid platform, how well behaved the bus is etc.

Then in future make the CM510 be a bridge to an external PC (like Limor and co, have already done for ro-board.

I will also try a cyclic packet buffer with linear interpolation using integer additions and right shifts for divide in the CM510. With this it may be possible to absorb a little jitter (at the expense of more latency).

using linux-rt on PC does sound like the best solution for what everyone wants to do. If only microsoft wrote a kernel patch to give better realtime behaviour, my god they have enough software engineers, you would think they could add it no problem?

[SK]

Interestingly enough, there seems to be a realtime extension system for Windows called RTX:
http://edageek.com/2010/03/03/soft-cont ... itectures/
I guess that it's not free though

I can only recommend switching to Linux, using a current Ubuntu makes switching much more painless than it was in earlier times. I find myself booting into Ubuntu basically all the time, even though I have a dual boot configuration and could also use Windows.

[Fraser]

Interestingly enough, there seems to be a realtime extension system for Windows called RTX:
http://edageek.com/2010/03/03/soft-cont ... itectures/
I guess that it's not free though

I can only recommend switching to Linux, using a current Ubuntu makes switching much more painless than it was in earlier times. I find myself booting into Ubuntu basically all the time, even though I have a dual boot configuration and could also use Windows.

I have used ubuntu, It runs real nice even on an old 800MHz laptop. But I have never developed on it or for it. I'm reluctant to change dev platforms unless really necessary, but if push comes to shove then i will have to!:)

Many thanks for the heads up on RTX, I would really like that kernel patch, just for a realtime serial port if nothing else.

The RTX website seems tight lipped on price. I'm guessing it will be quite pricey. They do a free evaluation version, I may give it a spin soon and report my findings.

[Fraser]

This evening I have finished some initial code for AX12 and PC-Windows:

PC sends out 8bit PWM val according to a slider position.
Servo replies with 8bit position, PC displays position.

phpBB [media]

edit:
A little about the AX12 code structure, there is nothing in the main loop.

There is an interrupt that continuously samples the position ADC at 125kHz, the position used is the average of the last 8 samples.

There is an interrupt for USART RX, at the moment this service routine simply updates the Timer1 PWM register and then TXs the smoothed position back over USART. I am now developing the full comms state machine within the service routine. The RX ISR itself cannot be interrupted.

18 servo comms specs at the moment:
PC->AX12s packet: 20bytes
AX12s->PC packet: 36 bytes
I am fairly confident that 250Hz loop frequency (for 18 servos) will be possible over 250kbps serial line.

[Fraser]

OK, just thinking aloud:

Work ToDo(1):
I don't want to damage any servos when I come to putting them into a bot with physical joint limits, so my next step is to write torque limiting into the servo code.

The servo will calculate approx torque like this (using scaled integer math):

torque ~= pwm now / (position now - position some time ago)

If the servo sees that the position is not changing very much despite quite a strong PWM signal then PWM will be stopped and the servo LED will flash quickly. Normal PWM will resume once an opposite direction PWM is received or the servo is rebooted.

I'm debating whether I need a status bit in the servo reply packet to indicate this to the main controller. It may not be needed because the main controller can detect the stall state by looking at the PWM and position data.

Work ToDo(2):
Write comms state machine to make n (2...31) servos collaborate in constructing a reply packet.

If possible I want the system to tolerate single servos dropping off the bus. I don't want the bots legs to go limp or crazy just because it's damaged a wrist servo.

Idea 1)
Each servo needs to listen for the expected number of bytes beneath it. If the bytes for servos beneath it are not heard within a certain amount of time then the listening servo will send out pad data (0x8000) for the missing servo before it sends out its own data. This could get complicated for multiple consecutive servos down on the bus, but it may be doable ,more thought required on this.

Idea 2)
Send 5bit servo ID with data for each servo. Then each servo replies as soon as it hears its lower neighbours ID. If its lower neighbour is not heard within a timout period then it will go ahead and TX anyway. Missing servos will then simply be missing in the reply packet.

[billyzelsnack]

OK, just thinking aloud:

Work ToDo(1):
I don't want to damage any servos when I come to putting them into a bot with physical joint limits, so my next step is to write torque limiting into the servo code.

The servo will calculate approx torque like this (using scaled integer math):

torque ~= pwm now / (position now - position some time ago)

How does this work when attempting to hold a position? Why not just monitor temperature?

[i-Bot]

I was not sure about using the averaged position. It depends on the servo algorithm you use on the PC. This why I proposed the position reading synchronised with the timestamp in case the PC servo code needed to integrate and differentiate for velocity.

Damage to servos is almost always due to temperature rise in the motor. While the temperature monitored by the thermistor is the board temperature, this is mainly driven by the dissipation in the H bridge (Rds on times motor current, so actually a good predictive measure due to the lower thermal mass.

Mechanical damage to servo gears is usually due to impacts, not torque related.

If you specifically want torque limiting for other external reasons then I think you need torque ~= pwm (1-speed) since the speed determines the reverse EMF.

[Fraser]

OK, just thinking aloud:

Work ToDo(1):
I don't want to damage any servos when I come to putting them into a bot with physical joint limits, so my next step is to write torque limiting into the servo code.

The servo will calculate approx torque like this (using scaled integer math):

torque ~= pwm now / (position now - position some time ago)

How does this work when attempting to hold a position? Why not just monitor temperature?

Good point! my above math would be divide by zero in position hold case. I guess when the position is not changing (in position hold mode) then the torque is simply proportional to the applied PWM.

At the moment I'm not reading temperature because I've found that it takes quite a long time (not measured exactly) for the ADC to settle after switching the multiplexer to sample a different pin. I don't know how robotis go about it but if you wanted to sample sequentially, like posn,temp,vots,posn,temp...etc, then the sampling rate would be reduced to a much lower rate.

Obviously I'm going to have to start sampling volts and temp, perhaps tonight I will experiment a little on just how slow the sampling is when switching mux.

[Fraser]

I was not sure about using the averaged position. It depends on the servo algorithm you use on the PC. This why I proposed the position reading synchronised with the timestamp in case the PC servo code needed to integrate and differentiate for velocity.

Damage to servos is almost always due to temperature rise in the motor. While the temperature monitored by the thermistor is the board temperature, this is mainly driven by the dissipation in the H bridge (Rds on times motor current, so actually a good predictive measure due to the lower thermal mass.

Mechanical damage to servo gears is usually due to impacts, not torque related.

If you specifically want torque limiting for other external reasons then I think you need torque ~= pwm (1-speed) since the speed determines the reverse EMF.

At the moment the AX12 sample rate for position is 125kHz, the positions it sends are oversampled 8x so the average sample age is still quite young at only 32us.

The single timestamped packet, I will definitely incorporate into the finished protocol. At the moment I'm still toying with the idea of using the CM5 as a hub so it's easy to add IMU etc. It would also be able to manage servos dropping out more easily because the AX12/CM5 interaction are nice and solid in time (compared to a sloppy PC), of course in this case the CM5 will be responsible for adding an overall checksum, on top of the servos checksums. Programmed correctly the CM5 can start sending out the overall packet before the servos have all replied.

Intersting to hear about usual servo damage mode, I have not had these servos for long so I have not seen any damage (yet).

Writing torque safety limit into servo might be able to protect against impact, much like a human does (should). For example if we were to run without correct adjustments then we would quickly damage ligaments, muscles and even bones. If the servo could sense impact quickly enough (using posn data) and react accordingly (pull away from collision) then I wonder if it would be possible to prevent such injuries? I have noticed a little bit of physical spring in the AX12 drivetrain, this could give us a little more time to react.

[Fraser]

On the subject of sample smoothing, I thought I would share this algorithm with you all, it is plenty fast enough to go inside an ISR as I have it:



http://www.ledset.com/bioloid/code/adc_filter.txt

As you can see it contains no loops and no divide. To double up the sample window to 16 samples one need only increase FILTER_SHIFT to 4 without incurring any extra clock cycles.

edit: after a few painful minutes I found I had to post the above code on my own server, this forum won't display it correctly, even in normal text it gets mangled (not just in code block).

[Fraser]

I just got the raw data (as seen by the PC) into a spreadsheet.

I'm driving the servos fully clockwise for 0.5s then anticlock for 0.5s. This is the blue trace.

The red trace shows the position feedback as received over the serial USB2Dynamixel (1kHz sample rate over 250kbps).



And zoomed into a transition for a closer look, x axis is labelled with ms.

we can see a lag of ~25ms, I'm not sure how much of this lag is physical inertia and how much is comms.:

[Fraser]

Well, my last bit of work before christmas, I manged to close the control loop over the PC using a simple proportional control scheme:

Code: Select all

   const int DEADZONE = 16;
   int iDelta = Servo[0].ucPositionDesired - Servo[0].ucPositionNow;
   if(iDelta == 0)
   {
      Servo[0].ucPwm = 0x80;//NO POWER
   }
   else
   {
      int iPwm = (iDelta*4) + 0x80;
      if(iPwm > 0x80)
         iPwm += DEADZONE;
      else
         iPwm -= DEADZONE;
      if(iPwm > 0xFF)
         iPwm = 0xFF;
      else if(iPwm < 0x00)
         iPwm = 0x00;
      Servo[0].ucPwm = iPwm;
   }


phpBB [media]

[Fraser]

Well humanoidA is all back together with 18 servos all with new low level firmware. That was quite a ride!

Over a 250kbps link (servos directly to usb2dynamixel to win7 PC) the system achieves a 111Hz loop for all servos. After playing for over half an hour with lots of movements I did not count one checksum error on the PC (over 3.5million checksums were checked). I don't know If any checksum errors happened in any AX12, but movement seemed smooth throughout.

Control loop is closed with my cheapo win7 netbook (via usb2dynamixel). This gives average latency of ~25ms and of course the packets do not all lie perfectly evenly spaced, that's why I have to drop to 111Hz. If I used a realtime controller instead of my PC then the rate should theoretically go to 250Hz at 250kbps, according to simulator timings the firmware should be fast enough to reach 1KHz over a 1Mbps link.

Despite the PCs latency and jitter the closed loop control is quite good and all servos hold there positions without stuttering. Using a realtime controller should give solid control.

With the protocol being just; send out PWM and receive Position, I had to write in a dead mans handle to prevent the servos continuing to rotate if the comms are dropped. So in a short time interrupt I check for valid packets received since last check, if not found then PWM is killed and servo LED lights. When good packets resume PWM is live again and LED goes out.

This evening I experimented with mixing sinusoidal position sequences on top of user control, this yielded some quite organic movements. I will shoot a short video tomorrow showing that.

I must add that debugging this system with multiple servos was quite tedious to say the very least! luckily I didn't have to go through too many debug iterations. Without a serial line sniffer it would have been painful. This sniffer worked great and is free to download:

http://www.serial-port-monitor.com/index.html

edit: I had better update the comms spec to reflect the actual protocol

[Fraser]

If anyone wants to play with closing the servo loop on their PC then here's the initial release:

http://www.ledset.com/bioloid/axons/