Basic working code!

We finally got some code to work and interface with the blue tooth through hyper-terminal. Check out the code below!
{{Truckbasic.spin}}
{{
11-25-08
Control E-MAXX truck equiped with 5 Ping))) sensors and a CMUcam2+
}}
Con
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
Th_Channel = 12 'servo control settings
St_Channel = 7
Rate = 1
MaxDist = 100 'ping range settings
NeutralDist = 40
MinDist = 1
MaxForward = 135 'speed controller settings
MinForward = 131
Neutral = 125
MinReverse = 122
MaxReverse = 115
ExtLeft = 190 'steering settings
ExtRight = 40
MaxSteer = 50
Center = (ExtLeft + ExtRight) / 2
SensLim = 100
SteeringProportionalGain = 1
ConfidenceThreshold = 75

OBJ
Servo : "PWM"
Serial_PC : "FullDuplexSerial"
Num : "Numbers"
Distance : "Ping"
VAR
Byte Throttle 'display values
Byte Steering
Byte Cam_angle
Word D0
Word D1
Word D2
Word D3
Word D4
Byte mx 'camera values
Byte my
Byte x1
Byte y1
Byte x2
Byte y2
Byte pixels
Byte confidence
Byte r1
Byte r2
Byte TargetAquired
Byte CrMean
Byte YMean
Byte CbMean
Byte CrDeviation
Byte YDeviation
Byte CbDeviation
PUB Main
BTSetup 'setup bluetooth connection
Throttle := Neutral
Steering := 115
Cam_angle := 125
r1 := 0
r2 := 0
repeat 'main loop
UserIn
UserOut
Servo.Send(Th_Channel, Rate, Throttle)
Servo.Send(St_Channel, Rate, Steering)
PUB BTSetup
Serial_PC.start(24, 25, 0, 9600) 'if bluetooth card is in 9600 mode change it to 115200
Serial_PC.str(string("set baud 115200 *", 13))
waitcnt(clkfreq/10 + cnt)
PUB UserIn | input
input := serial_PC.rx
Sample
if input == "w" 'forward
Throttle := Throttle + 3
elseif input == "s" 'reverse
Throttle := Throttle - 3
elseif input == "a" 'turn wheels left
Steering := Steering + 10
elseif input == "d" 'turn wheels right
Steering := Steering - 10
elseif input == " " 'all stop
Throttle := Neutral
Steering := 115
Cam_angle := 125
repeat
if serial_PC.rxcheck <> -1
quit
UserOut 'temp added for loop
Servo.Send(Th_Channel, Rate, Throttle)
Servo.Send(St_Channel, Rate, Steering)
if D4 < 5
quit
throttle := Neutral
Steering := 115
else
serial_PC.tx(13)
serial_PC.str(string("unknown"))
PUB UserOut
serial_PC.tx(13)
if Throttle
serial_PC.str(string("Throttle:"))
serial_PC.str(Num.ToStr(Throttle, Num#DDEC))
if Steering
serial_PC.str(string(" Steering:"))
serial_PC.str(Num.ToStr(Steering, Num#DDEC))
if D0
serial_PC.str(string(" D0"))
serial_PC.str(Num.ToStr(D0,Num#DDEC))
if D2
serial_PC.str(string(" D2"))
serial_PC.str(Num.ToStr(D2,Num#DDEC))
PUB Sample
D0 := Distance.Centimeters(0) 'only enable pins with a ping attached or code will hang
waitcnt(clkfreq/1000 + cnt)
D2 := Distance.Centimeters(2) 'only enable pins with a ping attached or code will hang
waitcnt(clkfreq/1000 + cnt)