fl

{
PropForth5.5(DevKernel)
2020/12/23 15:50:16

RoverController

JoyStick
                  P0 VRy
                  ^
  JoyStick board  |+                          JoyStick board
          ----------------                      ----------------
         |        |       |                    |        |       |
         |        |       |                    |        |       |
        -|        |       |+                  -|        |       |+
         | -------|------ |---> P1 VRx         | -------|------ |---> P2 Rotate       
         |        |       |                    |        |       |
         |        |-      |                    |        |       |
         |   GND VRx VRy  |                    |        |       |
          ----------------                      ----------------
              |  |   |                                |  |
             GND P1  P0                              GND P2
          Cutting 5V-line of VRx and VRy
P0 -----------------------
        |    open        |
        |     |          |
         ---->VRy(10k) Capacitance(0.1uF)
              |          |
             GND        GND
             
P1 -----------------------
        |    open        |
        |     |          |
         ---->VRx(10k)  Capacitance(0.1uF)
              |          |
             GND        GND

P2 ----------------------
      |    open         |
      |     |           |
       -->Rotate(10k) Capacitance(0.1uF)
            |           |
            GND        GND

}
: RoverControler ;

\ =========================================================================== 
\ Constants 
\ =========================================================================== 
\ ========= special register ========
h1F8 wconstant ctra
h1F9 wconstant ctrb
h1FA wconstant frqa
h1FB wconstant frqb
h1FC wconstant phsa
h1FD wconstant phsb

0 wconstant VRy
1 VRy lshift constant mVRy
d67100 constant Ymax
1 wconstant VRx
1 VRx lshift constant mVRx
d67500 constant Xmax
2 wconstant Rotate
1 Rotate lshift constant mRotate
d60500 constant Rotatemax

\ =========================================================================== 
\ Variables 
\ =========================================================================== 
variable Xcenter
variable Ycenter
wvariable Xdiv
wvariable Ydiv
variable Rotatecenter
wvariable Rotatediv

\ bit array to transmit(X:+127..-127  Y:+127..-127  rotate:+127..-127)
variable bitArray 8 allot

\ =========================================================================== 
\ Main 
\ =========================================================================== 
\ Count PHSA after charging capacitance
\ Ignore phsa-counting until pin is set to input
\ ( -- n1 ) n1:phsa value
: Xcount
dira COG@ mVRy or dira COG!             \ Set pin to output
1 drop                                  \ Wait charging
0 phsa COG!                             \ Clear phsa
mVRy invert dira COG@ and dira COG!     \ Set pin to input
1 delms
phsa COG@
;

\ Count PHSB after charging capacitance
\ Ignore phsb-counting until pin is set to input
\ ( -- n1 ) n1:phsb value
: Ycount
dira COG@ mVRx or dira COG!             \ Set pin to output
1 drop                                  \ Wait charging
0 phsb COG!                             \ Clear phsa
mVRx invert dira COG@ and dira COG!     \ Set pin to input
1 delms
phsb COG@
;

\ Count PHSB after charging capacitance
\ Ignore phsb-counting until pin is set to input
\ ( -- n1 ) n1:phsb value
: Rotatecount
dira COG@ mRotate or dira COG!          \ Set pin to output
1 drop                                  \ Wait charging
0 phsa COG!                             \ Clear phsa
mRotate invert dira COG@ and dira COG!  \ Set pin to input
1 delms
phsa COG@
;

\ Limit value if more than 128, set 127 and if abs(n1) is less than 10, set 0
\ ( n1 -- n2 ) n1:compared value  n2:result
: limit
dup                   \ ( n1 n1 )
\ Check max value
abs d127 >            \ ( n1 1/0 )
if 0>                 \ ( 1/0 )
  if 
    d127              \ ( d127)
  else 
    d-127             \ ( d-127)
  then
else
  dup                 \ ( n1 n1 )
  \ Check min value
  abs d20 <           \ ( n1 1/0 )
  if drop 0 then      \ ( 0 )
then                  \ ( n1 )
;


\ Save X and T in bitArray
\ ( -- )
: XYdir
\ Initialize counter mode
h20000000 VRy or ctra COG!              \ POS detector mode
1 frqa COG!
h20000000 VRx or ctrb COG!              \ POS detector mode
1 frqb COG!
outa COG@ mVRy or mVRx or outa COG!     \ Set pin-outport to 1
\ Measure center value
d100 delms
Xcount Xcenter L! 
Xmax Xcenter L@ - d127 / Xdiv W!
Ycount Ycenter L!
Ymax Ycenter L@ - d127 / Ydiv W!

begin
  bitArray 
  Xcount Xcenter L@ - Xdiv W@ / limit over L!
  4+
  Ycount Ycenter L@ - Ydiv W@ / limit swap L!
0 until
;

\ Save rotate in bitArray
\ ( -- )
: rotate_dir
\ Initialize counter mode
h20000000 Rotate or ctra COG!         \ POS detector mode
1 frqa COG!
outa COG@ mRotate or outa COG!         \ Set pin-outport to 1
\ Measure Rotate value
d100 delms
Rotatecount Rotatecenter L! 
Rotatemax Rotatecenter L@ - d127 / Rotatediv W!

begin
  Rotatecount Rotatecenter L@ - Rotatediv W@ / limit
  bitArray 8 + L!
0 until
;


{
: tab 9 emit ;
: XMax
\ Initialize counter mode
h20000000 VRy or ctra COG!              \ POS detector mode
1 frqa COG!
0
d1000 0 do Xcount + loop
d1000 / .    
;
: YMax
\ Initialize counter mode
h20000000 VRx or ctrb COG!              \ POS detector mode
1 frqb COG!
0
d1000 0 do Ycount + loop
d1000 / .    
;
: rotateMax
\ Initialize counter mode
h20000000 Rotate or ctra COG!              \ POS detector mode
1 frqa COG!
0
d1000 0 do Rotatecount + loop
d1000 / .    
;

\ Print X,Y
\ ( -- )
: demoXY
\ Initialize counter mode
h20000000 VRy or ctra COG!              \ POS detector mode
1 frqa COG!
h20000000 VRx or ctrb COG!              \ POS detector mode
1 frqb COG!
outa COG@ mVRy or mVRx or outa COG!     \ Set -pin-outport to 1
\ Measure center value
d100 delms
Xcount Xcenter L! 
Xmax Xcenter L@ - d127 / Xdiv W!
Ycount Ycenter L!
Ymax Ycenter L@ - d127 / Ydiv W!
                                
begin
     Xcount Xcenter L@ - Xdiv W@ / . tab
     Ycount Ycenter L@ - Ydiv W@ / . cr
     d100 delms
     fkey? swap drop
until
;
\ Print rotate
: demoRotate
\ Initialize counter mode
h20000000 Rotate or ctra COG!         \ POS detector mode
1 frqa COG!
outa COG@ mRotate or outa COG!        \ Set -pin-outport to 1
\ Measure center value
d100 delms
Rotatecount st? Rotatecenter L! 
Rotatemax Rotatecenter L@ - d127 / Rotatediv W!
                                
begin
     Rotatecount Rotatecenter L@ - Rotatediv W@ / . cr
     d100 delms
     fkey? swap drop
until
;

}