' NICOTHON.BSE - Nico-6 at Robothon Control Program
' (revised: 10/05/06).
' Oricom Technologies (c)2006 - www.oricomtech.com
' This file may be used for any purpose.

' Functions:
'  - Controls Nico-6 for Robothon walking contest.
'  - BS2e on BOTCOP pcb controls sensor reading + decision-making.
'  - WMC20 Walking Machine Controller controls gait generation.
'  - BS2e and WMC connected via local RS232 buss at 38400.
'  - Servo-Dog Led displays show mode, battery voltages,
'      sonar readings, and gait# selected.

' **** BOT-COP Hookup ****
'   BS2-P11 = S-Dog left ("J.9", mode display)
'   BS2-P12 =   "   right ("99", value display)
'   BS2-P13 = Ping-sonar 1 (frontal)
'   BS2-P14 =   "    "   2 (left-rear)
'   BS2-P15 =   "    "   3 (right-rear)

' **** WMC Hookup ****
'   WMC-A3 A/D 3 = right-side CdS
'   WMC-E0 A/D 5 = logic battery, 2:1 voltage divider
'   WMC-E1 A/D 6 = servo battery,  "     "       "
'   WMC-RB0..7 = servo# 0-7 - legs
'   WMC-RD0..3 =   "    8-11 - legs
'   WMC-RD6,7 = servo# 14=pan, 15=tilt

' Configuration - on BOTCOP pcb:
'   1. set jumpers J1+J2 = left-hand side to connect BS2 to DB09 conn.
'   2. set jumper J5 = left-hand side to connect BS2 to PZ sound transducer.
'   3. uses BOTCOP to WMC local RS232 buss.

' {$STAMP BS2e}
' {$PBASIC 2.5}
' {$PORT COM1}

#SELECT $STAMP
  #CASE BS2, BS2E
    T9600       CON     84              ' 54h = (0101'0100)
    T38400      CON      6
    TRIGGER     CON      5
    SCALE       CON   $200
#ENDSELECT


' **** Constants ****
RAWTOIN   CON   889            ' inch scaling
RAWTOCM   CON  2257            ' cm scaling

' *** servo pulse adjustment for WMC/MSCC
SDG_ADJ   CON    0             ' 0 for SDog on BS2, -6 on WMC

' **** S-Dog J9 mode offsets ****
SDOG_A    CON   1000+5+SDG_ADJ       ' start = "A.0"
SDOG_B    CON   1000+100+5+SDG_ADJ   ' start = "B.0"
SDOG_C    CON   1000+200+5+SDG_ADJ   ' start = "C.0"
SDOG_D    CON   1000+300+5+SDG_ADJ   ' start = "D.0"
SDOG_E    CON   1000+400+5+SDG_ADJ   ' start = "E.0"
SDOG_F    CON   1000+500+5+SDG_ADJ   ' start = "F.0"
SDOG_G    CON   1000+600+5+SDG_ADJ   ' start = "G.0"

SDOG_0    CON   1000+5+SDG_ADJ       ' display "00" in "99" mode

' *** RS232 parameters ***
PGM   CON    16           ' BS2 module main serial out pin
RX2   CON     5           ' local RS232 buss BS2 receive pin
TX2   CON     4           ' local RS232 buss BS2 transmit pin
' BAUD2   CON  T9600          ' local buss, non-inverted
BAUD2   CON  T38400
TIMO2   CON      20           ' 20-msec timeout on local RS232 buss

' **** BS2 Pin Defines ****
Spkr    PIN     8             ' output pin for FREQOUT
SDOG1   CON    11             ' S-dog left
SDOG2   CON    12             '   "   right
PING1   CON    13             ' frontal Ping
PING2   CON    14             ' left-side Ping
PING3   CON    15             ' right-side Ping

SW1     PIN     6           ' switch 1 pin
' SW2     PIN     7           ' switch 2 pin

