Hello, I’m still relatively new at the coding of the lynxmotion ps2 controller. My problem is that the code continously repeats all the “gosub” commands twice. Any ideas why or how to make it only do it once?
[code];-----------Bot Board Selection----------
;PS2 Controller / BotBoard II
DAT con P12
CMD con P13
SEL con P14
CLK con P15
;-------SSC-32----------
SERVO0PW VAR WORD
MOVETIME VAR WORD
S5 VAR WORD
S6 VAR WORD
S7 VAR WORD
S8 VAR WORD
S9 VAR WORD
S10 VAR WORD
TTM VAR WORD
bservol var word
bservor var word
fservol var word
fservor var word
bservol = 0
bservor = 0
fservol = 0
fservor = 0
SERVO0PW = 1500
MOVETIME = 300
TTM = 300
SEROUT P13,I38400, “#5P”,DEC SERVO0PW, “T”, DEC MOVETIME,13]
sound 9,[50\4400]
pause 50
sound 9,[100\3960]
pause 50
sound 9,[200\3400]
index var byte
temp var byte(18)
mode var byte
Small_Motor var byte
Large_Motor var byte
;PS2Init
high CLK
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high SEL
pause 1
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_LOCK
high SEL
pause 100
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high SEL
pause 1
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
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high SEL
pause 1
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_MODE_EXIT
high SEL
pause 1
main
;-----------PS2 Mode----------
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8]
shiftin DAT,CLK,FASTLSBPOST,[mode\8]
high SEL
;-----------------------------
pause 1
;-----------PS2 Data----------
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8]
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
;-----------------------------
;-----------PS2 Vibration motors----------
Small_Motor = 1 - Temp(1).bit5
Large_Motor = Temp(8)
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8,$0\8,Small_Motor\8,Large_Motor\8]
high SEL
;-----------Basic Micro IDE terminal----------
serout S_OUT,i57600,[13, hex2 mode\2]
for index = 1 to 18
serout S_OUT,i57600," ", dec3 temp(index)\3]
next
;-----------Proces Code--------------------
if temp(6) < 128 then
gosub walkforward
elseif temp(8) > 0
TTM = (TTM - 30)
elseif temp(7) > 0
TTM = (TTM + 30)
elseif temp(6) > 128
gosub walkbackward
elseif temp(5) < 128
gosub left_turn
elseif temp(5) > 128
gosub right_turn
elseif temp(11) > 1
gosub sit
elseif temp(12) > 1
gosub stand
elseif temp(1) = 247
gosub startbutton
elseif temp(4) > 128
fservol = fservol + 50
fservor = fservor - 50
gosub fservocontrol
elseif temp(4) < 128
fservol = fservol - 50
fservor = fservor + 50
gosub fservocontrol
elseif temp(3) > 128
bservol = bservol + 50
bservor = bservor - 50
gosub bservocontrol
elseif temp(3) < 128
bservol = bservol - 50
bservor = bservor + 50
gosub bservocontrol
endif
goto main
'----------------Subcommands----------------
walkforward:
'W1
S5= 1260 :
S6= 1200 :
S7= 1580 :
S8= 1500 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W2
S5= 1260 :
S6= 1080 :
S7= 1700 :
S8= 1380 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W3
S5= 1260 :
S6= 1190 :
S7= 1700 :
S8= 1620 :
S9= 1500 :
S10= 1740 :
GOSUB SEND_DATA
PAUSE TTM
'W4
S5= 1800 :
S6= 1680 :
S7= 1700 :
S8= 1620 :
S9= 1380 :
S10= 1860 :
GOSUB SEND_DATA
PAUSE TTM
'W5
S5= 1900 :
S6= 1680 :
S7= 1580 :
S8= 1500 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
return
walkbackward
'W1
S5= 1260 :
S6= 1200 :
S7= 1580 :
S8= 1500 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W2
S5= 1260 :
S6= 1080 :
S7= 1460 :
S8= 1620 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W3
S5= 1260 :
S6= 1190 :
S7= 1700 :
S8= 1240 :
S9= 1500 :
S10= 1260 :
GOSUB SEND_DATA
PAUSE TTM
'W4
S5= 1800 :
S6= 1680 :
S7= 1700 :
S8= 1380 :
S9= 1620 :
S10= 1140 :
GOSUB SEND_DATA
PAUSE TTM
'W5
S5= 1900 :
S6= 1680 :
S7= 1580 :
S8= 1500 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
return
sit:
S5=1510:
S6=1500:
S7=2100:
S8=1000:
S9=2170:
S10=900:
GOSUB SEND_DATA
PAUSE 150
return
stand:
S5=1510:
S6=1500:
S7=1580:
S8=1500:
S9=1500:
S10=1500:
GOSUB SEND_DATA
PAUSE 150
return
startbutton
sound 9, [10\1000]
sound 9, [10\2000]
sound 9, [10\3000]
sound 9, [10\4000]
sound 9, [10\5000]
pause 10
sound 9, [10\1000]
sound 9, [10\2000]
sound 9, [10\3000]
sound 9, [10\4000]
sound 9, [10\5000]
pause 10
return
right_turn
'W1
S5= 1260 :
S6= 1200 :
S7= 1580 :
S8= 1500 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W2
S5= 1260 :
S6= 1200 :
S7= 1700 :
S8= 1380 :
S9= 1500 :
S10= 1500 :
GOSUB SEND_DATA
PAUSE TTM
'W3
S5= 1260 :
S6= 800 :
S7= 1460 :
S8= 1380 :
S9= 1500 :
S10= 1260 :
GOSUB SEND_DATA
PAUSE TTM
'STAND
S5=1510:
S6=1500:
S7=1580:
S8=1380:
S9=1500:
S10=1260:
GOSUB SEND_DATA
PAUSE ttm
return
left_turn
sound 9, [50\9000]
pause 100
return
bservocontrol
S7=1580:
S8= 1500 :
S5= (1510 + bservor):
S6= (1500 + bservol):
GOSUB SEND_DATA
PAUSE ttm
return
fservocontrol
S9= (1500 + fservol)
S10= (1500 + fservor)
S7=1580:
S8=1500:
GOSUB SEND_DATA
PAUSE ttm
return
SEND_DATA:
SEROUT P11,I38400, “#5P”,DEC S5, “#6P”,DEC S6, “#7P”,DEC S7, “#8P”,DEC S8, “#9P”,DEC S9, “#10P”,DEC S10,“T”,DEC TTM,13]
RETURN
[/code]
Bane
