{  PropForth Robot Control - Basic Motion Commands, Maneuvers and Applications - StingRay Version
                           Nicholas G. Lordi  1/2013  V.0.1

   !! pfrcsr.f must be preloaded !!

This wordset uses an indexed approach with predefined data tables.  This approach 
simplifies robot control using android devices. Tables are defined for both right
and left wheels, allowing the user to calibrate the robot wheels to account for differences   
in wheel motion, especially on different surfaces.  

Note:  This is not complete - it is merely one example of an approach to extending the
       basic robot control vocabulary.   Wheel calibration has not been done.
}

fl

fload pfrca

: (PFRCA) ;


wvariable ci  \ current speed index number
: setSPD      \  ( n1 -- ) Stores the speed index number n1 in ci.
   ci W!  ;

wvariable pno  \ priority number used to prioritize maneuvers, e.g., 0, 1, 2, 3, ...


\  StingRay Calibration Tables 

variable lduty() 10 w, 150 w, 175 w, 200 w, 225 w, 250 w, 275 w, 300 w, 325 w, 350 w, 375 w, 
variable rduty() 10 w, 150 w, 175 w, 200 w, 225 w, 250 w, 275 w, 300 w, 325 w, 350 w, 375 w,

\ table listing times in millisec
variable time()  100 l, 200 l, 500 l, 1000 l, 2000 l, 5000 l, 10000 l, 20000 l, 50000 l, 100000 l,         

: L()  \ ( n1 -- )  n1 is left speed index 1 - 10
   dup 10 > if drop 10 then 2* 4 + lduty() + W@  ;

: R()   \ ( n1 -- ) n1 is right speed index 1 - 10
   dup 10 > if drop 10 then 2* 4 + rduty() + L@  ;

: T()   \ ( n1 -- ) n1 is index (1-20) selecting motion times from 100 ms to 10 sec.
   dup 120 > if drop 10 then 4 * 4 + time() + L@  ;

\ Indefinite Basic Motion Commands

\ (n1 -- ) where n1 is the speed index number

: F  \ go forward
    dup R() swap  L() setDUTY  ;

: B  \ go backwards  
    dup R() negate swap  L() negate setDUTY ;

: C  \ clockwise rotation in place
   dup R() negate swap L() setDUTY  ;

: CC \ counter cockwise rotation in place
    dup R() swap L() negate setDUTY  ;

: RT \ turn right 
    0 swap L() setDUTY ;

: LT \ turn left
    R() 0 setDUTY  ;

: rampUP  \ ( n1 -- )  Increments speed from index ci to n1 & updates ci.
    ci W@ do 50 delms i dup F ci W! loop ;

: rampDN  \ ( n1 -- )  Decrements speed from index ci to n1 & updates ci.
    ci W@ swap do  ci W@ 1- 50 delms  dup F ci W!  loop ;


\ Timed Basic Motion Commands - uses the speed index stored in ci. 
\ ( n1 -- )  where n1 is an index selecting the number of millisecs the defined motion is active.

: goF  
    T() ci W@  F  delms S ;

: goB
    T() ci W@  B  delms S ;

: goC
    T() ci W@  C  delms S ;

: goCC
    T() ci W@  CC  delms S ;

: goRT
    T() ci W@  RT  delms S ;

: goLT
    T() ci W@  LT  delms S ;

