\ PropForth Eddie Robot Control Vocabulary       N. G. Lordi 5/2012   V.0.8
\
\ This is the beta version of a PropForth core robot control vocabulary
\ for the Eddie robotic platform.  At this early stage, it includes 
\ drivers for motors (cog 1), three pings (cog 2)and 3 Sharp infrared 
\ distance sensors (cog 3).  See pfrc_doc.rft for detailed vocabulary 
\ definitions.


fl

fswrite pfrced.f

: (PFRCED) ;


\ 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

wvariable ladc
wvariable cadc
wvariable radc

\ pin assignments

0  wconstant RPG     \ right ping
1  wconstant CPG     \ center ping
2  wconstant LPG     \ left ping
19 wconstant LFW     \ left motor forward
20 wconstant LPW     \ left motor PWM
21 wconstant LBW     \ left motor backward
22 wconstant RFW     \ right motor forward
23 wconstant RPW     \ right motor PWM
24 wconstant RBW     \ right motor backward
25 wconstant _cs     \ M3208 enable
26 wconstant _dpin   \ M3208 data io
27 wconstant _clk    \ M3208 clock


\ 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 LPW  + constant LPWmode
h_1000_0000 RPW  + constant RPWmode

\ constants specific to Eddie

LFW >m  constant  Lfwd
LBW >m  constant  Lbkw
RFW >m  constant  Rfwd
RBW >m  constant  Rbkw

\ parameter and pin initialization 

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


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


: _cs_out_l _cs pinlo ;
: _cs_out_h _cs pinhi ;
: _dpin_out_l _dpin pinlo ;
: _dpin_out_h _dpin pinhi ;
: _clk_out_l _clk pinlo ;
: _clk_out_h _clk pinhi ;
: clk_out _clk_out_h _clk_out_l ;



\ motor control command

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

\ pasm ADC function (MCP3208)

\ ( n1 n2 n3 n4 n5 -- n6 )  
\ n1:channel number  n2:single/differential   n3:_dpin  n4:_clk  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



\ wheel, ping and adc drivers
    
: rc_wheel    
    set_pins
    1 frqa COG!  1 frqb COG! 
    LPWmode ctra COG! 
    RPWmode ctrb 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 Lfwd _maskouthi Lbkw _maskoutlo else 
          Lfwd _maskoutlo Lbkw _maskouthi  then   
      rduty L@ 0> if Rfwd _maskouthi Rbkw _maskoutlo else 
          Rfwd _maskoutlo Rbkw _maskouthi  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  ;

: rc_adc
  _cs pinout _dpin pinout  _clk pinout
  _cs_out_h
  begin 
      0 1 _clk _dpin _cs a_get_A/D 2 rshift ladc W!
      5 delms  
      1 1 _clk _dpin _cs a_get_A/D 2 rshift cadc W!
      5 delms   
      2 1 _clk _dpin _cs a_get_A/D 2 rshift radc W!
      5 delms  
  0 until ;


\ initialize and start motor, ping, & adc 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 ;

: Astart 3 cogreset 10 delms  c" rc_adc" 3 cogx 20 delms ;


: START  set_defaults  Wstart  Pstart  Astart ; 


\ basic 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  ;

  
...