The subroutines I use to set the controller:
Forward:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 127 ] 'RIGHT
PAUSE 25
SEROUT MCserin, BaudRate, [ $80, 0, 5, 127 ] 'LEFT
RETURN
Brake:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 0 ] 'RIGHT
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 5, 0 ] 'LEFT
RETURN
Backward:
SEROUT MCserin, BaudRate, [ $80, 0, 6, 127 ] 'RIGHT
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 4, 127 ] 'LEFT
RETURN
LeftRotate:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 100 ] 'RIGHT
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 4, 100 ] 'LEFT
RETURN
RightRotate:
SEROUT MCserin, BaudRate, [ $80, 0, 6, 100 ] 'RIGHT
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 5, 100 ] 'LEFT
RETURN
And heres the full code, if anyone needs it. (some parts are in French)
' Robot Subroutines
' {$STAMP BS2p}
' {$PBASIC 2.5}
' ----------[ IDENTIFICATION PINS ]----------------------------------------------------------
FollowerSerin PIN 0
FollowerCLK PIN 1
LeftEncoder PIN 2
RightEncoder PIN 3
MCserin PIN 4
MCreset PIN 5
RightPing PIN 6
CenterPing PIN 7
LeftPing PIN 8
LCDSerin PIN 9
Piezo PIN 10
SW1 PIN 11
SW2 PIN 12
SW3 PIN 13
SW4 PIN 14
' ----------[ CONSTANTES ]-------------------------------------------------------------------
CmConstant CON 904 ' 2260 for BS2 ; 904 for BS2P
InConstant CON 356 ' 890 for BS2 ; 356 for BS2P
BaudRate CON 240 ' Baud Rate: 9600 = [ bs2 = 84 ] [ bs2p = 240 ]
' ----------[ VARIABLES ]--------------------------------------------------------------------
counter VAR Nib
' ----------[ INITIALISATION ]---------------------------------------------------------------
PAUSE 20
SEROUT LCDSerin, BaudRate, [ 22, 17, 12 ]
PAUSE 5
SEROUT LCDSerin, BaudRate, [ 148, "***INITIALISATION***" ]
GOSUB Brake
GOSUB StartSound
GOSUB ResetDisplay
' ----------[ ROUTINE PRINCIPALE ]-----------------------------------------------------------
DO
PAUSE 5
IF SW1 = 0 THEN
GOSUB ForwardTenSeconds
ELSEIF SW2 = 0 THEN
GOSUB BackwardFiveSeconds
ELSEIF SW3 = 0 THEN
GOSUB LeftTurn
ELSEIF SW4 = 0 THEN
GOSUB RightTurn
ENDIF
LOOP
' ----------[ SOUS-ROUTINES NAVIGATION ]-----------------------------------------------------
ForwardTenSeconds:
SEROUT LCDSerin, BaudRate, [ 168, "Marche avant", 188, " Durée: "]
GOSUB Forward
FOR counter = 10 TO 1
SEROUT LCDSerin, BaudRate, [ 197, DEC counter, " " ]
PAUSE 1000
NEXT
GOSUB Brake
GOSUB ResetMC
GOSUB ResetDisplay
GOSUB EndSound
RETURN
BackwardFiveSeconds:
SEROUT LCDSerin, BaudRate, [ 168, "Marche arriere", 188, " Durée: "]
GOSUB Backward
FOR counter = 5 TO 1
SEROUT LCDSerin, BaudRate, [ 197, DEC counter ]
PAUSE 1000
NEXT
GOSUB Brake
GOSUB ResetMC
GOSUB ResetDisplay
GOSUB EndSound
RETURN
LeftTurn:
SEROUT LCDSerin, BaudRate, [ 168, "Rotation gauche", 188, " Durée: "]
GOSUB LeftRotate
FOR counter = 2 TO 1
SEROUT LCDSerin, BaudRate, [ 197, DEC counter ]
PAUSE 1000
NEXT
GOSUB Brake
GOSUB ResetMC
GOSUB ResetDisplay
GOSUB EndSound
RETURN
RightTurn:
SEROUT LCDSerin, BaudRate, [ 168, "Rotation droite", 188, " Durée: "]
GOSUB RightRotate
FOR counter = 2 TO 1
SEROUT LCDSerin, BaudRate, [ 197, DEC counter ]
PAUSE 1000
NEXT
GOSUB Brake
GOSUB ResetMC
GOSUB ResetDisplay
GOSUB EndSound
RETURN
' ----------[ SOUS-ROUTINES INTERFACE ]------------------------------------------------------
' ----------[ SOUS-ROUTINES MOTEURS ]--------------------------------------------------------
' Toujours [ $80, 0 puisque 1. start bit et 2. device type
' Pour avoir la valeur du bit 3, multiplier le numero du moteur par 2 et ajouter 1
' si la direction est avant. ( moteur no. 2 et 3).
Forward:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 127 ] 'DROITE
PAUSE 25
SEROUT MCserin, BaudRate, [ $80, 0, 5, 127 ] 'GAUCHE
RETURN
Brake:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 0 ] 'DROITE
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 5, 0 ] 'GAUCHE
RETURN
Backward:
SEROUT MCserin, BaudRate, [ $80, 0, 6, 127 ] 'DROITE
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 4, 127 ] 'GAUCHE
RETURN
LeftRotate:
SEROUT MCserin, BaudRate, [ $80, 0, 7, 100 ] 'DROITE
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 4, 100 ] 'GAUCHE
RETURN
RightRotate:
SEROUT MCserin, BaudRate, [ $80, 0, 6, 100 ] 'DROITE
PAUSE 2
SEROUT MCserin, BaudRate, [ $80, 0, 5, 100 ] 'GAUCHE
RETURN
' ----------[ SOUS-ROUTINES CONSTANTES ]-----------------------------------------------------
ResetDisplay:
SEROUT LCDSerin, BaudRate, [ 12 ]
PAUSE 10
SEROUT LCDSerin, BaudRate, [ 128, " |Test des moteurs", 148, " |_________________", 146, "|D", 166, "|G", 186, "|R", 206, "|A" ]
RETURN
ResetMC:
LOW MCReset
PAUSE 2
HIGH MCReset
RETURN
StartSound:
PAUSE 100
FREQOUT piezo, 566, 796
FREQOUT piezo, 566, 875
FREQOUT piezo, 566, 955
FREQOUT piezo, 566, 849
FREQOUT piezo, 472, 1000
PAUSE 200
RETURN
EndSound:
PAUSE 250
FREQOUT piezo, 566, 1000
FREQOUT piezo, 566, 1000
FREQOUT piezo, 566, 1000
RETURN