\ PropForth Stingray Robot Control Vocabulary   N. G. Lordi 5/2012  V.0.8
\
\ This is the beta version of a PropForth robot control vocabulary fot the 
\ Stingray robot.  At this early stage, it only includes drivers for motors 
\ (cog 1) and three pings (cog 2).  See pfrc_doc.rft for detailed vocabulary
\ definitions.
 

fl

fswrite pfrcsr.f

: (PFRCSR) ;


\ 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 cdist
variable ldist

wvariable scale
wvariable period
variable ladj

\ pin assignments

13 wconstant RPG     \ right ping
14 wconstant CPG     \ center ping
15 wconstant LPG     \ left ping
25 wconstant LFW     \ left motor forward 
24 wconstant LBW     \ left motor backward
26 wconstant RFW     \ right motor forward
27 wconstant RBW     \ right motor backward


\ program constants (counter modes)

h_1000_0000 RPG  + constant RPG1mode
h_2000_0000 RPG  + constant RPG2mode
h_1000_0000 CPG  + constant CPG1mode
h_2000_0000 CPG  + constant CPG2mode
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

\ parameter initialization

: set_defaults
     100 scale W!  4000 period W!  0 ladj L!  0 lduty L!  0 rduty L!  ;

: set_pins 
     LFW dup pinlo pinout  LBW dup pinlo pinout 
     RFW dup pinlo pinout  RBW dup pinlo pinout ;  


\ motor control command

: set_duty
   ladj L@ + scale W@ * lduty L!  scale W@ * rduty L!  ;

\ wheel and ping drivers

: rc_wheel    
    set_pins
    1 frqa COG!  1 frqb COG!     
    begin
      period W@ cnt COG@ + 0 waitcnt drop
      lduty L@ abs negate phsa COG!
      rduty L@ abs negate phsb COG!
      lduty L@ 0> if LFWmode ctra COG! else LBWmode ctra COG!  then
      rduty L@ 0> if RFWmode ctrb COG! else RBWmode ctrb COG!  then       
    0 until  ; 

: rc_ping
    1 frqa COG!
    begin
      RPG pinout RPG1mode ctra COG!
      -400 phsa COG!
      RPG pinin   RPG2mode ctra COG!
      0 phsa COG!  50 delms
      phsa COG@ 11800 / rdist L!
      
      CPG pinout CPG1mode ctra COG!
      -400 phsa COG!
      CPG pinin   CPG2mode ctra COG!
      0 phsa COG!  50 delms
      phsa COG@ 11800 / cdist L!

      LPG pinout LPG1mode ctra COG!
      -400 phsa COG!
      LPG pinin   LPG2mode ctra COG!
      0 phsa COG!  50 delms
      phsa COG@ 11800 / ldist L!
    0 until  ;

     
\ initialize and start drivers

: Wstart 1 cogreset  20 delms  c" rc_wheel" 1 cogx  10 delms ;

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

: START  set_defaults  Wstart  Pstart ; 

\ basic robot motion command set

: STOP
    5 0 do 
      lduty L@ 2/ lduty L!  rduty L@ 2/ rduty L!  100 delms loop
    0 0 set_duty  ;

: FRW 
    dup set_duty  ;

: BKW 
    negate dup set_duty ;

: CW
    dup negate swap set_duty  ;

: CCW
    dup negate set_duty  ;

: RTURN 
    0 swap set_duty ;

: LTURN
    0 set_duty  ;

  
...