' **** Play Mary tones ****
FSCL  CON 2
C     CON FSCL*2092/2         ' C note
D     CON FSCL*2348/2         ' D note
E     CON FSCL*2636/2         ' E note
G     CON FSCL*3136/2         ' G note
R     CON 8                   ' Silent pause (rest).

' **** Variables ****
x         VAR   Word            ' general timing index, 50-msec ticks

wtemp1    VAR   Word           ' temp use var word
wtemp2    VAR   Word           '  "    "   "   "
btemp3    VAR   Byte           '  "    "   "  byte

inch_l    VAR   Word           ' left-side distance
inch_r    VAR   Word           ' right-side distance
mode      VAR   Byte           ' switch mode var
vflag     VAR   Byte           ' general flag

' *** Parameters for differential testing ***
DEADB_W   CON      1            ' cm - sonar deadband near-wall
DEADB_H   CON      1            ' inches - sonar deadband going home
TOOFAR    CON     24            ' too far = sonar not turned to wall

diffs     VAR      Byte         ' var to store sonar-difference flag
DIFFEQ    CON      0            '  -- sonar 2+3 differential < deadband
LEFTGT    CON      1            '  -- left-sonar dist > right
RIGHTGT   CON      2            '  -- right-  "   "   >  left
OUTRNG    CON      3            '  -- sonar out of range.

' **** Parameters for Robothon ***

' advanceto_wall FSM - 50-msec ticks
' ADV_TIME   CON    30*20         ' 30-sec = max time to advance on wall
INNER     CON      7            ' min distance to wall before turning

' turn_home FSM - 10-msec ticks
' WTURN_TIME CON     8*20         '  8-sec = max turn time at wall

' DIFFENA   CON     10    ' inches - distance to wall for sonar2 before
                          '  -- enabling differential testing
DIFFENA   CON     35      ' 35cm = 14" - use for cm-scaling in turn

TURNEXT   CON     100           ' wall turn-time extension

' go_home FSM - 100-msec ticks
HOME_TIME  CON    60*10         ' 60-sec = max time to return home + shutdown
OUTER     CON     80            ' max distance from wall before done


' *****************************************
'       Main program starts here
' *****************************************
Main:
  ' give WMC time to bootup
  PAUSE 1000
  GOSUB wmc_setup
  INPUT SW1
'   INPUT SW2
  PAUSE 6000
  mode = 0

' all mode changes return to here
looptop:
  vflag = 0
  IF (mode=1) THEN mode1
  IF (mode=2) THEN mode2
  IF (mode=3) THEN mode3
  GOTO mode0

' **** Mode 2 - Compete ****
mode2:
  GOSUB disp_mode
  GOSUB start_servos

' go to wall
advanceto_wall:
  vflag=1
  GOSUB fast_forward
  x=0
  DO
    IF (mode <> 2) THEN looptop

    ' get distance to wall, display, turn home when close enough.
    GOSUB sig_sonar1
    GOSUB get_sonar1
    wtemp2 = wtemp2 ** RAWTOIN
    wtemp1 = wtemp2
    IF (wtemp2 <= INNER) THEN turn_home   ' min distance to wall
    GOSUB disp_dist

    IF (x=20) OR (x=40) OR (x=60) THEN
      GOSUB disp_volt2
      PAUSE 1000
    ENDIF

    x=x+1
'     IF (x >= ADV_TIME) THEN turn_home      ' time to turn home
    PAUSE 50
    GOSUB test_switch
  LOOP

' turn back towards start
' - this part uses 10-msec ticks
turn_home:
  vflag=1
  GOSUB turn_right
  x=0
  DO
    IF (mode <> 2) THEN looptop

    ' take action based upon left-right sonar difference
    IF (x > 400) THEN          ' start at 4-sec
      GOSUB read_sonar34
      IF (inch_l <= DIFFENA) THEN
        IF (inch_l >= inch_r) THEN
          PAUSE TURNEXT            ' fudge factor to extend turn-time
          GOTO go_home
        ENDIF
      ENDIF
    ELSE
      PAUSE 10
    ENDIF

