PhantomX with phoenix code, how to use timer interrupts?

estelle · February 19, 2014, 1:01pm

Hi
I’m currently sending out all 18 servo positions in serial to pin D2 on the Arbotix robocontroller with the use of the SoftwareSerial library. This is my function:

void BeenPos() { int id; for (id = 1; id<=18; id++) { mySerial.print(id,HEX); mySerial.print("-"); mySerial.print(ax12GetRegister(id,AX_PRESENT_POSITION_L,2),HEX); } }

I need to be sending this feedback into an extended Kalman filter at regular intervals and thus need to use a timer interrupt (to the best of my knowledge). I just can’t seem to figure out how to do this for the Arbotix in the phoenix code. Any advice will be appreciated a lot.

Thanks

kurte · February 19, 2014, 2:51pm

There are probably some others up on Trossens forum that use the Arbotix board more than I do and have more experience than I do with the Atmega644p and can answer some specific questions about the process better than I can. But as I am more or less the owner of the Phantom Phoenix code base. I will give you some hints on what I would try.

Timer Interrupts: If you wish to go this way. Look at the AVR ATMega 644p reference Manual (pdf) at the timers chapters. In the PDF I have this is chapters 12-14. This will show you the register names and values and the like. I would then look at examples of the code that use these. Example the servo library as an example…

However I personally would not go down that route as I think this will cause your Phantom to not move smoothly. Why? Most code that I have seen does not use the firmware of the servos to do the timed moves from a current location to a new location. In theory you can do this using the speed settings, but I never got this to work well for me. (Note: KevinO up on Trossen and here has had some better luck but that is not in this code base). So instead I use a slightly modified version the bioloid library Interpolate functions. (BioloidControllerEx::interpolateSetup and int BioloidControllerEx::interpolateStep(boolean fWait). In particular my version of the library decides if it will wait for the next step time or not.

So in order for the Bioloid interpolation to work smoothly, it needs to be called at the correct times, for it to calculate the next place the servos are supposed to be and to then output the commands to update the goal positions for each of the servos. So suppose that you create an interrupt for lets say Timer2, while walking and you try to do the function you showed from the ISR. You will run into lots of issues.

Timing for the next cycle(s) can and likely to be screwed up.
If you do your function from the ISR - by default interrupts are disabled in the ISR, and the default AX library uses interrupts to receive characters. So likely your ax12GetRegister will hang.
You might be able to enable interrupts again within your ISR, but SoftwareSerial disables Interrupts when it sends and as it’s ISR does not enable interrupts, during the reception of a character). So depending on other things, you can loose interrupts.
I could go into more on this part if that would help.

What I would try is: To handle the periodic updates of servos, I added some optional code into the servo drivers of the Arduino Phoenix code base, which is enabled on the PhantomX. Not sure which version of the code base you are using. But if you are using the self contained one (Phantom_Phoenix project up on Github?) Look at the _Phoenix_Driver_AX12.h file. If you are using my in parts project (drop the leading _).

If you look at the function: void ServoDriver::BackgroundProcess(void)
You will see that I call off to the Bioloid code to output the servos. Also my more recent versions shows calling out to a servo to get it’s battery voltage at a time that should not interfere with the timings.

So I would do yours the same way. That is I would add it to this function and/or a call of to your function, that checks to see if it is time to do something…

I would also try to decide if I really needed to ask the servo for it’s position (as that takes sending out a packet and waiting for a response). I would probably look at using the function:
int BioloidControllerEx::getCurPose(int id)
To retrieve the position I last output, knowing that we are only doing very fine adjustments. However if you really need the actual servo position for things like detecting obstructions or the like, well you can do.

Also I would try to break this up. That is requesting data from all 18 servos and then bitbanging the data back to other processor will take time and will probably again interfere with the next bioloid time frame. So you then decide to get the state of one servo per cycle and see if this gets fast enough response for you. Or you can try 2 or 3 and see if this is still working OK.

At least that is how I would try it out on the Arbotix. Personally I am having more fun using things like the Teensy 3.1 that has 4 UARTS (including USB) and on this one I would probably use an Interval timer. Would still want to be smart about not interfering with the updates for the interpolation…

Hope that helps
Kurt

estelle · February 20, 2014, 6:47am

Thanks so much for your help and insight. Will let you know how it goes.
Regards

estelle · February 20, 2014, 2:08pm

Hi Kurt
First, thanks a lot for your help. So I am using the standard PhantomX mark II kit as sold with your Github code that is given on the Interbotix Labs link provided by Trossen Robotics (learn.trossenrobotics.com/10-int … oenix-code ). I need the actual feedback from the servos for a state estimation filter. The filter needs all 18 servo positions every time step k. I actually just want to send the servo positions 1 to 18 every k ( only adding the “id-“ for debugging purposes now).
I tried calling my function in your void ServoDriver::BackgroundProcess(void) function as you suggested:

[code]//######################################
{
int id;
int BeenPosFeedback;
for (id = 1; id<=18; id++) {
BeenPosFeedback = (ax12GetRegister(id,AX_PRESENT_POSITION_L,2));
mySerial.print(id,HEX);
mySerial.print("-");
mySerial.print(BeenPosFeedback,HEX);
bioloid.interpolateStep(false);
}
}

//###############################################
//==============================================================================
// BackgroundProcess - Allows us to have some background processing for those
// servo drivers that need us to do things like polling…
//==============================================================================
void ServoDriver::BackgroundProcess(void)
{
if (g_fAXSpeedControl)
return; // nothing to do in this mode…

if (ServosEnabled) {
DebugToggle(A3);
//###########################################
BeenPos();
//###########################################

// int iTimeToNextInterpolate = 1;
bioloid.interpolateStep(false); // Do our background stuff…
}
}[/code]
Please help me if I understood you wrong with regards to how I implemented this.