{
 Note: Most of the following words have not been tested at this time, but are presented for review.
{
 

\ Encoder controlled motions  - need to be calibrated for different speeds.

: chkRENC \ ( n1 -- ) Robot moves until right differential encoder count = n1, then stops.
    renc L@ srenc L! begin  rdenc L@ > if until S  ; 

: chkLENC \ ( n1 -- ) Robot moves until left differential encoder count = n1, then stops.
    lenc L@ slenc L! begin  ldenc L@ > if until S  ; 

: 45RT  \ ( -- ) Turns 45 deg right.
   ci W@ RT 5142 chkRENC  ;

: 45LT  \ ( -- ) Turns 45 deg left.
   ci W@ LT 5142 chkLENC  ;

: 90RT  \ ( -- ) Turns 90 deg right.
   ci W@ RT 10283 chkRENC  ;

: 90LT  \ ( -- ) Turns 90 deg left.
   ci W@ LT 10283 chkLENC  ;


: VRT  \ ( -- ) Robot veers 10 deg right and continues forward.
    2 RT 1143 chkRENC ci W@ F  ;

: VLT  \ ( -- ) Robot veers 10 deg left and continues forward.
    2 LT 1143 chkLENC ci W@ F  ;

: REVERSE  \ ( -- )  Robot stops, then rotates 180 degrees and continues forward motion.
    S 2 C  10284 chkENC ci W@ F  ;  

{
Maneuvers - These words illustrate how maneuvers can be defined, using basic motion
              commands and sensor inputs, i.e., ping distance measurements. 
  
Note:  I was influenced by Steve Norris' Mission-Roam.spin program for the StingRay.    
}


\ BLOCK checks to establish whether the robot is blocked. If so, the robot backs up and 
\ and waits for additional commands.  Default priority = 0;
: BLOCK   \ ( -- ) Increments pno if active, otherwise, resets pno to default..
    FSTP STP or if clrFLAGS 1500 goB  1 pno W! else 0 pno W! then  ;

\ Checks center right ping reading. If < limit, increments pno, otherwise veers 45 deg right.
: chkCRPG  \ ( -- ) 
     crdist ?crpg W@ <= if  1 pno W+! else 45RT ci W@ F 0 pno W! then  ;

\ Checks center left ping reading. If < limit, increments pno, otherwise veers 45 deg left.
: chkCLPG  \ ( -- ) 
     cldist ?lrpg W@ <= if  1 pno W+! else 45LT ci W@ F 0 pno W! then  ;

\ Checks center right ping reading. If < limit, increments pno, otherwise veers 90 deg right.
: chkRPG  \ ( -- ) 
     cdist ?rpg W@ <= if  1 pno W+! else  90RT ci W@ F 0 pno W! then  ;

\ Checks center left ping reading. If < limit, increments pno, otherwise veers 90 deg left.
: chkLPG  \ ( -- ) 
     ldist ?lpg W@ <= if  1 pno W+! else  90LT ci W@ F 0 pno W! then  ;

{ Applications combine maneuvers to accomplish a specific task. In this example (TASK1), the
  robot enters a room in which there are obstacles to its free movement. The robot's objective
  is to navigate the obstacles until it finds the exit. This example also illustrates how 
  deferred words might be used in building applications.

  TASK1 sets the event counter to 0 and executes a forward motion at a predetermined speed, 
  e.g., 4 setSPD.  BLOCK followed by the deferred word act10 are executed in a loop which is
  terminated at a preset time, e.g., 10 minutes (4800 setEVT).  act10 defines the navigation 
  strategy used by the robot.  Two examples, OPTION1 & OPTION2, among many possibilities are
  defined. 

  10 actIS OPTION1 uses four pings to determine whether movement in a specific direction is 
                   feasible. The user needs to set the limits (setCRPG, etc.) corresponding 
                   to each ping. If all directions are blocked, the robot reverses.

  10 actIS OPTION2 uses only the center front ping and BLOCK to control its motion. An array 
                   of five deferred words, which define the five directions the robot can turn,
                   are randomly selected.           
                         
}

: TASK1  \ ( -- )
     resEVT  ci W@ F
     begin
       BLOCK  
       act10 
     evt L@ ?evt L@ < until  ;

: OPTION1 \ ( -- )  
    pno W@ dup 0= if          drop else dup
              1 = if  chkCRPG drop else dup
              2 = if  chkCLPG drop else dup
              3 = if  chkRPG  drop else dup
              4 = if  chkLPG  drop else 
              5 = if  0 pno W! REVERSE  thens  ;

1 actIS 45RT   2 actIS 45LT   3 actIS 90RT   4 actIS 90LT   5 actIS REVERSE

: OPTION2 \ ( -- ) generates pseudo-random numbers 0 - 5, executing deferred words act0 - act5.
    rnd 43 / ACT() execute ci W@ F ;



...