Hey guys i just got this hacked together and it works pretty good ,still have to add corner escape and fine tune the the math ,i came from eeprom navigation and ir distance detect, NOTE i have 3 IR,s so u might have to edit out the Center IR
'MC IR ROAMING SABERTOOTH
'{$STAMP BS2}
'{$PBASIC 2.5}
' ----- EEPROM Data ]--------------------------------------------------------
'
' ----- Variables ]----------------------------------------------------------
counter VAR Byte ' FOR...NEXT loop counter
pulseCount VAR Byte ' Stores navigation distance
pulseLeft VAR Word ' Stores pulse to left servo
pulseLeftOld VAR Word ' Previous pulse to left servo
pulseRight VAR Word ' Stores pulse to right servo
pulseRightOld VAR Word ' Previous pulse to right servo
freqselect VAR Nib
irfrequency VAR Word
irdetectleft VAR Bit
irdetectcenter VAR Bit
irdetectright VAR Bit
distanceleft VAR Nib
distancecenter VAR Nib
distanceright VAR Nib
' ----- Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000 ' Beep to signify program start
pulseLeft = 750 ' Set all pulse values to center
pulseRight = 750
pulseLeftOld = 750
pulseRightOld = 750
' ----- Main Routine ]-------------------------------------------------------
DO ' Begin main routine
GOSUB get_distances
pulseleft = pulsecount + 5 - distanceright *4 - distancecenter *2
pulseright = pulsecount - 5 + distanceleft *4 + distancecenter *2
pulseLeft = pulseLeft MIN 650 MAX 800 ' Keep 650 <= durations <= 850
pulseRight = pulseRight MIN 700 MAX 850
' This FOR...NEXT loop includes a navigation routine with ramping.
FOR counter = 1 TO pulseCount ' Repeat for number of pulses
' Navigation routine sets pulse width based on navigation character.
' Increment/decrement routine only changes pulse durations by 10 at a time.
IF pulseLeft > (pulseLeftOld + 10) THEN pulseleft = pulseLeftOld + 10
IF pulseLeft < (pulseLeftOld - 10) THEN pulseLeft = pulseLeftOld - 10
IF pulseRight > (pulseRightOld + 10) THEN pulseRight = pulseRightOld + 10
IF pulseRight < (pulseRightOld - 10) THEN pulseRight = pulseRightOld - 10
pulseLeftOld = pulseLeft ' Remember old values
pulseRightOld = pulseRight
PULSOUT 13, pulseLeft ' Send control pulses to servos
PULSOUT 12, pulseRight
PAUSE 20
NEXT ' Check FOR counter... condition
LOOP ' Repeat main routine
get_distances:
distanceleft = 0
distancecenter = 0
distanceright = 0
FOR freqselect = 0 TO 4
LOOKUP freqselect, [37500,38250,39500,40500,41500], irfrequency
FREQOUT 8,1, irfrequency
irdetectleft = IN9
distanceleft = distanceleft + irdetectleft
FREQOUT 6,1, irfrequency
irdetectcenter = IN7
distancecenter = distancecenter + irdetectcenter
FREQOUT 2,1, irfrequency
irdetectright = IN0
distanceright = distanceright + irdetectright
NEXT
RETURN
have fun
