4wheel.bsx

M:\Mike (4-19)\Stub Programs\Stamp Board Code - printer\4wheel.bsx
File saved: 4/23/2006 11:09:58 PM
Generated: 4/24/2006 6:53:42 PM

Table of Contents
Declarations


'Author: Michael Konicki  Table of Contents
'Date: 3/30/2005
'
' {$STAMP BS2sx}
' {$PBASIC 2.5}
'
'even servos = wheels
'odd servos = directionary servos


'Variables
position VAR Word
ra VAR Byte
ch VAR Byte
header VAR Word
command VAR Word
tail VAR Word


'Serial Communication settings for Servo Controller
Sdat CON 15 ' TPIN is pin to transmit through
baud CON 1021  'BAUD RATE 1021

'NOTE: 250 to 1250 is servo range

start:

GOSUB stop_machine

DO
    
    'Get the command
    GOSUB get_input_from_com

LOOP

END

center_machine:
'*********************************************
' Center all the servos, maintain speed.
'*********************************************
'DEBUG "center machine", CR
'Set Direction to straight
ch = 0
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 650
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 4
position = 700
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 625
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN


forward_machine:
'*********************************************
' Center all servos and set the robot moving
' forward
'*********************************************
'DEBUG "forward machine", CR
'Start the wheels moving froward
ch = 1
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 3
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 5
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 7
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN


reverse_machine:
'*********************************************
' Center all servos and set the robot moving
' backwards
'*********************************************
'DEBUG "reverse machine", CR
'Start the wheels moving backwards
ch = 1
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 3
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 5
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 7
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN



left_2wheel_machine:
'*********************************************
' Center the two backwheels and move the front
' two so they turn left. This function does
' not apply speed, as it should already be
' moving with this type of turn command
'*********************************************
'DEBUG "left 2wheel machine", CR
'Turn forward wheels left
ch = 0
position = 650
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'position the back wheels to driving forward, push robot left
ch = 4
position = 700
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 625
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN


right_2wheel_machine:
'*********************************************
' Center the two backwheels and move the front
' two so they turn right. This function does
' not apply speed, as it should already be
' moving with this type of turn command
'*********************************************
'DEBUG "right 2wheel machine", CR
'Turn forward wheels right
ch = 0
position = 900
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 800
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'position the back wheels to driving forward, push robot right
ch = 4
position = 700
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 625
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN


left_4wheel_machine:
'*********************************************
' Sping the robot in a 360 left. all 4 wheels
' will turn and drive the robot in a stationary
' 360 manuver
'*********************************************
'DEBUG "left 4wheel machine", CR
'Turn forward wheels left
ch = 0
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 800
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 4
position = 550
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 825
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'Make the wheels all push the robot in the same circular motion
ch = 1
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 3
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 5
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 7
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN




right_4wheel_machine:
'*********************************************
' Sping the robot in a 360 right. all 4 wheels
' will turn and drive the robot in a stationary
' 360 manuver
'*********************************************
'DEBUG "right 4 wheel machine", CR
'Turn forward wheels right
ch = 0
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 800
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 4
position = 550
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 825
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'Make the wheels all push the robot in the same circular motion
ch = 1
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 3
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 5
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 7
position = 1000
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN



stop_machine:
'*********************************************
' Center all the servos to the stop position.
'*********************************************
'DEBUG "stoping machine", CR
'Set Wheel Speed To STOp
ch = 7
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 5
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 1
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 3
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'Set Direction to straight
ch = 0
position = 750
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 650
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 4
position = 700
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 625
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN

left_mid_machine:
'*********************************************
'Put the front wheels in standard left turn
'but also use the back wheels to push making a
'semi 360
'*********************************************
ch = 0
position = 650
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 500
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'Move the backwheels in the other direction
ch = 4
position = 875
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 825
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN


right_mid_machine:
'*********************************************
'Put the front wheels in standard right turn
'but also use the back wheels to push making a
'semi 360
'*********************************************
ch = 0
position = 900
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 2
position = 800
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
'Move the backwheels in the other direction
ch = 4
position = 550
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
ch = 6
position = 450
ra = 1
SEROUT Sdat, baud,["!SC", ch, ra, position.LOWBYTE, position.HIGHBYTE, CR]
RETURN




get_input_from_com:
'*********************************************
'Get the input from COM Port via PC and send it to the servo
'*********************************************
DEBUGIN DEC header, DEC command, DEC position, DEC tail

IF header = 11 THEN
    'We have a movement packet so get the rest of the data
    'DEBUGIN DEC command, DEC tail
    'Check to make sure the tail is correct before moving forward
    IF tail = 22 THEN
        SELECT command
        CASE = 0
            GOSUB stop_machine
            DEBUG "ACK:",CR
        CASE = 1
            GOSUB forward_machine
            DEBUG "ACK:",CR
        CASE = 2
            GOSUB center_machine
            DEBUG "ACK:",CR
        CASE = 3
            GOSUB reverse_machine
            DEBUG "ACK:",CR
        CASE = 4
            GOSUB left_2wheel_machine
            DEBUG "ACK:",CR
        CASE = 5
            GOSUB right_2wheel_machine
            DEBUG "ACK:",CR
        CASE = 6
            GOSUB left_4wheel_machine
            DEBUG "ACK:",CR
        CASE = 7
            GOSUB right_4wheel_machine
            DEBUG "ACK:",CR
        CASE = 8
            GOSUB left_mid_machine
            DEBUG "ACK:",CR
        CASE = 9
            GOSUB right_mid_machine
            DEBUG "ACK:",CR
        CASE ELSE
            DEBUG "NCK:",CR
            ENDSELECT
        ELSE
            DEBUG "NCK:",CR
        ENDIF
    ELSEIF header = 33 THEN
        'we have a servo movement packet so process it accordingly
        IF tail = 44 THEN
            SEROUT Sdat, baud,["!SC", command, 1, position.LOWBYTE, position.HIGHBYTE, CR]
            DEBUG "ACK:",CR
        ELSE
            DEBUG "NCK:",CR
        ENDIF
    ELSE
        DEBUG "NCK:",CR
    ENDIF
    
    RETURN