Strange Spin Problems. Help please!

ReiserificK · 2009-05-20 01:53

Hi everyone, I am working on an autonomous truck, and I am having problems with my servo control code. The following is a loop that constantly runs, and checks two longs, ServoThrottleAngle and ServoSteeringAngle, which are just numbers from -40 to 40 to be added to the 1.5 us pulse to the servos (150 + ServoSteeringAngle). I have had some very strange bugs with the code. For starters, I had to add the iterator (iter) to stop the loop from stalling, seemingly randomly, and it also doesn't respond to values between -10 and 10, however, a zero value for one of the longs seems to pulse a perfect 1.5 us. Any guidance would be greatly appreciated.

Thanks!

PUB ServoControlLoop( Lock ) | iter

  iter := 0

  dira[noparse][[/noparse]ServoThrottlePin]~~         'set pin to output direction
  dira[noparse][[/noparse]ServoSteeringPin]~~ 
  repeat
    if ( iter > 500 )
      iter := 0
    iter += 1
    repeat until not lockset(Lock) 'wait until we lock the resource
    if( LCDDebug == 1 )
      'debugLCD.cls
      'debugLCD.str(string("SSAVal: "))
      'debugLCD.str(N.dec(ServoSteeringAngle))
      'debugLCD.gotoxy(0,1)
      'debugLCD.str(string("Iteration "))
      'debugLCD.str(N.dec(iter))
    
    
    'Throttle
    outa[noparse][[/noparse]ServoThrottlePin] := 1                              'set it to 1
    waitcnt(clkfreq / 100000 * ( 150 + ServoThrottleAngle ) {x.xx ms} + cnt)           'not ms, but ms/10, takes about 20 us to complete this operation
    outa[noparse][[/noparse]ServoThrottlePin] := 0
    waitcnt(clkfreq / 1000 * 20 + cnt)          'ms this time, wait 20 ms before pulsing again

    'Steering
    outa[noparse][[/noparse]ServoSteeringPin] := 1                              'set it to 1
    waitcnt(clkfreq / 100000 * ( 150 + ServoSteeringAngle ) {x.xx ms} + cnt)           'not ms, but ms/10, takes about 20 us to complete this operation
    outa[noparse][[/noparse]ServoSteeringPin] := 0
    waitcnt(clkfreq / 1000 * 20 + cnt)          'ms this time, wait 20 ms before pulsing again
    
    lockclr(Lock) 'unlock the resource

W9GFO · 2009-05-20 02:35

I highly recommend using the Servo32v5 object available in the Obex. It is dead easy to use and you don't need to be concerned with the timing.

Rich H

agodwin · 2009-05-20 06:55

The need for 'iter' is very suspicious - it shouldn't have any effect at all.
Any time I have unexpected effects like that, the first thing I check is whether I allocated enough stack.

Ale · 2009-05-20 07:08

If all the code runs on the same COG, the use of a lock is not needed and as agodwin pointed out, you can do without such "wait" mechanism using waitcnt.

I'd recommend the use of the Servo32 so many have used and are happy with. Of course you can use your own code, but first get something working

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Visit the home of pPropQL, pPropQL020 and OMU for the pPropQL/020 at omnibus.uni-freiburg.de/~rp92

MagIO2 · 2009-05-20 08:48

I guess the ServoControlLoop is continuously running in a separate COG whereas the values of ServoThrottleAngle and ServoSteeringAngle are modified in another COG.

The lock is needed if the values of ServoThrottleAngle and ServoSteeringAngle are related with each other - which means you want to avoid the following scenario:
COG2 (running ServoControl) reads ServoThrottleAngle and sends the throttle pulse
COG1 updates ServoThrottleAngle and ServoSteeringAngle
COG2 reads ServoSteeringAngle and sends the steering pulse
...
But I'd do the lock different. If you have a look at the loop, COG1 seldomly gets a chance to·catch the lock. That's why the iterator helped a little bit. It simply added runtime without locking the resource by COG2.

PUB ServoControlLoop( Lock ) | [color=red]localThrottle, localSteering[/color] [s]iter[/s]

  [s]iter := 0[/s]

  dira[noparse][[/noparse]ServoThrottlePin]~~         'set pin to output direction
  dira[noparse][[/noparse]ServoSteeringPin]~~ 
  repeat
    [s]if ( iter > 500 )[/s]
      [s]iter := 0[/s]
    [s]iter += 1[/s]
    repeat until not lockset(Lock) 'wait until we lock the resource
[color=red]    localThrottle := ServoThrottleAngle[/color]
[color=red]    localSteering := ServoSteeringAngle[/color]
    [color=red]lockclr(Lock) 'unlock the resource
[/color]    
    'Throttle
    outa[noparse][[/noparse]ServoThrottlePin] := 1                              'set it to 1
    waitcnt(clkfreq / 100000 * ( 150 + [color=red]localThrottle[/color] ) {x.xx ms} + cnt)           'not ms, but ms/10, takes about 20 us to complete this operation
    outa[noparse][[/noparse]ServoThrottlePin] := 0
    [s]waitcnt(clkfreq / 1000 * 20 + cnt)          'ms this time, wait 20 ms before pulsing again[/s]

    'Steering
    outa[noparse][[/noparse]ServoSteeringPin] := 1                              'set it to 1
    waitcnt(clkfreq / 100000 * ( 150 + [color=red]localSteering[/color] ) {x.xx ms} + cnt)           'not ms, but ms/10, takes about 20 us to complete this operation
    outa[noparse][[/noparse]ServoSteeringPin] := 0
    waitcnt(clkfreq / 1000 * 20 + cnt)          'ms this time, wait 20 ms before pulsing again
    
    [s]lockclr(Lock) 'unlock the resource[/s]

One waitcnt for 20ms is enough, otherwise the wait time will extend to 40ms per output (wait 20ms after throttle and wait 20ms after steering).
Currently I don't see a reason, why the pulses should not work with any given throttle/steering angle.

PS: and remember ... local variable added, so the stack size might need to be adjusted

Post Edited (MagIO2) : 5/20/2009 8:54:31 AM GMT

Strange Spin Problems. Help please!

Comments