'    IF (x >= WTURN_TIME) THEN go_home     ' max turn time
    x=x+1
    GOSUB test_switch
  LOOP

go_home:
  vflag=0                      ' execute "immediate" gait change
  GOSUB fast_forward
  x=0
  DO
    IF (mode <> 2) THEN looptop

    ' check after 4-sec
    IF (x > 80) THEN
      GOSUB read_sonar23
      GOSUB chk_differ2
      IF (diffs=DIFFEQ) THEN go_home2
      IF (diffs=RIGHTGT) THEN adj_right
      GOTO adj_left
    ENDIF

    x=x+1
    PAUSE 50
    GOSUB test_switch
  LOOP

adj_left:
  vflag=1
  GOSUB half_left
  DO
    ' take action based upon left-right sonar difference
    GOSUB read_sonar34
    IF (inch_r >= inch_l) THEN go_home2:
  LOOP

adj_right:
  vflag=1
  GOSUB half_right
  DO
    ' take action based upon left-right sonar difference
    GOSUB read_sonar34
    IF (inch_l >= inch_r) THEN go_home2:
  LOOP

' go back to start/finish line
go_home2:
  vflag=1                      ' execute "immediate" gait change
  GOSUB fast_forward
  x=0
  DO
    IF (mode <> 2) THEN looptop

    ' take action based upon left-right sonar difference
    IF (x > 80) THEN GOSUB read_sonar23       ' after 3-sec

    ' adjust attitude every 4-sec: 4,8,12,16,20
    IF (x=40) OR (x=80) OR (x=120) OR (x=160) OR (x=200) THEN
      GOSUB read_sonar23
      GOSUB chk_differ2
      IF (diffs=LEFTGT) THEN GOSUB bear_left
      IF (diffs=RIGHTGT) THEN GOSUB bear_right
    ENDIF

    ' get range from wall, display, and check for done
    wtemp1 = (inch_l + inch_r)/2
    IF (wtemp1 >= OUTER) AND (wtemp1 < 96) THEN turnto_wall  ' dist from wall before done

     ' start displaying distance from wall, once > 60 inches
     IF (inch_l > 60) THEN
      wtemp1= SDOG_E+20
      GOSUB set_sdog1
      wtemp1= SDOG_0+(10*inch_l)
      GOSUB set_sdog2
    ENDIF

'     IF (x >= HOME_TIME) THEN turnto_wall     ' max time to walk to start
    x=x+1
    PAUSE 100            ' NOTE that OUTER uses tick-time
    GOSUB test_switch
  LOOP

' turn back towards wall, and finish
turnto_wall:
  vflag=1
  GOSUB turn_left
  x=0
  DO
    IF (mode <> 2) THEN looptop

    IF (x >= 160) THEN              ' turn for 160*0.05 = 8-sec
      vflag=0
      GOSUB anticipate
      wtemp2=0
      GOSUB play_mary
      GOSUB center_servos
      PAUSE 1000
      GOSUB scan_sonar
      wtemp2=10
      GOSUB play_mary
      PAUSE 1000
      GOSUB center_servos

      mode=0
      GOTO looptop
    ENDIF

    x=x+1
    PAUSE 50
    GOSUB test_switch
  LOOP

' **** Mode 0 - Standby (display sonars+batteries) ****
mode0:
  GOSUB disp_mode
  GOSUB start_servos
  GOSUB center_servos
  x=0
  DO
    IF (mode <> 0) THEN looptop

    ' perform scheduled actions
    IF (x=0) THEN GOSUB disp_mode
    IF (x=20) THEN GOSUB disp_volt1   ' 2-sec
    IF (x=40) THEN GOSUB disp_volt2   ' 4-sec
    IF (x=60) THEN GOSUB disp_sonar1  ' 6-sec
    IF (x=75) THEN GOSUB disp_sonar2  ' 7.5-sec
    IF (x=90) THEN GOSUB disp_sonar3  ' 9-sec
    IF (x=100) THEN GOSUB stop_servos

    x = x+1
    IF (x >= 105) THEN x=0    ' wrap at 10.5-sec
    PAUSE 100                 ' update tick 10/sec
    GOSUB test_switch
  LOOP

