ServoEx: Start from Servo or SoftPWMServo???

Created Wed, 21 Mar 2012 00:28:07 +0000 by KurtE

KurtE

Wed, 21 Mar 2012 00:28:07 +0000

Background: I have a few different Hex robots that are built with the Lynxmotion Servo Erector Set components. Originally all of them ran on processors developed by Basic Micro (Basic Atom Pro28 and Arc32) running their version of Basic. I later ported this code to C that ran on these same processors and then later to other processors including the Arduino as well as the Uno32.

On several of my robots, I use a Lynxmotion Servo Controller (SSC-32), which takes care of generating the servo pulses for up to 32 servos. A nice feature of this controller, is I can give a command like: #0P1500#1P1500#2P1500T500 And it will move servos 0,1,2 to the pulse width of 1500 from whatever they were before and they should all get that position in 500ms This is great for doing walking gaits and the like and our hexapod code (Phoenix) uses this. My first port of our code to the Arduino/Uno32 used the SSC-32 to do this.

On some of my robots, I have the main processor also do the Servos. The first was the Arc32 still in basic. The basic had a command HSERVO which allows you to specify a new servo position as well as a speed value (how much to add/subtract per each 20ms cycle), Not as nice as the SSC-32 command, but it is workable. That is suppose you wish for a servo to go from its current position of 1500 to 2000 and you wish for it to get there in 500ms. From this you compute a delta or 500 (2000-1500) and this will take 25 (500/20) servo cycles. So you need for that servo to change by 500/25=20 units per servo cycle.

Now I would like to do the same on the Arduino and Chipkits. On Arduino 1.0 I already hacked up a version of the Servo library (I called ServoEx), I added a 2nd class that says start a group move, then I do my calls to the servos to tell them their new position. While a group move is active, they do not do it until I call the commit method with a time value. The commit code computes the delta times like I mentioned for the HSERVO command above. I modified the interrupt handler to then update the servo position after each cycle. I think I have the code working reasonably well on the Arduino Mega. You can see a quick and dirty video of it up on the Lynxmotion forum: http://www.lynxmotion.net/viewtopic.php?f=20&t=8038 Also up there is a checkpoint of the code, including the ServoEx library.

