Trouble shooting a simple spin servo driver
Copper
Posts: 48
So I've run into a little trouble with my servo driver, and I'm sort of at a loss as to how I should proceed.
First off, you should know, I'm writing this driver as a learning exercise. I'm very new to programming. So while it may seem like a wasted effort, I would really like to continue developing this bit of code. I have downloaded and mulled over a number of servo drivers from the object exchange, but find most of it goes over my head. So I'm hoping building this driver from the ground up will help to expose me to all that needs to be addressed in a servo driver, and at least one way of doing it, so that I can begin to explore other drivers with some understanding.
I believe I have narrowed the problem down to some inter-relationship between multiple cog management and my signal timing.
Above, Is a trimmed up version of my driver (no ramping or serial port communication [neither of which probably work yet anyhow; we'll leave that for next time]).
When I run the code, the servo drives for 3sec at the given rate, but then there is a min+ delay before it starts again, it does start at the given rate and runs for 3sec, but I can't get it to start a third time (by changing the given parameters) or reduce the delay.
Hopefully something is glaringly wrong and easily explained, but whatever I've done wrong it's not apparent to me.
As always,
Thanks for your help.
First off, you should know, I'm writing this driver as a learning exercise. I'm very new to programming. So while it may seem like a wasted effort, I would really like to continue developing this bit of code. I have downloaded and mulled over a number of servo drivers from the object exchange, but find most of it goes over my head. So I'm hoping building this driver from the ground up will help to expose me to all that needs to be addressed in a servo driver, and at least one way of doing it, so that I can begin to explore other drivers with some understanding.
I believe I have narrowed the problem down to some inter-relationship between multiple cog management and my signal timing.
{{Basic Servo Stuff}}CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
#0, For4, For3, For2, For1, stop, Rev1, Rev2, Rev3, Rev4
VAR
long stackspace[100]
word cog
PUB Main
Start_Servo(12, For2, 3000)
waitcnt(clkfreq*3)
Start_Servo(12, For4, 3000)
waitcnt(clkfreq*3)
Start_Servo(12, Rev2, 6000)
waitcnt(clkfreq*6)
PUB Start_Servo(servoPin, speed, duration)
cog:=cognew(Run_servo(servoPin, speed, duration), @stackspace[0]) + 1
PUB Stop_Servo
if cog
cogstop(cog~ - 1)
PUB Run_Servo(servoPin, speed, duration) | tInc, dInc, tc, tHa, t, run_time
'configure counterA
ctra := (100 << 26 | servoPin) 'Counter A to NCO & Apin to servoPin
frqa := 1 'increment phsa by 1
dira[servoPin]~~ 'set servoPin to output
'configure pwm signal
tInc := clkfreq/1_000_000 'set tInc == 1 microsecond
tC := tInc * 20_000 'determine pulse delay
tHa := tInc * Speeds[speed] 'determine pulse width
t := cnt 'mark counter time
'detirmine runtime
dInc := clkfreq/1000 'set dInc == 1millisecond dInc == Duration Resolution
run_time:=((duration*dInc)+t) 'set run_time
'begin sending signal
repeat until t=>run_time 'start PWM signal
phsa := -tHa 'send pulse for tHa seconds
t += tC 'calculate end of cycle
waitcnt(t) 'wait for next cycle
DAT
Speeds long 1000, 1200, 1450, 1500, 1515, 1530, 1580, 1830, 2000
Above, Is a trimmed up version of my driver (no ramping or serial port communication [neither of which probably work yet anyhow; we'll leave that for next time]).
When I run the code, the servo drives for 3sec at the given rate, but then there is a min+ delay before it starts again, it does start at the given rate and runs for 3sec, but I can't get it to start a third time (by changing the given parameters) or reduce the delay.
Hopefully something is glaringly wrong and easily explained, but whatever I've done wrong it's not apparent to me.
As always,
Thanks for your help.
Comments
and kuroneko was right about the waitcnt command. So I went about adding a method for Ramping the servo speed, and a debug method to report the actual "speed" of each pulse. But I've hit another roadblock.
I think the problem is in my ramping method
{{Basic Servo Stuff}}CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 #0, For4, For3, For2, For1, stop, Rev1, Rev2, Rev3, Rev4 OBJ pst : "Parallax Serial Terminal" VAR long servo_space[14] long ramp_space[32] long debug_space[32] word servo_cog, ramp_cog, debug_cog PUB Main debug_cog:=cognew(DEBUG(12), @debug_space[0]) waitcnt(clkfreq*3) Start_Servo(12, For2, 3000) waitcnt(clkfreq*4+cnt) Start_Servo(12, For4, 3000) waitcnt(clkfreq*4+cnt) Start_Servo(12, rev3, 6000) waitcnt(clkfreq*7+cnt) PUB Start_Servo(servoPin, speed, duration) servo_cog:=cognew(Run_servo(servoPin, speed, duration), @servo_space[0]) + 1 'launch and remember servo_cog @servo_space ramp_cog:=cognew(Ramp_Servo(servoPin, speed), @ramp_space[0]) + 1 'launch and remember ramp_cog @ramp_space PUB Stop_Servo if servo_cog 'if servo_cog == not 0 cogstop(servo_cog~ - 1) 'stop and postclear servo_cog cogstop(ramp_cog~ - 1) 'stop and postclear ramp_cog PUB Run_Servo(servoPin, speed, duration)| t, run_time 'time management tInc := clkfreq/1_000_000 'set tInc == (cnt ticks per microsecond) dInc := clkfreq/1000 'set dInc == (cnt ticks per millisecond) tC := tInc * 20_000 'set delay period to 20_000 microseconds 'configure counterA ctra := (100 << 26 | servoPin) 'Counter A to NCO & Apin to servoPin frqa := 1 'increment phsa by 1 dira[servoPin]~~ 'set servoPin to output 'detirmine start and runtime repeat until t >< 0 'repeat until start[servoPin]== >< 0 t := start[servoPin] 'set t== start[servoPin] run_time[servoPin]:=((duration*dInc)+t) 'set run_time 'begin sending signal repeat until t=>run_time[servoPin] 'start PWM signal phsa := -(tInc * Speeds[LS[servoPin]]) 'send pulse for tHa seconds t += tC 'calculate end of cycle waitcnt(t) 'wait for next cycle 'clear servo data LS[servoPin] := 0 start[servoPin] := 0 PUB Ramp_Servo(servoPin, speed) | t if speed > LS[servoPin] 'if... then ramp up t := start[servoPin] := cnt 'mark and share start time repeat until LS[servoPin] >= speed 'repeat until at speed LS[servoPin]++ 'increment speed t += tC 'calculate end time waitcnt(t) 'wait until then elseif speed < LS[servoPin] 'if... then ramp down (currently unecessary) t := start[servoPin] := cnt repeat until LS[servoPin] <= speed LS[servoPin]-- 'decrement speed t += tC waitcnt(t) PUB DEBUG(servoPin) | i pst.start(57600) 'start pst waitcnt(clkfreq*2) 'wait dira[12]~ 'set 12 to input repeat if tInc >< 0 'If tInc has been set pst.str(string(13, "tInc==")) ' pst.bin(tInc, 32) ' pst.str(string(13, "dInc==")) 'report config values pst.bin(dInc, 32) ' pst.str(string(13, "tC==")) ' pst.bin(tC, 32) ' pst.str(string(13, "*****************************")) 'indicate begining of next loop repeat 'repeat forever repeat until dira[12]==1 'do nothing until dira[12]==high 'do nothing i++ 'increment i pst.str(string(13, "Pulse#")) 'display pulse number using i as index pst.dec(i) ' pst.str(string(13, "LS==")) 'report LS of pulse#i pst.dec(LS[servoPin]) ' elseif tInc == 0 'If tInc has not been set pst.str(string(13, "servo not yet configured")) 'report "servo not yet configured" DAT tInc long 0 dInc long 0 tC long 0 Speeds long 1000, 1200, 1450, 1500, 1515, 1530, 1580, 1830, 2000 LS long 0[32] start long 0[32]I'm sure there is some fundamental flaw in my approach here; of course Whatever advice you have would be greatly appreciated.
PS: given the delay between posts, should I start a new thread?
Only if you want to be annoying!
I think I pointed out that I and others have written viable servo drivers. I would suggest walking away from your project -- only for a moment -- to disect what others have done. Then go back. I use this trick on myself all the time. When I'm stuck, I turn to another technical bit of work to get my mind moving again. You may find, as I frequently do, that the time spent away allows you to see your project in a new light when you come back to it.
PS: I asked about new threads because I was un-sure of the existing forum etiquette.