con cntMin = 400 'Minimum waitcnt value to prevent lock-up Max_uS = 2350 'longest valid servo pulse (in microseconds) Min_uS = 300 'shortest valid servo pulse (in microseconds) Precision = 5 'how small of a step the search uses Start_offset = 100 'how far to offset form the assumed position to start PP_wait_div = 200 '1 / PP_wait_div seconds between search pulses var 'these variables are unique to each instance of this object. long error_code 'stores the most recent status code of the servo long cog 'stores the cog number used, and weather it is still active long us pub Init {{initalise all the timing for this monster code. Init needs to be called at startup and anytime the clock speed changes}} us := clkfreq / 1_000_000 ' Clock cycles for 1 us Pub Pulseout_uS(Pin, Duration) | SyncCNT ''generate a pulse on Pin, Duration microseconds long. Duration := Min_uS #> Duration <# Max_uS 'limit Duration to valid values dira[pin]~~ ' Set to output SyncCNT := cnt + 1000 'snag value of cnt + some time waitcnt(SyncCNT) 'sync to cnt !outa[pin] 'set Pin to opposite state {do math while we wait for CNT to reach our target} SyncCNT += ((Duration * us) #> cntMin) 'modify sync point to a future time. duration*(clk cycles for us) waitcnt(SyncCNT) 'wait until clk gets there !outa[pin] 'return Pin to orig. state con Offset_up_err = 50 'offset error of up search method pub Find_pos_up(Pin,AssumedPos):RealPos | error {{search for the current position of the servo. assumes signal pin is Pin and mode pin is Pin+1}} dira[Pin+1]~ 'make the mode pin an input error := 0 'counter for how many time's search has happened RealPos := AssumedPos - Start_offset - Offset_up_err 'start the search just below AssumedPos {setup for main loop} Pulseout_uS(Pin,RealPos) 'pulse the servo waitcnt(clkfreq/PP_wait_div + cnt) 'wait for 5ms if ina[Pin+1] 'if servo's internal position matches the command RealPos += Offset_up_err 'adjust for offset error return 'return RealPos {main loop} repeat 'loop through the avalable positions RealPos += Precision 'increment tested position by 5us if RealPos => Max_uS 'check if the command pulse is too long RealPos := Min_uS 'start over at the min pulse length error++ Pulseout_uS(Pin,RealPos) 'pulse the servo waitcnt(clkfreq/PP_wait_div + cnt) 'wait for 5ms until ina[Pin+1] or (error > 3) 'I'm stuck in here till I match positions with the servo's internals or I give up waitcnt(clkfreq/50 +cnt) 'kill a glitch if error > 3 'did I give up? RealPos := -1 'something's not right. return an error code. RealPos += Offset_up_err 'adjust position for offset error dira[Pin+1]~~ 'we're done, make the mode pin an output con Offset_down_err = 25 'offset error of up search method pub Find_pos_down(Pin,AssumedPos):RealPos | error {{search for the current position of the servo. assumes signal pin is Pin and mode pin is Pin+1 for some reson this is more glitchy than the up search}} dira[Pin+1]~ 'make the mode pin an input error := 0 'counter for how many time's search has happened RealPos := AssumedPos + Start_offset - Offset_down_err 'start the search just abovve AssumedPos {setup for main loop} Pulseout_uS(Pin,RealPos) 'pulse the servo waitcnt(clkfreq/PP_wait_div + cnt) 'wait for 5ms if ina[Pin+1] 'if servo's internal position matches the command RealPos += Offset_down_err 'adjust for offset error return 'return RealPos {main loop} repeat 'loop through the avalable positions RealPos -= Precision 'decrement tested position by 5us if RealPos =< Min_uS 'check if the command pulse is too short RealPos := Max_uS 'start over at the max pulse length error++ Pulseout_uS(Pin,RealPos) 'pulse the servo waitcnt(clkfreq/PP_wait_div + cnt) 'wait for 5ms until ina[Pin+1] or (error > 3) 'I'm stuck in here till I match positions with the servo's internals or I give up waitcnt(clkfreq/50 +cnt) 'kill a glitch if error > 3 'did I give up? RealPos := -1 'something's not right. return an error code. RealPos += Offset_down_err 'adjust position for offset error dira[Pin+1]~~ 'we're done, make the mode pin an output