The results look much better in terms of the data being sent out at a more regular pace. (Please see the logic analyzer images attach.) The servo positions 1 to 18 are send out in +/-35ms-43ms with a 3.2ms-3.6ms gap to the next 1-18 package. The total time from one 1-18 package to the next differ a lot. There is however a gap of +/- 10ms every now and then where the for loop of my function looks to be interrupted. The position of the specific servo that follows after this gap is an error. Also, sometimes there seems to not even be a gap between the one 1-18 feedback and the next 1-18 feedback (which I don’t mind if it could be like that the entire time).

Thanks again for helping.
Regards

kurte · February 20, 2014, 3:26pm

As I mentioned, you may find difficulties with timings, also note, that there is other code that needs to run if the robot is actually going to walk… Also not sure of your complete setup. Example are you still using the XBee on the Arbotix? What Baud rate are you using to output at for software serial (myserial?) I am assuming probably not over 38400?

When I am trying something like this, I first try to do some of the simple math to see how possible it is, then since I have logic analyzers, I instrument the code some, to see what is happening. Can I figure out when things are conflicting and I am getting bad results. Then adjust.

Example: By default I think I have the Phantom Phoenix setup to do the interpolation 50 times per second (#define BIOLOID_FRAME_LENGTH 20)
So a frame is 20ms. To output a new move, the SYNC_WRITE is 62 bytes to be output at 1mb baud at best would take 620us. Normally I think I have observed it more like 650us So that fits. Note: you can play with this value, Some programs only update 33 times per second, but the smaller the value (more frames), the smoother things can go, but it also uses more of the CPU. So it needs to be balanced.

Now looking at your loop: You do an ax12GetRegister of 2 bytes: So the packet out is I believe 8 bytes, and the response packet is 8 bytes. So at minimum with no delays for the response to start or delays in the output of bytes by the servo. At minimum the call to ax12GetRegister will take 150us but more likely closer to 500-1000us and sometimes longer. Example look at the thread: forums.trossenrobotics.com/showt … ght=usb2ax (post #36) I found at times an AX servo may delay as much as about 1ms between characters. Again to help with this you should look into updating the servos return delay times and the like:

But for argument sake lets assume you can get the call down to 200us, times 18 = 3600us. Now you need to take into account how long your output to myserial is taking: About 6 bytes per servo so about 108 bytes per pass. If my simple math is correct at 38400, each character takes about: 260us * 108 = 28125 or 28ms which is greater than our refresh cycle… So probably an issue. Obviously if you are running Software Serial at the highest speed of 115200 you can get this down to maybe: 9.4ms assuming flat out now delays, but this is pushing it.

Again with Logic Analyzer, I would start adding debug stuff, like setting an IO line high when in some functions to see if that is coming into play. Example are you getting corruption or delays when you are in processing input from the XBee? So in the main loop, I might set high a IO pin before the call to Control Input and set it low afterwards. In fact there are calls in my code that are currently commented out.

I might also toggle an IO pin each time I am about to commit new changes (start a new interpolation). g_ServoDriver.CommitServoDriver(ServoMoveTime);
And in fact there is code setup with a DebugToggle (which can be turned into an actual toggle or compiled out). This way you might find out if starting up a new position is screwing up.

…

Good Luck
Kurt

estelle · February 27, 2014, 9:59am

Hi

So I have a solution to what I wanted. If anyone is interested, this is how I did it with Kurt’s guidance as you can read above. (Thanks Kurt).

Basically I’m sending the servo positions, 1 to 18 out every 50ms to pin D2. I made use of the Arduino SoftwareSerial library as well as a SimpleTimer library that you can find on Arduino (playground.arduino.cc/Code/SimpleTimer#.Uw8HJ4X4wSk).

In void setup() of the Phoenix_Code.h I set up my serial communication speed:

mySerial.begin(115200);

The following was inserted in Phoenix_Driver_AX12.h:

Included the libraries and initialize my software serial pins:

#include <SoftwareSerial.h> SoftwareSerial mySerial(3, 2); //RX = 3 en Tx = 2= D2 SimpleTimer timer;

The function needed to send out the servo positions 1 to 18:

void BeenPos(){ int id; int BeenPosFeedback; for (id = 1; id<=18; id++) { BeenPosFeedback = (ax12GetRegister(id,AX_PRESENT_POSITION_L,2)); mySerial.print(BeenPosFeedback,HEX); } }

In Kurt’s void ServoDriver::BackgroundProcess(void) function I added the code between “#”:

[code]void ServoDriver::BackgroundProcess(void)
{
if (g_fAXSpeedControl){
return; // nothing to do in this mode…
}

if (ServosEnabled) {
DebugToggle(A3);
//############################################
timer.setInterval(50, BeenPos); // Call BeenPos every 50ms
timer.run();
//###############################################
bioloid.interpolateStep(false); // Do our background stuff…
}
}[/code]

Lastly, to keep the SimpleTimer library from sending out the positions 10 times because it is starting up 10 timers, I just changed the maximum number of timers in SimpleTimer.h from 10 to 1:

[code]typedef void (*timer_callback)(void);

class SimpleTimer {

public:
// maximum number of timers
//###########################################
const static int MAX_TIMERS = 1; // was 10
//###########################################
[/code]

Most of you will probably be able to this cleaner and better as I’m not such a good programmer, but I hope this helps someone…

Regards Estelle (;