PDA

View Full Version : Fixed Servo Repeat Object



RubrChicken
03-21-2009, 01:57 PM
Travis' Simple Capture and repeat to servo was very jerky, so I decided to fix it. I kind of stabbed in the dark and got it right! Instead of pushing pulsins to servo 32v3, I made them go directly to pulsout and it fixed the jerkiness.


The new code :

NOTE: I took out the original debug code, because 1) I didn't need it for my project, and 2) I have a Mac, which currently doesn't have a serial terminal (that I can get working)






{{
Origional Code: Travis
Modded By: Forrest (rubrchicken)

}}


CON

_CLKMODE = XTAL1 + PLL16X
_XINFREQ = 5_000_000

'Pin on Prop Chip for LED Indicator
PIN_LED = 15

'Pins on Prop Chip for Servo Input/Output
PIN_SERVO_IN = 0
PIN_SERVO_OUT = 1


var
long us

OBJ
'Debug : "FullDuplexSerial" 'standard from propeller library
SERVO : "Servo32v3" 'standard from propeller library

PUB Begin(in,out) | x,y,z
us := clkfreq / 1_000_000 ' Clock cycles for 1 us

'SERVO.Set(out,1422)
'SERVO.Start
y := PULSIN(in,1)

'SERVO.Set(out,(y * 2))
PULSOUT(out,y)
'waitcnt(clkfreq / 4 + cnt) '0.25 seconds


PUB PULSIN (Pin, State) : Duration | usa
usa := clkfreq / 1_000_000 ' Clock cycles for 1 us

Duration := PULSIN_Clk(Pin, State) / usa / 2 + 1 ' Use PulsinClk and calc for 2uS increments \

PUB PULSIN_Clk(Pin, State) : Duration

DIRA[pin]~
ctra := 0
if state == 1
ctra := (%11010 << 26 ) | (%001 << 23) | (0 << 9) | (PIN) ' set up counter, A level count
else
ctra := (%10101 << 26 ) | (%001 << 23) | (0 << 9) | (PIN) ' set up counter, !A level count
frqa := 1
waitpne(|< Pin, |< Pin, 0) ' Wait for opposite state ready
phsa:=0 ' Clear count
waitpeq(|< Pin, |< Pin, 0) ' wait for pulse
waitpne(|< Pin, |< Pin, 0) ' Wait for pulse to end
Duration := phsa ' Return duration as counts
ctra :=0 ' stop counter

PUB PULSOUT(Pin,Duration) | clkcycles
{{
Produces an opposite pulse on the pin for the duration in 2uS increments
Smallest value is 10 at clkfreq = 80Mhz
Largest value is around 50 seconds at 80Mhz.
BS2.Pulsout(500) ' 1 mS pulse
}}
ClkCycles := (Duration * us * 2 - 1250) #> 400 ' duration * clk cycles for 2us
' - inst. time, min cntMin
dira[pin]~~ ' Set to output
!outa[pin] ' set to opposite state
waitcnt(clkcycles + cnt) ' wait until clk gets there
!outa[pin]

RubrChicken
03-22-2009, 01:06 PM
I'll attach the actual spin file for ease of use.