{   
 BOEPAB  - Tachyon (V2.3)  (Forth) Robot Control for BOE Platform + Prop Activity Board

  Designed for the Prop ActivityBot.  The bot includes encoders and a servo driven Ping.
  Communication is through a XBee ( dout: P11, din: P10) with a netbook or a pin equivalent
  bluetooth module (RN-XV) with an android tablet. Controls two wheel servos in cog 6, two
  encoders in cog 5, and the ping-servo in cog 4.

  TACHYON v2.3 (Tx: P11, Rx: P10  115200 baud) and EXTEND.fth  are installed on the 
  Prop Activity Board.  This file is then loaded and a BACKUP executed.

  The primary user words are summarized here.

    ROBOT CONTROL LANGUAGE CORE

     MSTART  executes the wheel servo driver in cog 4.

     n1 M1  sets the left wheel duty n1 ( -1000 - 1000 ).
     n2 M2  sets the right wheel duty n2 ( -1000 - 1000 ).

     .duty  displays current duty values.

     D1  toggles right wheel direction. (Default is forward.)
     D2  toggles left wheel direction.  (Default is forward.)

     S  abruptly stops bot motion.

     ESTART  executes the wheel encoder drivers in cog 5.

     .enc  displays current encoder counts (64/rotation).  
     zenc  resets encoder counters to 0.

     PSTART  executes ping and standard servo drivers. 

     .dist  displays current ping reading in inches. 
     dist   places current ping reading on stack. 
     dist?  a constant which sets the automatic stop condition (default: 8 inches).

     n !pos  sets servo position to n degrees ( -90 to 90 ). 

     stp    parameter which identifies stop state ( FALSE if stopped ).
     restp  resets stp to TRUE. 

     R  hardware reset of system, required to reset cogs to idle state.

     START  executes MSTART, ESTART, PSTART, and sets DECIMAL mode.


    SIMPLE MOTIONS 

     Robot speeds are indexed in 3 calibration tables: left servo, +right servo,
     and -right servo duties. Tables are referenced to the left servo. The
     following commands are executed n <cmd> where n is an index 0 -9.  

     F  go forward   B  go backward   RR rotate right   RL rotate left
     TR turn right   TL turn left  

     n1 n2 GO  causes robot to move in a direction and speed determined by the
               indexes n1 (left wheel) and n2 (right wheel). Negative values are
               permitted - a negative index defines a backwards motion. 
               Range: -9 to 9.

    DEFINED MOTIONS     

     ci      places the current index on the stack.
     n !spd  stores the index n in ci (default: 4).

     n RAMP  ramps up speed to maximum specified by the index n and stores n in ci.
     H  (HALT) stepwise reduction in speed to stopped state.

     The following commands run at a speed determined by ci.

     n GF  goes forward n inches.
     n GB  goes backward n inches.

     n GOF goes forward for n seconds.
     n GOB goes backward for n seconds.

     R180  rotates right for 180 degrees.

     The following commands turn the bot left/right for the indicated degrees.
       TL15 TR15 TL45 TR45 TL90 TR90  
    
    MANEUVERS are motions which are controlled by ping sensed distances to objects
    in the bot's path.  Applications use maneuvers to produce autonomous control.

      VR  bot veers right to avoid an object sensed in its path.
      VL  bot veers left.

      AVOID  prioritizes various actions the bot may take if in a (or potential) 
             stopped state.

    UTILITIES
             
     TO  allows the user to change constant vaules without recompilation:
         e.g.,  6 TO dist? changes value of dist? from 8 to 6.

      The following words allow the user to change calibration table values.
         !li  !ri  !vi   ( new_value index <cmd> )

      NOTE:  Any changes made in parameters must be done before START is executed.
             Changes are permanent only if BACKUP is execeuted. DO NOT BACKUP if
             the idle cogs have been assigned.     

}
 
FORGET BOEPAB.fth

TACHYON