' **** Mode 1 - Test Gaits ****
mode1:
  GOSUB start_servos
  GOSUB disp_mode
  vflag=1
  x=0
  DO
    IF (mode <> 1) THEN looptop

    ' perform scheduled actions - 5-sec at each gait
    IF (x=20) THEN GOSUB forward
    IF (x=70) THEN GOSUB backward
    IF (x=120) THEN GOSUB turn_right
    IF (x=170) THEN GOSUB turn_left
    IF (x=220) THEN GOSUB stance_low
    IF (x=270) THEN GOSUB center_servos
    IF (x=320) THEN GOSUB scan_sonar
    IF (x=420) THEN GOSUB center_servos

    x = x+1
    IF (x >= 470) THEN mode=0    ' end at 47-sec
    PAUSE 100                    ' update tick 10/sec
    GOSUB test_switch
  LOOP

' **** Mode 3 - Pack-Up Turret for Shipping ****
mode3:
  GOSUB disp_mode
  GOSUB start_servos
  x=0
  DO
    IF (mode <> 3) THEN looptop

    IF (x=20) THEN GOSUB pack_up          ' after 2-sec
    IF (x=50) THEN GOSUB stop_servos      ' after 5-sec

    x = x+1
    PAUSE 100
    GOSUB test_switch
  LOOP

  GOTO looptop
END


' ***********************************************
'  check left-side right-side sonar difference
' ***********************************************
' set return flag
chk_differ2:
  diffs = DIFFEQ
  IF (inch_l - inch_r) < DEADB_H THEN RETURN
  IF (inch_r - inch_l) < DEADB_H THEN RETURN
  diffs = RIGHTGT
  IF (inch_l >= inch_r) THEN diffs = LEFTGT
  RETURN

' *********************************************
'  read left-side, right-side sonars
' *********************************************
' output in inches
read_sonar23:
  ' left-side - average 2 sonar readings
  GOSUB get_sonar2
  inch_l = wtemp2
  GOSUB get_sonar2
  inch_l = (inch_l + wtemp2) / 2
  inch_l = inch_l ** RAWTOIN

  ' right-side - average 2 sonar readings
  GOSUB get_sonar3
  inch_r = wtemp2
  GOSUB get_sonar3
  inch_r = (inch_r + wtemp2) / 2
  inch_r = inch_r ** RAWTOIN
  RETURN

' output in centimeters
read_sonar34:
  ' left-side - average 2 sonar readings
  GOSUB get_sonar2
  inch_l = wtemp2
  GOSUB get_sonar2
  inch_l = (inch_l + wtemp2) / 2
  inch_l = inch_l ** RAWTOCM

  ' right-side - average 2 sonar readings
  GOSUB get_sonar3
  inch_r = wtemp2
  GOSUB get_sonar3
  inch_r = (inch_r + wtemp2) / 2
  inch_r = inch_r ** RAWTOCM
  RETURN

' *******************************
'   mary had a lamb
' *******************************
play_mary:
  FOR x = 0 TO 28 ' play the 29 notes of the LOOKUP table.
    LOOKUP x,[E,D,C,D,E,E,E,R,D,D,D,R,E,G,G,R,E,D,C,D,E,E,E,E,D,D,E,D,C],wtemp1
    ' uses dual beat-tones.
    FREQOUT Spkr,225,wtemp1,(wtemp1-wtemp2) MAX 32768
  NEXT
  RETURN

' ***********************************
'   test switch presses, set mode
' ***********************************
test_switch:
    IF (SW1=0) THEN PAUSE 50         ' wait-debounce
    IF (SW1=0) THEN
      mode = mode+1
      IF (mode > 3) THEN mode=0
      GOSUB disp_mode