[color=#FF0000]Now for the question: [/color] I would like to build a library for the Chipkit that gives me this same type of functionality. The question is, should I start off with the Servo Library or should I try building it off of the SoftPWMServo library? Hopefully later this week I will have one of my prototype Servo shields assembled enough to try it out with my Max32 board.

Suggestions?

Kurt

EmbeddedMan

Wed, 21 Mar 2012 02:35:03 +0000

I'm not saying go one way or the other, but make sure you understand the limits of each before you head too far down the road. I can't remember exactly, but I think the normal servo library uses one timer for each servo output, right? There are 5 timers on the PIC32, so you could have a total of 5 timers. (Somebody correct me if I'm wrong here.) With the SoftPWM library, you can have up to about 84 servo outputs. (every I/O pin)

That said, as you add more and more servos to the SoftPWM library, they will have more and more jitter. You may not notice it, you may. Depends a lot on what kind of servos you use. But do some simple testing out before committing to it.

Also, I think you could use both at the same time - 5 low jitter hardware channels and the rest soft-servo channels.

*Brian

KurtE

Wed, 21 Mar 2012 04:35:42 +0000

Thanks,

My impression is that the Servo library can handle several servos per timer. I thought on the Arduino it was 12 servos per timer. It appears that on the Chipkit this is 8 servos pr timer. It says that it ahas a maximum of 24 servos, which implies 3 timers can be used.

I like your idea of trying both out and see what happens. So I will do that as soon as I can get the hardware setup.

Thanks again Kurt

KurtE

Thu, 29 Mar 2012 20:18:08 +0000

I have my code running on my hacked up version of the servo library and it appears like my Hex robot with 18 servos can probably walk :). However the servos are not moving as smoothly as I would expect and I need to investigate. I thought it would be interesting to see how the results compare if I instead started from the SoftPWMServo code instead as this would alleviate 3 timer ISR procs running.

I am trying to work out a reasonable way to adjust the timings of each of the pulses during each servo cycle, to make smooth coordinated moves for my robots. While doing this I am finding, I have questions about the code or obvious optimizations, that can be done in certain or all cases. I will give a few examples below.

[color=#FF0000]But the main question is, are there others out there who would like to follow along as I work my way through this (and or at the end). If so I will continue to post here or PM if appropriate... If not, will I am having fun.[/color]

Note: I am mainly only interested in Servo pulses. Observations/questions:

1. With servo pulses: we only set the pulse high once per servo cycle, but we set it low every Frame Counter or 10 times per servo pulse. This includes setting up the code to be called to do this. So if my 18 servos all have different pulse widths, we are being called 162(18*9) times per servo cycle that is not necessary. For my first pass through, I added 2 global bool values (HaveServos and HaveNonServos) that added code to SoftPWMServoPinEnable:

HaveServos |= PinType;			// Note: We only set this, never clear once set...
	HaveNonServos |= !PinType;		// dito...

Then changed the HandlePWMServo code that when we are in the Rising Edge code, instead of simply testing CurChanP for Null, I also check to see if our ServoFrameCounter is 0 or we have some non servos... Like:

do 
    {
        // If it's time to do the rising edge of all enabled channels-
        if (RisingEdge)
        {
            // Start at the first channel
#ifdef DEBUG_SIGS
			*(g_InvPort3) |= g_bit3;
#endif    
            CurChanP = ISRFirstChanP;

            // Check to see if we have zero channels actually loaded with data
            if ((CurChanP != NULL) && (!ServoFrameCounter || HaveNonServos))
            {

Note: this also has some debug code, so I can watch what is going on, on my Logic Analyzer...

1-a) Have not done this, but could add a few more tests when there are non-servos, to only process the servos on ServoFrameCounter ==1...

2 Nit) I thought I saw code that would fault:

else
            {
                // Now start working on the next channel in the linked list
                CurChanP = CurChanP->NextChanP;

                // Time to compute the NextEdgeTime for the next channel
                // But only if we're not at the end.
                if (CurChanP != NULL)
                {
                    // Compute the next channel's NextEdgeTime based upon our current time and it's PWMValue
                    CurChanP->NextEdgeTime = CurChanP->PWMValue - CurrentTime;
                }

                // Just load up the time of the next falling edge
                NextTime = CurChanP->NextEdgeTime;
            }

That is we test CurChanP for NULL and use it and then after the if, we use it again without testing for NULL. But found that we were safe as the else of this code is for the if

if (CurChanP->NextChanP == NULL)

So in the above else code, you can remove the test for NULL...

3. Related to above. I am not sure why NextEdgeTime needs to be part of the array of structures. You only set it for the next item to be processed. Which more or less is a duplicate of the variable NextTime. We can probably simply keep one static value for this and save: 8642=688 bytes.

3a) Size of data elements. I am still semi new to Pic32, so I don't know if reading/writing 32 bytes at a time is much faster, cut currently we have:

struct ChanStruct
{
    int32_t             NextEdgeTime;           // Time in 40MHz units before next edge (i.e. this pin's falling edge)
    volatile uint32_t   *SetPort;               // Pointer to port register (SET) for this pin
    volatile uint32_t   *ClearPort;             // Pointer to port register (CLEAR) for this pin
    uint32_t            Port;                   // Port number for this pin
    uint32_t            Bit;                    // Bit of Port that this pin is on
    uint32_t            PWMValue;               // Length of high time for this pin, in 40MHz units
    uint32_t            IsServo;                // True if this channel is being used as a servo rather than PWM
    ChanType *          NextChanP;              // Pointer to next channel in the list (next edge)
};

On a Max32 there are 86*2 elements of this structure, so it is eating up a reasonable number of bytes. Yes I understand MAX32 has a reasonable amount of memory, but I hate wasting it. Right now I believe that this can be trimmed back quite a bit, like: NextEdgeTime (mentioned above), Port(not needed as we have set port and Clear Port). Bit only needs 16 bits, IsServo only needs 1 bit. My guess is that PWMValue can maybe be trimmed to 16 bits?...

4. CopyBuffers can be sped up. You only have to copy the PWMValue, NextEdgeTime (if I keep it). But things like SetPort/ClearPort/Bit won't change. Possibly need to copy IsServo, if we think this will change...

5. Looking at when I change the PWMValue to only have to remove the element and re add it when the element needs to be moved in the list...

That is all for now.

Kurt

Edit: Thought I would upload a WIP version I have, that removed 12 bytes per element, reduced calls for all servo setup...

EmbeddedMan

Fri, 30 Mar 2012 19:14:23 +0000

Kurt,

As far as I know, you're the only person to have really dug into my library in this detail. So I'm sure there are tons of things that can be optimized and made better. All your suggestions sound great!

I'd love to test out your changes once you have them all made. Everything you listed makes sense to me, with nothing that jumps out as "not a good idea". You will need to very careful profile changes related to changing the data sizes (i.e. read through the compiler generated assembly in detail) as manipulating 1 bit compared with 1 word might generate a lot more instructions. It might not. I haven't tried yet.

So go make your optimization, and get it working for what you need, then send it along to me so I can test it too. I'd love to get it merged back into the official MPIDE distribution.

*Brian