fl

{
PropForth 5.5(DevKernel)

RoverC-Pro
2020/12/30 21:57:34

    IR receiver
    VS838
     Vcc ----100ohm----3V3
     GND --------------GND
     OUT --------------P1
               |
              3V3
              
  IR-Receiver output (Output is Lo when 38kHz carrier)
               b7....b0 b7....b0 b7....b0
  ----      --     
      |    |  |========|========|========|= 
       ----       
        10T 5T  1stByte  2ndbyte  3rdByte Seperation(Lo)
    Leader-code   X        Y        Rotate
    
    
                  -                   -
      bit"1"     | |         bit"0"  | |
              ---                   -
              3T  1T               1T  1T    T=500usec
}

\ =========================================================================== 
\ Constants 
\ =========================================================================== 
\ ========= special register ========
h1F8 constant ctra
h1F9 constant ctrb
h1FA constant frqa
h1FB constant frqb
h1FC constant phsa
h1FD constant phsb
\ IR
0 wconstant IRsensor
IRsensor >m constant mIRsensor
d4000000 constant 50ms
\ For searching LeaderCode 
d440000 constant 10T+a    \ 5.5msec
d360000 constant 10T-a    \ 4.6msec
d220000 constant 5T+a     \ 2.75msec
d180000 constant 5T-a     \ 2.25msec
\ 
d44000 constant 1T+a          \ 550usec
d36000 constant 1T+a          \ 450usec
d132000 constant 3T+a         \ 1.65msec
d108000 constant 3T-a         \ 1.35msec               
: RoverC-Pro1 ;
\ =========================================================================== 
\ Main
\ =========================================================================== 
\ ---------------
\     IR part 
\ ---------------
\ Wait until IRsensor goes to Hi 
\ ( -- )
: waitUntilHi mIRsensor mIRsensor waitpeq ;
\ Wait until IRsensor goes to Lo
\ ( -- )
: waitUntilLo mIRsensor mIRsensor waitpne ;      

\ Watch IR-line(Clear buffer if Hi-state keep 50msec)
\ ( -- ) 
: watchIR
\ Wait until IR-line pass 50msec under Hi-state
cnt COG@
begin
  mIRsensor ina COG@ and            \ Check rise-down of IR-signal 
  if                                \ IR-signal:Hi state
    cnt COG@ over - 50ms > 
    if                              
      drop 1
    else
      0
    then
  else
    drop cnt COG@    
    0
  then
until
;
          : tab 9 emit ;
\ Save pulse-count in buffer(here W@)
\ ( -- n1 )  n1:last address+4 for saved pulse count(phsa/phsb)     
: pulseCount
here W@ alignl
0 phsb COG!
waitUntilLo

d25 0 do
  0 phsa COG!                         \ Clear Hi-counter(phsa)
  \ Wait until IR-line is Hi
  waitUntilHi
  phsb COG@ over L!                   \ Save Lo-counter(phsb)
  4+                                  \ address+4

  0 phsb COG!                         \ Clear Lo-counter(phsb)
  \ Wait until IR-line is Lo
  waitUntilLo
  phsa COG@ over L!                   \ Save Hi-counter(phsa)
  4+                                \ address+4  
loop
;

{
: tab 9 emit ;   
\ Measure pulse-width(base:0.1T)
\ ( -- )
: test    watchIR pulseCount    st?
here W@ alignl - here W@ alignl swap   st?
bounds do i L@ d4000 / . tab i 4 + L@ d4000 / . cr 8 +loop
;
}

\ Set minus if minus value 
\ ( n1 -- n2 ) n1:checked value  n2:result
: minus? hFF and dup h80 and if hFFFFFF00 or then ;

\ Analize receiving IR-signal
\ ( n1 -- ) n1:last address+4 for saved pulse count(phsa/phsb) 
: analizeIR
  \ Check Leader-code
  here W@ alignl dup L@ swap 4+ L@            
  5T+a 5T-a between swap
  10T-a 10T+a between or                    \ ( last_address 1/0 )                                    
  if
    0 
    swap                                    \ ( 0 last_address )
    here W@ alignl - 8 - here W@ alignl 8 + swap  \ ( 0 start_address number )
    bounds do
      1 lshift
      i L@ 3T-a > if 1 or then
    8 +loop                                 \ ( 24bit )
    
    \ Skip X and Y if rotate is not zero                                    
    dup hFF and 0<>
    if
      \ Rotate                 
      minus?                                    
      dup 0> 
      if       
        rr W! 0 rl W! else invert 1+ rl W! 0 rr W! 
      then 
      0 f W! 0 b W! 0 r W! 0 l W!
    else  
      \ Y                  
      8 rshift dup minus?                        
      dup 0> 
      if 
        f W! 0 b W! else invert 1+ b W! 0 f W! 
      then
      \ X                  
      8 rshift minus?                            
      dup 0> 
      if 
        r W! 0 l W! else invert 1+ l W! 0 r W! 
      then
    then
  else
    drop                                       
  then
;

\ Move RoverC-Pro
\ ( -- )
: moveRover
sendBuffer d16 0 fill                     \ Clear buffer for motor
h20000000 IRsensor or ctra COG!           \ POS detect
1 frqa COG!
h30000000 IRsensor or ctrb COG!           \ NEG detect
1 frqb COG!                                               

begin
  watchIR            
  pulseCount      
  analizeIR      
  setSpeed driveWheel  
  d200 delms  
  \ Stop motor
  0 rr W! 0 rl W! 0 f W! 0 b W! 0 r W! 0 l W!
  setSpeed driveWheel
\  fkey? swap drop
\  until  
 0 until
;   

\ Boot after Power-on
: onreset5 onreset moveRover ;