TS3: IF (SW1=0) THEN TS3             ' wait-release
    ENDIF
    RETURN

' ************************************
'  display sonars - output in inches
' ************************************
sig_sonar1:
  ' set S-Dog left = "E.1"
  wtemp1 = 10

sigs_20:
  wtemp1 = SDOG_E + wtemp1
  GOSUB set_sdog1
  RETURN

sig_sonar2:
  ' set S-Dog left = "E.2"
  wtemp1 = 20
  GOTO sigs_20:

sig_sonar3:
  ' set S-Dog left = "E.3"
  wtemp1 = 30
  GOTO sigs_20:

disp_sonar1:
  ' set S-Dog left = "E.1"
  wtemp1 = SDOG_E+10
  GOSUB set_sdog1
  GOSUB get_sonar1
  wtemp1 = wtemp2
  GOSUB get_sonar1
  GOTO son10

disp_sonar2:
  ' set S-Dog left = "E.2"
  wtemp1 = SDOG_E+20
  GOSUB set_sdog1
  GOSUB get_sonar2
  wtemp1 = wtemp2
  GOSUB get_sonar2
  GOTO son10

disp_sonar3:
  ' set S-Dog left = "E.3"
  wtemp1 = SDOG_E+30
  GOSUB set_sdog1
  GOSUB get_sonar3
  wtemp1 = wtemp2
  GOSUB get_sonar3

son10:
  wtemp1 = (wtemp1 + wtemp2) / 2
  wtemp1 = wtemp1 ** RAWTOIN

disp_dist:
  wtemp1 = SDOG_0 + (10*wtemp1)
  GOSUB set_sdog2
  RETURN

' ***************************************
'   display operating mode
' ***************************************
disp_mode:
    ' set S-Dog left to display "A.0"+mode - (J.9 mode)
    wtemp1 = SDOG_A+(10*mode)
    GOSUB set_sdog1
    ' set S-Dog right to underflow "oo"
    wtemp1 = SDOG_0 - 10
    GOSUB set_sdog2
    RETURN

' *********************************************
'   display battery voltages - read WMC A/D
' *********************************************
disp_volt1:
    ' set S-Dog left to display "B.1" - (J.9 mode)
    wtemp1 = SDOG_B+10
    GOSUB set_sdog1
    ' read A/D 5 on WMC-RE0
    SEROUT TX2,BAUD2,["A05",CR]
    SERIN RX2,BAUD2,v1err,TIMO2,v1err,[HEX2 wtemp1]
    GOTO dv_20
v1err:
    wtemp1 = -10
    GOTO dv_20

disp_volt2:
    ' set S-Dog left to display "B.2" - (J.9 mode)
    wtemp1 = SDOG_B+20
    GOSUB set_sdog1
    ' read A/D 6 on WMC-RE1
    SEROUT TX2,BAUD2,["A06",CR]
    SERIN RX2,BAUD2,v2err,TIMO2,v2err,[HEX2 wtemp1]
    GOTO dv_20
v2err:
    wtemp1 = -10

dv_20:
    ' display A/D on S-Dog right - (99 mode)
    wtemp1 = SDOG_0 + (4*wtemp1)
    GOSUB set_sdog2
    RETURN

' ***********************************
'   subroutine to update S-Dog
' ***********************************
set_sdog1:
    ' left S-Dog connected to BS2-P11, using "J9" mode
    ' pulse resolution on BS2 = 2-usec
'     wtemp1 = 1000 + 400 + (10*inches/12)
    wtemp1 = wtemp1 >> 1                     ' divide by resolution
    PULSOUT 11, wtemp1
    RETURN

set_sdog2:
    ' right S-Dog connected to BS2-P12, using "99" mode.
    ' pulse resolution on BS2 = 2-usec.
'     wtemp1 = 1000 + (10*inches) + 5
    wtemp1 = wtemp1 >> 1                      ' divide by resolution
    PULSOUT 12, wtemp1
    RETURN


