{
           PropForth Robot Control Core Vocabulary  StingRay Version 3.2  2/2013
                                  Nicholas G. Lordi

 This document describes the  PropForth Core Robot Control word set for the Parallax Stingray Robotic platform.
 The logic for selecting forth as a software platform for developing robot applications has been discussed in
 the HBRC (Home Brew Robot Club) Wiki Forth on Robots. My objective, in using a forth based  approach to robot
 control, is to develop a common platform which could be used on any propeller based robot.  

 My StingRay includes the following hardware:      

   1. Parallax Propeller Robot Control Board (#282430)
   2. Parallax Li-ion Power Pack/Charger – 2 Cells, 7.4 VDC
   3. 7.2 Volt 310 rpm Geared Motors  (30:1 gear ratio)
   4. Lynx Motion Encoders (100 ticks/rotation) 
   5. 5 Ping Sensors facing left side, center left, front center, center right  and right sides.
   6. 1 Sharp (GP2Y0A21yk0f) IR Distance Sensor facing rear
   7. A PCB Board (replaces breadboard) to which is attached:
         a. MCP3204 12 bit adc chip. .
         b. SD card adapter if the sd version instead of the eeprom version of the forth os is used.
   8. A bumper attached to the front through 3 momentary contact switches wired to a single pin.
   9. Xbee (series 1) is used for wireless communication with the Tera Term serial port emulation on
      a PC, used to download  forth programs to the eeprom as well as operate the robot.  Alternately,
      a bluetooth board can replace the Xbee, allowing the robot to be controlled from an android device.
  10. A Quick Start board used as a coprocessor for interfacing with other sensors that might be added
      to the StingRay, e.g., a  GPS module, accelerometer, camera, environmental monitoring, etc.
      Communication between the coprocessor and the main control board will be a through a 4-bit parallel port. 
      I plan to program the Quick Start in Tachyon, making the StingRay a two forth as well as two processor system.
    
    
 The PropForth V.5.03 eeprom developers version was installed on the Stingray propeller. PropForth is modified
 so that pin 6 is RX and pin 7 is Tx, which are connected to the XBee or bluetooth board.
 The program, pfrcsr.f, is stored in the upper 32 kb of the 64 kb eeprom. 
    
 The robot control vocabulary is designed as four code layers on top of the forth OS. The lowest layer is the core program,
 which defines the parameters and drivers specific to the  robotic system (in this case, the Stingray). Second is a
 code extension which defines commands common to robot control, i.e., forward, backward, etc. Next, maneuvers which
 define more complex robot actions, combining sensor inputs with basic robot motions. Finially, applications
 which combine maneuvers, enabling the robot to exhibit autonomous behaviors.      

    This program, pfrcsr.f, is stored on the eeprom and loaded using fsload when needed.
    The first defined word is (PFRCSR), which establishes a marker in the dictionary,
    locating the start of the core program. The command, forget (PFRCSR), deletes the core
    program from the dictionary.

    The User Interface  consists of those words which allow the user to develop and debug a robot control vocabulary.
    Five cogs are used by the program.

      Execute - 
 
        START to initialize parameters and start all driver cogs.
       
                          Cog 1  Lstart – Left wheel driver and encoder.
                          Cog 2  Rstart – Right wheel driver and encoder.
                          Cog 3  Pstart   Left, Center Left, Center Right, and Right Ping drivers.
                          Cog 4  Bstart – Front Bumper, Center Ping, and Rear ADC sensors.
                          Cog 5  Cstart   Monitor parallel port, calculate wheel velocities & encoder
                                          differential counts. Increment event clock.

        setDEFAULT initializes parameters - executed by START.

        n1 STOP, where n1 is 1 - 5, to stop a specific cog.

        Lstart, Rstart, etc., to restart a stopped cog.

        resENC to reset encoder counts to 0.  This command resets cogs 1 & 2.

        n1 n2 setDUTY, where n1 references the right wheel and n2 references the left wheel. Values range
               between -1000 and 1000. Practical values are limited to -500 to 500 to avoid excessive speeds.
               This single command can define all possible robot motions.  

        n1 radj and/or n2 ladj to increment or decrement right/left duties by n1/n2 values. These commands 
               allow the user to dynamically alter robot speed and direction. 

        S  to stop robot - sets left & right duties to 0.

        H  to halt robot - incremetally reduces right & left duties to 0.           
    
        The following words display ping and encoder values:

            prDIST displays current left, center left, center, center right & right ping measured distances in inches.

            prENC  displays current left & right raw encoder counts.

            prVEL  displays current left & right wheel velocities in counts/sec. Values are computed at 125 ms intervals.

            prDENC displays current left & right wheel differenital counts, referenced to user defined
                   prameters, slenc & srenc, i.e., dlenc = lenc - slenc &  rlenc = renc - srenc. 

            prADC  displays rear adc values.

        The following words allow the user to set ping limits in inches:
            set?RPG,  set?CRPG,  set?CPG,  set?CLPG, and  set?LPG.

        clrFLAGS  sets center ping, bumper & adc flags to 0, clearing automatic stop modes. 

        resEVT  sets event timer to 0.  The event timer increments the parameter evt at 125 ms intervals. 

        setEVT  sets event time limit (?evt). 

        The following words can be used to change constant values.   Use:  value TO|WTO <constant>.
        Word constants which one might wish to change are scale (used to adjust range of duties) and dt 
        (delay time between actions assigned to cog 5). 
         
            WTO (words) & TO (longs)

        

Note:  The event timer functions as a resetable internal robot timer. It can be used to schedule
       time dependent maneuvers.

       n1 actIS <name>  where n1 is an index 0 - 15  and <name> is a word which will be executed when
                        actn1 (e.g., act15) is executed as part of a predefined maneuver.  act0 - act15
                        represent deferred actions, allowing the user to define maneuvers whose actions
                        can be changed without redefining the maneuver. 

Note: Another use of the act() array included in pfrcsr, is to define actions that are directed by the
      coprocessor through the uni-directional 4 bit port which connects the coprocessor to the control
      processor. This port is monitored every 125 ms in cog 5. Two actions are predefined: act0 executes
      noop and act15 executes a halt.       

Note: Five constants have been predefined, which may be useful in developing maneuvers using the encoders.
      wcirc - wheel circumference in 0.1 inch units
      counts - number of encoder counts per wheel rotation
      ctin - encoder counts per inch of travel
      rdiam - rotation diameter in 0.1 nich units
      tdiam - turn diameter in 0.1 inch units

Note: Since the encoders are attached to the rear shaft of geared motors, the encoder count number is 
      very sensitive to variations in wheel velocity characteristic of the high speed StingRay motors.
      The quadratic feature of the encoders is not used in this version.  
              
}

\ PropForth Robot Control StingRay Source Code

fl

fswrite pfrcsr.f

: (PFRCSR) ;


\ cog register addresses

h1F8	wconstant ctra

h1F9	wconstant ctrb

h1FA	wconstant frqa

h1FB	wconstant frqb

h1FC	wconstant phsa

h1FD	wconstant phsb


\ system variables

variable lduty

variable rduty

variable rdist

variable crdist

variable cdist

variable cldist

variable ldist

variable lenc

variable renc

variable srenc

variable slenc

variable rdenc

variable ldenc

variable lvel

variable rvel

variable evt

wvariable stp
wvariable bstp
wvariable fstp

wvariable badc
wvariable ?badc
wvariable ?evt

wvariable ?lpg       \ ping user defined limits
wvariable ?clpg
wvariable ?cpg
wvariable ?crpg
wvariable ?rpg


\ pin assignments

 0 wconstant _cs     \ M3204 enable
 1 wconstant _dpin   \ M3204 data io
 2 wconstant _clk    \ M3204 clock

 3 wconstant BMP     \ bumper 

 \ 6 xbee/bluetooth din
 \ 7 xbee/bluetooth dout 

 8 wconstant LE1     \ left encoder

10 wconstant RE1     \ right encoder

11 wconstant CPG     \ center ping

12 wconstant RPG     \ right ping

13 wconstant CRPG    \ center right ping
    
14 wconstant CLPG    \ center left ping
     
15 wconstant LPG     \ left ping
     
25 wconstant LFW     \ left wheel forward
     
24 wconstant LBW     \ left wheel backward
    
26 wconstant RFW     \ right wheel forward
    
27 wconstant RBW     \ left wheel bakward

16 wconstant PT1     \ reserved for parallel port
17 wconstant PT2
18 wconstant PT3
19 wconstant PT4

21 wconstant CS      \ reserved for sd card   
22 wconstant DI      
23 wconstant CLK 
24 wconstant DOUT


\ counter modes

h_1000_0000 RPG  + constant RPG1mode

h_2000_0000 RPG  + constant RPG2mode

h_1000_0000 CRPG  + constant CRPG1mode

h_2000_0000 CRPG  + constant CRPG2mode

h_1000_0000 CPG  + constant CPG1mode

h_2000_0000 CPG  + constant CPG2mode

h_1000_0000 CLPG  + constant CLPG1mode

h_2000_0000 CLPG  + constant CLPG2mode

h_1000_0000 LPG  + constant LPG1mode

h_2000_0000 LPG  + constant LPG2mode

h_1000_0000 LFW  + constant LFWmode

h_1000_0000 RFW  + constant RFWmode

h_1000_0000 LBW  + constant LBWmode

h_1000_0000 RBW  + constant RBWmode

h_2800_0000 LE1  + constant LE1mode

h_2800_0000 RE1  + constant RE1mode


\ system constants

10   wconstant scale     \ scales user input duty range
4000 wconstant lperiod
4000 wconstant rperiod
125  wconstant dt        \ millisec delay time in driver assigned to cog 5  

\ user constants

153  wconstant wcirc  \ wheel circumfirence in 0.1 inch units
3000 wconstant counts \ number of counts per wheel rotation
196  wconstant ctin   \ counts per inch 
314  wconstant rdiam  \ rotation diameter in 0.1 inch units
628  wconatant tdiam  \ turn diameter in 0.1 inches
 

\ parameter and pin initialization  

: setDEFAULT
      0 lduty L! 0 rduty L! 0 renc L! 0 lenc L! 0 slenc L! 0 srenc L!
      0 lvel L!  0 rvel L! 0 evt L!  0 ldenc L!  0 rdenc L!  
      0 ldist L! 0 cldist W! 100 cdist L! 0 crdist W! 0 rdist L!  
      30 ?lpg L! 30 ?clpg W! 15 ?cpg L! 30 ?crpg W! 30 ?rpg L!
      1500 ?badc W!  0 stp W!  0 bstp W!  0 fstp W!  ;


\ asm ADC function  (Code provided by caskaz)

\ ( n1 n2 n3 n4 n5 -- n6 )  
\ n1:channel number  n2:single/differential   n3:_clk  n4:_dpin  n5:_cs  n6:result
lockdict create a_get_A/D forthentry
$C_a_lxasm w, h13D  h113  1- tuck - h9 lshift or here W@ alignl h10 lshift or l,
z2WyPW1 l, zfiPZB l, z1SyLI[ l, z2WyPb1 l, zfiPeB l, z1SyLI[ l, z2WyPj1 l, zfiPmB l,
z1SyLI[ l, z2WiPuB l, z1SyLI[ l, z1YyPr1 l, z20yPOG l, z20oPO8 l, zfyPOR l, z2WyPr5 l,
z1[ixZC l, zgyPO1 l, z1bfxZD l, z1Sya[p l, z1[ixZD l, z3[yPv[ l, z1[ixmD l, z1Sya[p l,
z2WyPrD l, zfyPO1 l, z1Sya[p l, 0 l, z1YFPil l, z20oPO1 l, z3[yPvf l, zbyPO1 l,
z1bixZC l, z1bixmD l, z1SV01X l, z1bixZE l, z2WiQFk l, z20yQ8G l, z3ryQ80 l, z1[ixZE l,
z1SF04v l, 0 l,
freedict 

\  utilities

: noop ;

: WTO  \ ( n1 -- <name> ) change wconstant value to n1
    ' 2+ W! ; 
: TO   \ ( n1 -- <name> ) change constant value to n1
    ' 4+ L! ;
    

: prDUTY      \ displays current left & right scaled duties (range -1000 to 1000)
   lduty L@ . 2 spaces rduty L@ . cr  ;

: clrFLAGS    \ clears all flags to 0
   0 stp W!  0 bstp W!  0 fstp W! ;

: resSENC      
    0 srenc L! 0 slenc L! ;

: resEVT
    0 evt L! ;

: setEVT  \ ( n1 -- ) where n1 is the event timer limit
    ?evt L!  ;

: setCRPG  \ ( n1 -- ) Sets center right ping limit to n1 inches.
    ?crpg W!  ;

: setCPG  \ ( n1 -- ) Sets ceter ping limit to n1 inches.
    ?cpg W!  ;
 
: setCLPG  \ ( n1 -- ) Sets center left ping limit to n1 inches.
    ?clpg W!  ;

: setRPG  \ ( n1 -- ) Sets right ping limit to n1 inches.
    ?rpg W!  ;

: setLPG  \ ( n1 -- ) Sets left ping limit to n1 inches.
    ?lpg W!  ;
 

\ ( -- t/f ) leaves the current flag on the stack
: FSTP  \ t if front center ping distance is less than ?cpg                     
    fstp W@  ;

: BSTP  \ t if rear adc reading is less than ?badc
    bstp W@  ;

: STP   \ t if the bumper is activated
    stp  W@  ;

: EVT   \ ( -- n1 ) where n1 is the current event count
    evt L@  ;

: prDIST      \ displays current left, center left, center, center right & right ping measured distances in inches
   ldist L@ . 2 spaces  cldist L@ . 2 spaces  cdist L@ . 2 spaces  crdist L@ . 2 spaces  rdist L@ . cr ;

: prENC       \ displays current left & right raw encoder counts
   lenc L@ . 2 spaces  renc L@ . cr   ;

: prVEL       \ displays current left & right wheel velocities in counts/sec
   lvel L@ . 2 spaces  rvel L@ . cr  ; 

: prDENC      \ displays current left & right differential encoder values
   ldenc L@ . 2 spaces rdenc L@ . cr  ;
 
: prADC      \ displys rear Sharp IR sensor (adc) value
   badc W@ . cr ;


: array  lockdict create $C_a_dovarw w, 0 w, 2* allot 999 w, forthentry freedict ;
   

\ deferred words

: act0 noop ;  : act1 noop ;  : act2 noop ;   : act3 noop ;  
: act4 noop ;  : act5 noop ;  : act6 noop ;   : act7 noop ; 
: act8 noop ;  : act9 noop ;  : act10 noop ;  : act11 noop ;  
: act12 noop ; : act13 noop ; : act14 noop ;  : act15 noop ;  

16 array act()
     
: ACT()  \ ( n1 -- addr ) leaves address of n1 ( index 0 - 15) array act()
     2* act() + ;

\ act0 - act15 are deferred words which can be defined to execute user selected words as needed 
 
' act0 0 ACT() W!  ' act1 1 ACT() W!  ' act2 2 ACT() W!  
' act3 3 ACT() W!  ' act4 4 ACT() W!  ' act5 5 ACT() W!  
' act6 6 ACT() W!  ' act7 7 ACT() W!  ' act8 8 ACT() W!
' act9 9 ACT() W!  ' act10 10 ACT() W!  ' act11 11 ACT() W!  
' act12 12 ACT() W!  ' act13 13 ACT() W!  ' act14 14 ACT() W!  
' act15 15 ACT() W!  

: actIS   \ ( n1 -- ) e.g., 12 actIS <name> sets act12 to execute <name>    
     >r ' r> ACT() W@ W! ;


\ motor control command

: setDUTY  \ ( n1 n2 -- ) where n1 is right speed & n2 is left speed (-1000 to 1000). 
    scale * lduty L!  scale * rduty L!  ;

: ladj  \ ( n1 -- )  where n1 is an adjustment made to the left wheel duty.
    lduty L@ + lduty L!  ;

: radj  \ ( n1 -- )  where n1 is an adjustment made to the right wheel duty.
    rduty L@ + rduty L!  ;


: H   \ steps down speed, halting robot
      5 0 do 
      lduty L@ 2/ lduty L!  rduty L@ 2/ rduty L!  100 delms loop
    0 0 setDUTY  ;

15 actIS H
0  actIS noop

: S   \ stops robot
    0 0 setDUTY  ;



\ drivers 

\ The left wheel is controlled by Counter A (NCO single-ended mode) and the left encoder counts
\ accumulated by Counter B (pos edge detector mode).
: rc_lwheel
    LFW dup pinlo pinout  LBW dup pinlo pinout 
    LE1 dup pinlo pinin  
    1 frqa COG!  1 frqb COG!
    0 phsa COG!  0 phsb COG! 0 lenc L!    
    LE1mode ctrb COG!  
    begin
      lperiod cnt COG@ + 0 waitcnt drop
      lduty L@ abs negate phsa COG!      
      lduty L@ 0> if LFWmode ctra COG! else LBWmode ctra COG!  then
      phsb COG@ lenc L!
    0 until  ; 

\ The right wheel is controlled by Counter A (NCO single-ended mode) and the right encoder counts
\ accumulated by Counter B (pos edge detector mode).
: rc_rwheel    
    RFW dup pinlo pinout  RBW dup pinlo pinout 
    RE1 dup pinlo pinin  
    1 frqa COG!  1 frqb COG!
    0 phsa COG!  0 phsb COG! 0 renc L!   
    RE1mode ctrb COG!
    begin
      rperiod cnt COG@ + 0 waitcnt drop      
      rduty L@ abs negate phsa COG!
      rduty L@ 0> if RFWmode ctra COG! else RBWmode ctra COG!  then 
      phsb COG@ renc L!
    0 until  ; 

\ Four pings share two counters sequentially - pos detector mode is used to activate the ping and 
\ the NCO single-ended mode is used to measure the pulse width which is converted into inches.
: rc_ping
    1 frqa COG!  
    begin
     
      RPG pinout RPG1mode ctra COG!
      LPG pinout LPG1mode ctrb COG!
      -400 phsa COG!  -400 phsb COG!
      RPG pinin   RPG2mode ctra COG!
      LPG pinin   LPG2mode ctrb COG!
      0 phsa COG! 0 phsb COG! 40 delms
      phsa COG@ 11800 /  rdist L!
      phsb COG@ 11800 /  ldist L!

      CRPG pinout CRPG1mode ctra COG!
      CLPG pinout CLPG1mode ctrb COG!
      -400 phsa COG!  -400 phsb COG!
      CRPG pinin   CRPG2mode ctra COG!
      CLPG pinin   CLPG2mode ctrb COG!
      0 phsa COG! 0 phsb COG! 40 delms
      phsa COG@ 11800 / crdist L!
      phsb COG@ 11800 / cldist L!

    0 until  ; 

\ This driver stops the robot if the bumper is activated, the center ping detects a block,
\ or the rear adc detects a block.  clrFLAGS must be executed to correct its movement. 
\ Counter A is used to detect and measure the center ping's forward position. 
: rc_blocked
     BMP pinin    
    _cs pinout _dpin pinout  _clk pinout _cs pinhi
    1 frqa COG!  
    begin

      BMP px? 0= if -1 stp W!  S  then  

      CPG pinout CPG1mode ctra COG!
      -400 phsa COG!
      CPG pinin CPG2mode ctra COG!
      0 phsa COG!  30 delms
      phsa COG@ 11800 /  cdist L!  

      fstp W@ 0= if   
         cdist L@ ?cpg W@ < if  S -1 fstp W! thens
 
      bstp W@ 0= if       
        0 1 _clk _dpin _cs a_get_A/D dup badc W!
        ?badc W@ > if S  -1 bstp W! thens
            
    0 until ;

\ This driver monitors the 4-bit parallel port signal, executing one of 16 possible
\ predefined actions.  In addition, it calculates the the average wheel velocities in
\ counts/sec and the difference in counts between the current and previously defined
\ references. It also increments the event timer. All these actions occur every dt 
\ (default: 125) ms.    
: rc_port
    PT1 dup pinlo pinin  PT2 dup pinlo pinin
    PT3 dup pinlo pinin  PT4 dup pinlo pinin
    0 lvel L! 0 rvel L! 
    begin
 
      PT4 1+ PT1 do  i px? abs loop
      3 0 do  1 lshift + loop  ACT() W@ execute

      lenc L@ renc L@ dt delms lenc L@ renc L@ rot
      - abs 3 lshift rvel L!  - abs 3 lshift lvel L!   

      lenc L@ slenc L@ - ldenc L!
      renc L@ srenc L@ - rdenc L!

      evt L@ 1+ evt L!   

    0 until  ;
 

   
\ The following words ( -- )  reset the driver cogs and start the specified drivers.

: Lstart 1 cogreset  20 delms  c" rc_lwheel" 1 cogx  10 delms ;

: Rstart 2 cogreset  20 delms  c" rc_rwheel"   2 cogx  10 delms ;

: Pstart 3 cogreset  20 delms  c" rc_ping"  3 cogx  10 delms ;

: Bstart 4 cogreset  20 delms  c" rc_adc"  4 cogx  10 delms ; 

: Cstart 5 cogreset  20 delms  c" rc_port"  5 cogx  10 delms ;



: resENC  \ ( -- ) resets encoders - required to set encoder accumulated values to 0
    Lstart Rstart  ;  

: START  \ ( -- )  initialize parameters and executes drivers
    setDEFAULT  Lstart Rstart  Pstart  Bstart  Cstart ; 

: STOP  \ (n1 -- ) where n1 is the cog number 1 - 5
    cogstop  ;


...