sure, but from everything I’ve seen the sabertooth doesn’t use standard servo pulses, or that the timing is different when using pulseout? The provided sample code shows 3000 as centered, ~2000 as full forward and ~4000 as full reverse, and this seems the same on my set up. So 3500 should be slightly reversed. The code works, I’m running it around nicely right now. It just takes a few resets to start up most of the time. This is a fairly standard PS2 control set up, using one of the push buttons for “turbo mode” to speed it up, and writing all sorts of junk to a serial LCD.
'PS2 Controller / BotBoard II
DAT con P12
CMD con P13
SEL con P14
CLK con P15
index var byte
temp var byte(18)
mode var byte
rhori var word
rvert var word
lhori var word
lvert var word
lmotor var sword
rmotor var sword
boost var byte
circlelast var bit
squarelast var bit
displaybutton var word
displayvalue var word
'---------------------------------------------------------------------
high CLK
test
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;Config Enable
high SEL
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;Set Mode and Fix(Analog
high SEL
pause 100
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4D\8,$00\8,$00\8,$01\8,$ff\8,$ff\8,$ff\8,$ff\8] ;Vibration Enable
high SEL
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;Enable Pressure
high SEL
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;Press_trans_start
high SEL
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;Config Exit
high SEL
main
' This section find the mode the PSX controller is in.
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8]
shiftin DAT,CLK,FASTLSBPOST,[mode\8]
high SEL
low SEL
' asking data to PS2 controller
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8]
' reading data from controller
shiftin DAT,CLK,FASTLSBPOST,[temp(0)\8,temp(1)\8,temp(2)\8,temp(3)\8,temp(4)\8,temp(5)\8,temp(6)\8,temp(7)\8,temp(8)\8, |
temp(9)\8,temp(10)\8,temp(11)\8,temp(12)\8,temp(13)\8,temp(14)\8,temp(15)\8,temp(16)\8,temp(17)\8,temp(18)\8]
high SEL
'---------------------------------------------------------------------
'temp 3-6 are joysticks
'temp 7-10 are d-pad buttons
'11=triangle 12=circle 13=X 14=square
'15=L1 16=r1 17=L2 18=R2 255 is full on, 0 is off
' this section takes the status of circle, if on, double the speed
if (temp(12) >10) and circlelast = 0 then
if (boost = 1) then
boost = 2
elseif (boost = 2 or boost = 0)
boost = 1
endif
circlelast = 1
elseif temp(12) = 0
circlelast = 0
endif
'---------------------------------------------------------------------
' this section will control servo position using L & R buttons -interrupts?
'---------------------------------------------------------------------
'prep the inputs to be displayed to screen. this is debugging more than anything
if temp(11)>0 then
displaybutton = "^" 'triangle
displayvalue = temp(11)
elseif temp(12)>0
displaybutton = "O" 'circle
displayvalue = temp(12)
elseif temp(13)>0
displaybutton = "X" 'x
displayvalue = temp(13)
elseif temp(14)>0
displaybutton = "=" 'Square
displayvalue = temp(14)
else
displaybutton = " " 'blank
displayvalue = 0
endif
'---------------------------------------------------------------------
' this section does the mixing for throttle steering on a diff steered vehicle.
rhori = (temp(3))
rvert = (temp(4))
lhori = (temp(5))
lvert = (temp(6))
if lvert < 80 then
lvert = 0
serout 4,i9600,[254] 'send command
serout 4,i9600,[128] 'send position to move to
serout 4,i9600,"l: forward ", displaybutton] 'write the text
elseif lvert > 200
lvert = 255
serout 4,i9600,[254]
serout 4,i9600,[128]
serout 4,i9600,"l: reverse ", displaybutton]
else
lvert = 128
serout 4,i9600,[254]
serout 4,i9600,[128]
serout 4,i9600,"l: stopped ", displaybutton]
endif
if rvert < 80 then
rvert = 0
serout 4,i9600,[254]
serout 4,i9600,[192]
serout 4,i9600,"r: forward ", dec3 displayvalue\3]
elseif rvert > 200
rvert = 255
serout 4,i9600,[254]
serout 4,i9600,[192]
serout 4,i9600,"r: reverse ", dec3 boost\3]
else
rvert = 128
serout 4,i9600,[254]
serout 4,i9600,[192]
serout 4,i9600,"r: stopped ", dec3 lmotor\3]
endif
lmotor = 3000 - ((4 * boost) * (128-lvert)) ;converts 0 to 2000, 128 to 3000, 255 to 4000
rmotor = 3000 - ((4 * boost) * (128-rvert))
pulsout 0, lmotor
pulsout 1, rmotor
'---------------------------------------------------------------------
goto main