My GCB robot

[stan cartwright]

I didn't have the object avoiding abilities of my robot programmed with GCB. It wasn't big or clever but thought I'd share. It's so easy to tweak what it does when it senses an object.
The only features of the code are hpwm for motor speed and the servo command which is 50hz interrupt and is turned off when using the hso4 ultrasonic then back on. Incase you want to make one.
https://youtu.be/VDr_0NmGu84
I zipped the code but can't find the attatch file option so sorry long code post.

    ;us on servo robot
    #chip 18F25k22,16
    #option explicit
    ;#include <glcd.h>
    #include <srf04.h>
    #config osc=int,WDT=OFF
    ;
    #define servo_pin portb.0 ; Servo signal On PIN 21
    #define lmotordira portb.2 ;h-bridge motor pins
    #define lmotordirb portb.1
    #define rmotordira portb.3
    #define rmotordirb portb.4
    #define US1Ping PORTb.5
    #define US1Echo PORTb.5
    ;
    dir lmotordira out : dir lmotordirb out : dir rmotordira out : dir rmotordirb out : dir servo_pin out
    dir portc.2 out ;pwm out pin
    ;
    dim range as word
    dim rturncount as byte : rturncount=0
    dim lturncount as byte : lturncount=0
    dim last_turn as Byte : last_turn=0
    dim tmp as byte
    dim saferange as word : saferange = 40
    dim radardir as byte : radardir = 1
    dim servoposition as byte : servoposition = 150 ;---- point radar ahead
    ;
    ;set up servo interrupt 50Hz
    on interrupt timer1overflow Call servo
    inittimer1(OSC,PS1_2)
    Starttimer 1
    ;
    wait 50 ms
    ;----------------------- START ------------------
    main:
      HPWM 1, 1, 224
      motors_forward
      do
        get_range
        rotate_radar
        ;
        ;---- set saferange for different directions
        if servoposition > 130 and servoposition < 170 then
            saferange = 40 ; front
        else
            saferange = 46 ; left and right
        end if
        ;
      loop until range < saferange;--- test for obstacles
      ;
      ;object detected
      motors_stop
      reverse
      if servoposition >= 160 then ;--- object left
        turnright
        goto main
      end if
    ;
      if servoposition <= 140 then ;--- object right
        turnleft
        goto main
      end if
      ;ahead blocked
      ;---- object ahead so random test turn left or right
      tmp = Random
      if tmp < 128 then
        servoposition = 130 ;point radar far right
        wait 22 ms
        get_range
        if range > saferange then ;---- right is clear
          turnright
        else
          turnleft
        end if
        goto main
      else
        servoposition = 170 ;---- point radar far left
        wait 22 ms
        get_range
        if range > saferange then ;---- left is clear
          turnleft
        else
          turnright
        end if
        goto main
      end if
    ;
    goto main
    ;---------- end of main program
    ;................... Subroutines ...................................
    sub turnright
    tmp=servoposition+30
    do until (servoposition=tmp) or (servoposition > 210)
      do : scanrotateright : loop until range > saferange ;---- spin clockwise until no object ahead
      servoposition=servoposition + 10
      wait 22 ms
    loop
    radardir = 1 : servoposition = 150 ;---- point radar ahead
    wait 22 ms
    reverse
    end sub
    ;......................................................
    sub turnleft
    tmp=servoposition-30
    do until (servoposition=tmp) or (servoposition < 90)
    do : scanrotateleft : loop until range > saferange ; ---- spin anti-clockwise until no object ahead
      servoposition=servoposition - 10
      wait 22 ms
    loop
    reverse
    ;
    radardir=0 : servoposition=150 ;---- point radar ahead
    wait 22 ms
    end sub
    sub scanrotateleft
    get_range
    HPWM 1, 1, 255
    rotate_anticlockwise
    wait 14 ms
    motors_stop
    motors_off
    end sub
    sub scanrotateright
    get_range
    HPWM 1, 1, 255
    rotate_clockwise
    wait 14 ms
    motors_stop
    motors_off
    end sub
    sub reverse
    HPWM 1, 1, 255
    motors_reverse
    wait 30 ms
    motors_stop
    motors_off
    end sub
    sub rotate_anticlockwise
    set lmotordira off; left motor forward
    set lmotordirb on
    set rmotordira on ; right motor backward
    set rmotordirb off
    end sub
    sub rotate_clockwise
    set lmotordira on; left motor backward
    set lmotordirb off
    set rmotordira off ; right motor forward
    set rmotordirb on
    end sub
    sub motors_forward
    set lmotordira on ; left motor forward
    set lmotordirb off
    set rmotordira on ; right motor forward
    set rmotordirb off
    end sub
    sub motors_off
    set lmotordira off; left motor off
    set lmotordirb off
    set rmotordira off ; right motor off
    set rmotordirb off
    end sub
    sub motors_reverse
    set lmotordira off ; left motor backward
    set lmotordirb on
    set rmotordira off ; right motor backward
    set rmotordirb on
    end sub
    sub motors_stop
    set lmotordira on ; short left motor
    set lmotordirb on
    set rmotordira on ; short right motor
    set rmotordirb on
    wait 5 ms
    end sub
    sub rotate_radar
    ;
    if radardir = 1 then
      servoposition = servoposition + 10
      if servoposition = 210 then
        radardir = 0
      end if
    else
      servoposition = servoposition - 10
      if servoposition = 90 then
        radardir = 1
      end if
    end if
    wait 20 ms
    end sub
    sub get_range ;us sensor
    range=0
    do until range > 0
    intoff
    range = USDistance(1)
    inton
    wait 10 ms
    loop
    end sub
    ;;;;;;;;
    sub servo ;servo interrupt
    settimer 1, 25535
    pulseout Servo_Pin , servoposition 10us
    end sub

formatting disappeared.

[stan cartwright]

A few code twiddles and it got better but no where finished yet. leds later,more fun. https://youtu.be/Bs6rR2-KpAE