Code review: Mobile robot

Dgswaner

This is my first real SPIN project. can I get a review from the experts (or anyone)? I'm looking for advice on how to make things simpler, better and faster. as well as any other type of advice related to programing.

CON
    _clkmode = xtal1 + pll16x                           
    _xinfreq = 5_000_000                                'Note Clock SPEED for your setup!!

        PINGServo = 0                                     ' PING pan servo
        PING_Pin  = 2                                     ' I/O Pin For PING)))
        propTX    = 30                                    ' programming output
        propRX    = 31                                    ' programming input
        CLEARDIST = 18                                    ' bot will go forward until < cleardist
        TURNDIST  = 45                                    ' bot will turn until > then turndist
VAR
    LONG  RANGE                                           ' PING sensor DISTANCE
    LONG  OLDRANGE        
    LONG  DIRECTION[noparse][[/noparse]6]     
    LONG  DISTANCE[noparse][[/noparse]6]
    BYTE  HLIGHT                                          ' headlights  1=off 0=on
    BYTE  DEBUGSEMID
    LONG  INDEX                                           ' lock to be used by debug
    LONG  SPEED
    BYTE  CLEAR
OBJ
    SERVO  : "Servo32v3"                                  ' servo vontrol
    ping   : "ping"                                       ' PING control
    go     : "motor control"                              ' HB25 controller
    Sound  : "sound"                                      ' sounds for bot
    debug  : "MultiCogSerialDebug"                        ' debug serial driver  
PUB init | TEMP
    go.start
    dira[noparse][[/noparse]23]~~                                            'set PIN 23 for output, for headlights
    HLIGHT := 1                                           'turn off Headlights
    Headlight                   
    sound.ready
    SERVO.Start 
    repeat temp from 0 to 1                               'Preset ALL servo's to center position
        SERVO.Set(temp,1350)
    debug.start(propRX,propTX,0,9600,DEBUGSEMID)
    SPEED := -2000
    MAIN
PUB MAIN 
    CLEAR :="n"    'assume it's not CLEAR to go
    repeat 
       PING_AROUND
       SET_SPEED
       if DISTANCE + DISTANCE < DISTANCE + DISTANCE 
          CLEAR := "r"
       if DISTANCE + DISTANCE > DISTANCE + DISTANCE 
          CLEAR := "l"
       if DISTANCE > 15 and DISTANCE  > 15 and DISTANCE > 15 and DISTANCE > 15
          CLEAR := "y"
     case CLEAR 
        "y" :  IS_CLEAR
        "r" :  TRTC
        "l" :  TLTC
PUB IS_CLEAR                                                      ' all clear go forward
    SOUND.BEEPS(1)                     
    GET_DISTANCE
    SET_SPEED
    GO.FORWARD_SPEED(SPEED)
    repeat while RANGE > CLEARDIST
       GET_DISTANCE
    go.stop
    CLEAR := "n"          
    MAIN  
PUB TLTC                                                          ' Turn Left 'Til CLEAR
    SOUND.BEEPS(2)
RANGE := 0
    SERVO.Set(PINGServo,900)
    waitcnt (10_000_000+cnt)
    REPEAT WHILE RANGE < TURNDIST
       GET_DISTANCE
       go.left
    GO.STOP
    MAIN
    SERVO.Set(PINGServo,1350) 
PUB TRTC                                                          ' Turn Right 'Til CLEAR  
    SOUND.BEEPS(3)
    RANGE := 0
    SERVO.Set(PINGServo,1700)
    waitcnt (10_000_000+cnt)
    REPEAT WHILE RANGE < TURNDIST
       GET_DISTANCE
       go.RIGHT
    GO.STOP 
    MAIN
    SERVO.Set(PINGServo,1350)
PUB PING_AROUND| TEMP                                              'Pan PING around to 4 spots then center
    INDEX:= 1
    temp := 0                                       
    oldRANGE := 0
    repeat INDEX FROM 1 TO 5                                     
       TEMP := LOOKUP(INDEX: 1700, 1600, 1100, 900, 1350)
       SERVO.Set(PINGServo,TEMP)  
       waitcnt (30_000_000+cnt)                                     'wait for servo to get to it's position
       GET_DISTANCE
       DIRECTION[noparse][[/noparse]INDEX] := temp
       DISTANCE[noparse][[/noparse]INDEX] := RANGE
    if RANGE > OLDRANGE                 
          DIRECTION := Temp
          oldRANGE := RANGE
PUB GET_DISTANCE 
    RANGE := ping.Inches(PING_Pin)
    waitcnt(clkfreq / 10 + cnt)
PUB HEADLIGHT
   case HLIGHT
     0:   outa[noparse][[/noparse]23]~~    'turn on headlights
     1:   outa[noparse][[/noparse]23]~     'turn off headlights
PUB SET_SPEED | A
     repeat A from 1 to 5
        if DISTANCE[noparse][[/noparse]A] > TURNDIST
            SPEED := -2000
        else
            SPEED := -1700

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
"A complex design is the sign of an inferior designer." - Jamie Hyneman, Myth Buster

DGSwaner

Dgswaner

Sweet Glad I nailed it the first time!!!

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
"A complex design is the sign of an inferior designer." - Jamie Hyneman, Myth Buster

DGSwaner