\ PropForth Robot Control (pfrc) Vocabulary          Nicholas G. Lordi 5/2012   Ver.1.0
\
\ Designed for PropBoeBot board.  The bot includes encoders, bumpers and a servo 
\ driven Ping.  Communication is through a XBee wireless network. The XBee Dout and Din
\ are connected to prop pins 13 & 12.  Tx and Rx pin assignments must be changed in 
\ PropForthEeprom (v.5) source. 
\  
\ Controls two wheel servos in cog 1 and  one ping servo in cog 2. 
\ Reads Ping data in inches and activates bumpers in cog 3.
\ Reads encoder counts (8 per wheel rotation) in cog 4.
\ 
\ The basic user words follow:
\
\    set_duty ( n1 n2 --- ) where n1 (right wheel servo) and n2 (left wheel servo)
\             range in value from -100 to 100, defining relative speed and direction.
\
\    set_pos  ( n1 --- ) where n1 is the position of the ping servo in degrees (-90 to 90).
\                        Note: + values turns the ping to left & - values turns it to right.
\
\    Wstart ( --- ) executes the wheel servo controls (rc_servo) in cog 1.
\                 
\    Sstart ( --- ) executes the ping servo control (rc_pos) in cog 2.
\
\    Pstart ( --- ) executes the Ping control (rc_ping)in cog 3.  Returns the distance 
\                   between the bot and an obstacle in inches. Values are updated
\                   in the variable 'dist'. Also activates bumpers.
\
\    Estart ( --- ) executes the encoder controls (rc_enc) in cog 4. Returns the counts
\                  (8 per each wheel rotation) of both wheeels. Values are updated at 
\                  in the variables Lenc & Renc. 
\  
\    START  ( --- ) executes all 4 start words. 
\                   
\ First execute START, then use set_duty and/or set_pos to change parameters. 
\ The following words are examples defined to set specific bot motions.
\
\    STOP   ( --- ) stops bot motion by setting both servos to their stationary state.  
\
\ n1 is the relative speed 0 to 100 in all cases. The stated motions are indefinite
\ unless the command is followed by 'x delms STOP', where x is the number of milliseconds
\ allowed for the bot motion. Commands may be sequenced in any order to execute
\ complex motions.    
\
\    BKW    ( n1 --- )  moves backward 
\
\    FRW    ( n1 --- )  moves forward 
\
\    CW     ( n1 --- )  clockwise rotation in place 
\
\    CCW    ( n1 --- )  counter-clockwise motion in place 
\
\    RTURN  ( n1 --- )  turns right
\
\    LTURN  ( n1 --- )  turns left 





fl

fswrite pfrc.f

: (PFRC) ;


\ counter registers

h1F8	wconstant ctra
 
h1F9	wconstant ctrb

h1FA	wconstant frqa

h1FB	wconstant frqb

h1FC	wconstant phsa

h1FD	wconstant phsb

\ program variables

variable lduty
variable rduty
variable pos
variable dist
variable lenc
variable renc
wvariable lbump
wvariable rbump

\ prop pin assignments

10 wconstant LB    \ left bumper
11 wconstant RB    \ right bumper 
14 wconstant LE    \ left encoder
15 wconstant RE    \ right encoder
16 wconstant LW    \ left wheel servo
17 wconstant RW    \ right wheel servo
18 wconstant PS    \ ping servo
19 wconstant PG    \ ping 

\ program constants (counter modes)

h_1000_0000 LW  + constant LWmode
h_1000_0000 RW  + constant RWmode
h_1000_0000 PS  + constant PSmode
h_1000_0000 PG  + constant P1mode
h_2000_0000 PG  + constant P2mode
h_2800_0000 LE  + constant LEmode
h_2800_0000 RE  + constant REmode


\  parameter defining words

: set_duty   
   1500 + 80 * lduty L!  1500 + 80 * rduty L!  ;

: set_pos  
   10 * 1500 + 80 *  pos L!  ;

\ robot control words assigned to different cogs

: rc_pos  \ ping servo
   PS dup pinlo pinout
   0 set_pos
   1 frqa COG!
   PSmode ctra COG!
   begin
     pos L@ negate phsa COG!
     20 delms
   0 until  ;

: rc_servo   \ wheel servos    
    LW dup pinlo pinout  RW dup pinlo pinout
    1 frqa COG!  1 frqb COG!  
    1 -1 set_duty
    LWmode ctra COG!  RWmode ctrb COG!
    begin
      lduty L@ negate phsa COG!
      rduty L@ negate phsb COG!
      20 delms
    0 until  ; 

: rc_ping   \ ping & bumpers
    LB pinin  RB pinin
    0 lbump W!  0 rbump W!  
    1 frqa COG!
    begin
       PG pinout P1mode ctra COG!
       -400 phsa COG!       
       P2mode ctra COG! 0 phsa COG!
       PG pinin       
       50 delms 
       phsa COG@ 11800 / dist L! 
       LB px? if 0 Lbump W! else 1 -1 set_duty -1 Lbump W! then
       RB px? if 0 Rbump W! else 1 -1 set_duty -1 Rbump W! then
       0 until  ;

: rc_enc   \ wheel encoders
    LE dup pinlo pinin  RE dup pinlo pinin   
    1 frqa COG!  1 frqb COG! 
    0 phsa COG!  0 phsb COG!  
    0 lenc L!   0 renc L! 
    LEmode ctra COG!  REmode ctrb COG!    
    begin 
      phsa COG@ lenc L!
      phsb COG@ renc L!
    0 until ; 
      
: Wstart 1 cogreset  20 delms  c" rc_servo"  1 cogx  10 delms ;

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

: Estart 4 cogreset  20 delms  c" rc_enc"    4 cogx  10 delms ;  


: START    
    Wstart  Sstart  Pstart  Estart  ;

: STOP  1 -1 set_duty  ;


: BKW  dup negate set_duty ;
 
: FRW  dup negate swap set_duty ;

: CW  dup set_duty ;

: CCW  negate dup set_duty ;

: RTURN  0 swap set_duty ;

: LTURN  negate 0 set_duty ;

    
...