[~


pub  BOEPAB.fth   ." TACHYON Robot Control for BOE Platform & Prop Activity Board "   ;

{
 Motor drivers and utilities.
}

#P12 2 MASKS  ==  pwmpins

#1495 #80 * == ld0  \ sets zero position of left servo

#1504 #80 * == rd0  \ sets zero position of right servo

LONG lduty, rduty

pub .duty ( -- ) \ displays current right & left duties
    lduty @ . SPACE rduty @ . CR ;

pub M1 ( duty -- ) \ sets left servo duty: -1000 - 1000
  #10 * ld0 + lduty !  ;
	
pub M2 ( duty -- ) \ sets right servo duty: -1000 - 1000
  NEGATE #10 * rd0 + rduty !  ;

pub S ( -- ) \ abruptly stops bot motion 
    0 M1  0 M2  ;

pub D1 ( -- )  \ toggles left wheel direction
    lduty @ ld0 - NEGATE ld0 + lduty ! ;

pub D2 ( -- )  \ toggles right wheel direction
    rduty @ rd0 - NEGATE rd0 + rduty ! ;

pri rc_motors ( -- ) \ left and right servo pwm drivers
    pwmpins OUTCLR  S
    A NCO #P12 APIN  1 FRQ 0 PHSA COG!
    B NCO #P13 APIN  1 FRQ 0 PHSB COG!    
    BEGIN
      lduty @ NEGATE PHSA COG!
      rduty @ NEGATE PHSB COG!

      20 ms
    0 UNTIL  ;

pub MSTART  ( -- ) \ executes servo drivers in a cog
    ' rc_motors TASK? RUN  50 ms  ;

 
      
{
  Encoders record current total counts per wheel. 
}

#P14  2 MASKS  ==  encpins

LONG lenc, renc, xl, xr

pub .enc ( -- ) \ lists current  encoder counter values
    lenc @  . SPACE  renc @ . CR ;

pub zenc ( -- ) \ set left and right encoder counters to 0
    0 lenc !  0 renc !  ;

pri rc_enc ( -- ) \ simple encoder drivers based on basic stamp boebot program
    TRUE xl ! TRUE xr !  encpins INPUTS  zenc 
    BEGIN
      #P14 PIN@ DUP xl @ = IF DROP ELSE xl ! lenc ++  THEN
      #P15 PIN@ DUP xr @ = IF DROP ELSE xr ! renc ++  THEN
    0 UNTIL ; 
 
pub ESTART  ( -- ) \ executes encoder drivers in a cog
     ' rc_enc TASK? RUN 50 ms ; 

{
 Ping ultrasonic distance measurement (inches).
}

#1400 == p0   \ sets standard servo 0 position
#8 == dist?   \ sets ping stop condition in inches

LONG (dist), pos

pub !pos  ( angle -- ) \ where angle is -90 - 90 degrees (right to left)
   #10 * p0 + #80 * pos !  ;

pub dist ( -- value ) \ places current ping value (inches) on stack
    (dist) @ ;

pub .dist ( -- ) \ displaces current ping value (inches)
    dist . CR  ;

BYTE (stp)  \ stores stop flag

pub restp ( -- ) \ resets stop flag to TRUE
    TRUE (stp) C!  ;

pub stp ( -- flag ) \ places stop flag on stack
    (stp) C@  ;

pri rc_ping ( -- ) \ ping driver - automatic stop condition 
   #P17 PINCLR 0 !pos
   B #P17 APIN  NCO  1 FRQ
   A 1 FRQ
   BEGIN
     #P16 PINCLR
     A #P16 APIN NCO #400 NEGATE PHSA COG!
       #1000 CTRMODE 0 PHSA COG! #P16 PININP
     pos @ NEGATE PHSB COG!
     50 ms
     PHSA COG@ #11800 / DUP (dist) !
     dist? < stp  AND IF
        FALSE (stp) C! S THEN 
   0 UNTIL  ;
  

: PSTART  ( -- ) \ executes ping driver in a cog
   restp ' rc_ping TASK? RUN 50 ms ;

{
  User parameters.
}


\ user constants - 2.625 inch wheel diameter  4.312 inch between wheels

   #8   ==   wcirc   \ wheel circumference in inches
 
  #64   ==   counts  \ number of counts per wheel rotation

   #8   ==   ctin    \ counts per inch
 
 #136   ==   rcirc  \ rotation circumfirence in 0.1 inch units

 #271   ==   tcirc  \ turn circumfirence in 0.1 inches

pub R  ( -- )  \ hardware reset - execute START to continue
    #P9 PINCLR  ;

pub START
    MSTART  ESTART  PSTART  DECIMAL ;



{
********************************************************************************
This completes the RCL Core.    

Simple (indefinite) motion commands, which use indexed tables to select duties.
}


\ Wheel Calibration Tables (left wheel used as reference)

TABLE lduty()  \ lists left wheel forward & backward duties 
   #64 , #96 , #128 , #160 , #192 , #224 , #256 , #288 , #320 , #352 , 

pri li  ( index -- value )
    2 SHL lduty() + @ ;


TABLE rduty()  \ lists calibrated right wheel forward duties 
   #60 , #93 , #126 , #158 , #189 , #221 , #253 , #282 , #314 , #345 ,

pri ri ( index -- value )
    2 SHL rduty() + @ ;

TABLE rbduty()  \ lists calibrated right wheel backward duties
   #50 , #82 , #114 , #145 , #176 , #208 , #239 , #272 , #304 , #338 ,

pri rbi ( index -- value )
    2 SHL -rduty() + @ ;


TABLE vel()  \ lists velocities in counts/10 sec for left wheel reference
   #48 , #122 , #193 , #263 , #331 , #396 , #462 , #522 , #583 , #638 , 

pri vi ( index -- value ) 
    2 SHL vel() + @ ;


pri ck ( index -- index ) \ ensures that maximum index entered is 9
  ABS DUP #9 > IF DROP #9 THEN  ;

\ ( n -- ) where n is the speed index no. ( 0 - 9 )

pub F  \ go forward
  ck DUP li M1 ri M2  ;

pub B  \ go backward
  ck DUP li NEGATE M1 -ri NEGATE M2  ;

pub RL  \ rotate in place counter-clockwise
  ck DUP li NEGATE M1  ri M2 ;

pub RR  \ rotate in place clockwise
  ck DUP li M1 -ri NEGATE M2 ;

pub TR  \ turn right
  ck li M1 0 M2  ;

pub TL  \ turn left
  ck 0 M1 ri M2 ;

pub GO  ( n1 n2 -- ) \ n1 (left index) and n2 (right index) may have values -9 to 9 
  DUP 0< IF ABS -ri NEGATE  ELSE ABS ri THEN M2
  DUP 0< IF ABS li NEGATE  ELSE ABS li THEN M1  ;

{
 Definite (automatically halt) motion commands. 
}

BYTE (ci)  \ current speed index (0 - 9)

LONG X     \ for local storage of encoder value

pub ci ( -- index ) \ places current speed index on stack
  (ci) C@ ;


pub !spd  ( index -- ) \ sets current speed index
  ck (ci) C! ;

#4 !spd  \ sets default speed index


pub RAMP ( index -- ) \ ramps up speed to index maximum and stores index
  DUP !spd 0 DO I F 100 ms LOOP  ;

pub H  ( -- ) \ stepwise reduction in speed from current index to 0
  ci 0 DO ci I - F 50 ms LOOP S ;
    

\ In the following two functions, motion continues until the right (left)
\ encoder count excedes the value stored in X. 

pri ckRENC  ( -- ) 
  BEGIN renc @ X @ => UNTIL S ;

pri ckLENC  ( -- )
  BEGIN lenc @ X @ => UNTIL S ;

pri !X ( cnts -- index ) \ stores desired encoder value in X & places current speed index on the stack
  X ! ci zenc ;

pub GF ( n -- ) \ go forward n inches
  ctin * !X F ckRENC  ;

pub GB ( n -- ) \ go backward n inches
  ctin * !X B ckRENC  ;


pub GOF ( n -- )  \ go forward for n seconds
  ci F #1000 * ms S ;

pub GOB ( n -- )  \ go backward for n seconds
  ci B #1000 * ms S ;






rcirc ctin * #20 /ROUND == c180

pub R180 ( -- )  \ rotate right 180 degrees
  c180 !X RR ckRENC  ;


tcirc ctin * #40 /ROUND == c90

c90 2 /ROUND == c45

c45 3 /ROUND == c15

pub TR15
  c15 !X TR ckLENC  ;

pub TL15
  c15 !X TL ckRENC  ;

pub TR45
  c45 !X TR ckLENC  ;

pub TL45
  c45 !X TL ckRENC  ;
  
pub TR90
  c90 !X TR ckLENC  ;

pub TL90
  c90 !X TL ckRENC  ;



{
Some maneuver examples.
}

: APPS ;

\ The following words form the basis of avoidance maneuvers.

BYTE (pry)   \ priority number

pri !pry  ( n -- ) \ set current priority number
  (pry) C! ;

pri pry ( -- n )  \ places current priority number on stack 
  (pry) C@ ;
   
pri ckPG  ( -- flag ) \ true if current ping distance is < 2* reference
  dist dist? 2* <  ;


  


\ Use the veer commands only if the bot is moving forward.
\ Veering occurs only if the bot 'sees' an obstacle less than 2* the reference
\ distance ( default: 8 inches).

pub VR ( -- )  \ veer right 
  ci ri BEGIN 2- DUP M1 100 ms ckPG 0= UNTIL DROP ci F ; 

pub VL ( -- )  \ veer left 
  ci li BEGIN 2- DUP M2 100 ms ckPG 0= UNTIL DROP ci F ;

\ In this example, only three actions are allowed if the robot stops due to
\ an obstacle in its path.

pri ACT1 ( -- ) \ 
  #45 !pos 250 ms ckPG IF 0 !pos TL45 ci F ELSE 1 !pry THEN ;

pri ACT2
  -#45 !pos 250 ms ckPG IF 0 !pos TR45 ci F ELSE 2 !pry THEN  ;

pri ACT3
  R180 0 !pry ci F  ;

\ 'AVOID' is an example of a lookup table which enables priority selection
\ of different actions. There is no action if a number = or > 6 is entered.
\ The table can be extended if the robot is allowed a greater no of choices.
\ Note that both example applications use the same table.

pri AVOID ( n -- ) \ where n is the priority number
  6 LOOKUP NOOP ACT1 ACT2 R180 VL VR ; 

 
 
\ An example of an avoidance application based on the internal stop mechanism.
\ Timeout (halts) after a user specified time in sec.

pub APP1 ( sec -- ) 
  #1000 * 0 TIMEOUT 0 !pry ci F  BEGIN  stp 0= IF #6 GB restp pry AVOID  THEN 
  0 TIMEOUT? UNTIL H ;
  
\ An example of an avoidance application using the veer commands. 
\ Halts when ESC key is pressed. Randomly selects right or left veers.

pub APP2  (  -- )
  ci F BEGIN  ckPG IF #5 #3 GETRND AVOID THEN ESC? IF EXIT THEN AGAIN H ;  
  

]~

END
 
{
 Utilities not needed as part of the RCL, but useful to debug & make changes in
calibration tables or constants without recompilation. Discarded when not needed.
}


: UTILITIES ;


: TO  ( value -- <name> ) \ change value of constant <name>, e.g., #6 TO dist?
   ' 2+ ! ;

: !li ( value index -- ) \ change ith (0-9) value in left duty table 
    2 SHL lduty() + ! ;

: !ri ( value index -- ) \ change ith (0-9) value in right (forward) duty table
  2 SHL rduty() + ! ;

: !rbi ( value index -- ) \ change ith (0-9) value in right (backward) duty table
  2 SHL rduty() + ! ;

: !vi ( value index -- ) \ change ith (0-9) value in velocity table
    2 SHL vel() + ! ;

: test ( duty -- )  \ displays left and right encoder values after ten seconds
    DUP M1 M2 zenc #10000 ms S .enc CR  ;