' *************************************************
'  subroutine - get 1-way sonar distance in usec
' *************************************************
get_sonar1:
  LOW PING1
  PULSOUT PING1, TRIGGER
  PULSIN  PING1, 1, wtemp2

son_20:
  wtemp2 = wtemp2 */ SCALE
  wtemp2 = wtemp2 / 2
  RETURN

get_sonar2:
  LOW PING2
  PULSOUT PING2, TRIGGER
  PULSIN  PING2, 1, wtemp2
  GOTO son_20

get_sonar3:
  LOW PING3
  PULSOUT PING3, TRIGGER
  PULSIN  PING3, 1, wtemp2
  GOTO son_20

' *************************************
'   turn servo pulses = on/off on WMC
' *************************************
start_servos:
  SEROUT TX2,BAUD2,["SO",CR]     ' turn on servo ops
  RETURN
stop_servos:
  SEROUT TX2,BAUD2,["SF",CR]     ' turn off servo ops
  RETURN

' *******************************
'   WMC setup subroutine
' *******************************
wmc_setup:
  SEROUT TX2,BAUD2,["FF",CR]         ' WMC debug echos=off
  SEROUT TX2,BAUD2,["FL",CR]         ' WMC long response delay

  ' center pan+tilt servo + set velocity
  SEROUT TX2,BAUD2,["SV0E04",CR]
  SEROUT TX2,BAUD2,["SV0F04",CR]

  SEROUT TX2,BAUD2,["SA00CFFF",CR]   ' enable drive servos + pan

center_servos:
  ' move servos to center position
  SEROUT TX2,BAUD2,["BF",CR]
  SEROUT TX2,BAUD2,["SC",CR]
  GOSUB start_servos
  RETURN

' *********************************************
'   select gaits stored in WMC eeprom
' *********************************************
forward:
  btemp3 = $02
  GOTO load_wait:
fast_forward:
  btemp3 = $02
'   btemp3 = $0D
  GOTO load_wait:

backward:
  btemp3 = $05
  GOTO load_wait:

bear_left:
  btemp3 = $06
  GOTO load_wait:
turn_left:
  btemp3 = $03
  GOTO load_wait:
half_left:
  btemp3 = $24
  GOTO load_wait:

bear_right:
  btemp3 = $07
  GOTO load_wait:
turn_right:
  btemp3 = $04
  GOTO load_wait:
half_right:
  btemp3 = $25
  GOTO load_wait:

ready_pos:
  btemp3 = $00
  GOTO load_wait:
stance_low:
  btemp3 = $08
  GOTO load_wait:
anticipate:
  btemp3 = $1C
  GOTO load_wait:

scan_sonar:
  btemp3 = $1E
  GOTO load_wait:

' send gait load command to WMC
load_wait:
  IF (vflag=1) THEN
    GOSUB start_servos
    SEROUT TX2,BAUD2,["EQ", HEX2 btemp3, CR]
  ELSE
    SEROUT TX2,BAUD2,["EL", HEX2 btemp3, CR]
  ENDIF

    ' set S-Dog left to display "G.0" - (J.9 mode)
    wtemp1 = SDOG_G
    GOSUB set_sdog1
    ' set S-Dog right to gait#
    wtemp1 = SDOG_0 + (10*btemp3)
    GOSUB set_sdog2

  PAUSE 20
  RETURN

' **********************************************
'   send turret to pack position for shipping
' **********************************************
pack_up:
  GOSUB freeze_ready
  PAUSE 100
  SEROUT TX2,BAUD2,["M0EFF04",CR]
  SEROUT TX2,BAUD2,["M0FFF04",CR]
  RETURN

' **********************************************
'   stop behavior exection, and center servos
' **********************************************
freeze_ready:
  SEROUT TX2,BAUD2,["BF",CR]
  SEROUT TX2,BAUD2,["SC",CR]
  RETURN