fl { Rover PropForth 5.5(DevKernel) 2020/04/27 22:04:53 Forward TB6612 TB6612 -------- Motor -------- Motor -------- | | Power-|VM PWMA|-- P0 Power-|VM PWMA|-- P6 |M4 M2| 3V3-|Vcc AIN2|-- P1 3V3-|Vcc AIN2|-- P7 | | |GND AIN1|-- P2 |GND AIN1|-- P8 Left| |Right M3--|A01 STBY|-- 3V3 M1--|A01 STBY|-- 3V3 | | M3--|A02 BIN1|-- P3 M1--|A02 BIN1|-- P9 |M3 M1| M4--|B01 BIN2|-- P4 M2--|B01 BIN2|-- P10 | | M4--|B02 PWMB|-- P5 M2--|B02 PWMB|-- P11 -------- |GND GND| |GND GND| Reverse -------- -------- IRdetector(VS1838) --- |Vcc|------3V3 |Out|------P12 |GND|------GND --- IR controller(NEC format type) Wave-form of Ir-controller(for DVD player) -------- -------------- -------------------------------- | | | | | | | IR code | 40msec | | 96msec | | -------------- -- -- 70msec Repeat Repeat code code } : 4WD ; \ -- variables -- variable pwmL variable pwmR wvariable IRcode wvariable rpcode \ -- constants -- h1F8 constant ctra h1F9 constant ctrb h1FA constant frqa h1FB constant frqb h1FC constant phsa h1FD constant phsb \ pin 0 wconstant P0 1 wconstant P1 2 wconstant P2 3 wconstant P3 4 wconstant P4 5 wconstant P5 6 wconstant P6 7 wconstant P7 8 wconstant P8 9 wconstant P9 d10 wconstant P10 d11 wconstant P11 d12 wconstant P12 \ movement pattern \ h50A wconstant forward h50A wconstant reverse \ h294 wconstant reverse h294 wconstant forward h28A wconstant spinTurnL h514 wconstant spinTurnR d8000 constant 100usec \ pwm cycle:10kHz 100usec invert 1+ constant 100% 100% d10 / constant incremental \ IR d12 wconstant IRsensor IRsensor >m constant mIRsensor d8000000 constant 100ms d800000 constant 10ms \ LeaderCode for NEC format d674400 constant NEC15T \ 8430usec d764320 constant NEC17T \ 9554usec d314720 constant NEC7T \ 3934usec d404640 constant NEC9T \ 5058usec d89920 constant NEC2T \ 1124usec \ --------------- \ Motor \ --------------- \ Left motors \ ( -- ) : PWML h10000000 P0 or ctra COG! \ NCO single-end for counterA 1 frqa COG! h10000000 P5 or ctrb COG! \ NCO single-end for counterB 1 frqb COG! P0 pinout P5 pinout 100usec cnt COG@ + begin pwmL L@ dup phsa COG! phsb COG! 100usec waitcnt 0 until ; \ Right motors \ ( -- ) : PWMR h10000000 P6 or ctra COG! \ NCO single-end for counterA 1 frqa COG! h10000000 P11 or ctrb COG! \ NCO single-end for counterB 1 frqb COG! P6 pinout P11 pinout 100usec cnt COG@ + begin pwmR L@ dup phsa COG! phsb COG! 100usec waitcnt 0 until ; { \ 100% dup pwmL L! pwmR L! : test1 h79E dira COG! \ Set pin for direction to output forward outa COG! c" PWML" 0 cogx c" PWMR" 1 cogx begin fkey? swap drop until 0 cogreset 1 cogreset ; : test2 h79E dira COG! \ Set pin for direction to output reverse outa COG! c" PWML" 0 cogx c" PWMR" 1 cogx begin fkey? swap drop until 0 cogreset 1 cogreset ; h28A wconstant spinTurnL : test3 spinTurnL outa COG! c" PWML" 0 cogx c" PWMR" 1 cogx begin fkey? swap drop until 0 cogreset 1 cogreset ; : test4 spinTurnR outa COG! c" PWML" 0 cogx c" PWMR" 1 cogx begin fkey? swap drop until 0 cogreset 1 cogreset ; } \ --------------- \ 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 rpcode/IRcode if Hi-state keep 100msec) \ ( -- ) : watchIR cnt COG@ begin mIRsensor ina COG@ and \ Check rise-down of IR-signal if \ IR-signal:Hi state 0 phsb COG! cnt COG@ over - 100ms > if drop cnt COG@ 0 rpcode W! 0 IRcode W! \ Clear rpcode/IRcode when no IRsignal 0 else 0 then else \ IR-signal:Lo state drop 1 then until ; \ Save pulse-count in buffer \ ( -- n1 ) n1:last address+4 for saved pulse count(phsa/phsb) : pulseCount \ Measure pulse-width here W@ alignl begin \ Lo count 0 phsa COG! waitUntilHi phsb COG@ over L! \ Get Lo-part 0 phsb COG! 4+ \ address+4 \ Hi count begin mIRsensor ina COG@ and \ Check if Hi if \ Check time-out phsa COG@ 10ms > if 1 1 else 0 then else phsa COG@ over L! 4+ \ address+4 0 1 then until until ; \ Analize receiving IR-signal \ ( n1 -- ) n1:last address+4 for saved pulse count(phsa/phsb) : analizeIR here W@ alignl 4+ L@ here W@ alignl L@ \ ( last_address LeaderCode(Hi) LeaderCode(Lo) ) NEC15T NEC17T between \ ( last_address LeaderCode(Hi) 1/0 ) if \ ." NEC format" cr NEC7T NEC9T between \ ( last_address 1/0 ) if 0 swap \ ( 0 last_address ) \ Check Hi-time because of 1/0 judgement \ Ignore first 3long(Leader-code and first Hi-time) and last 1long(Hi-time:stop-signal) here W@ alignl - d16 - here W@ alignl d12 + swap bounds do 1 rshift i L@ NEC2T > if h80000000 or then 8 +loop \ b7........b0 b7..b0 b15......b0 \ invertedData Data CustomCode hFF0000 and d16 rshift IRcode W! else drop 1 rpcode W+! rpcode W@ d10 > if d10 rpcode W! then then else 2drop then ; \ Check IR \ ( -- ) : irCheck h20000000 IRsensor or ctra COG! \ POS detect 1 frqa COG! h30000000 IRsensor or ctrb COG! \ NEG detect 1 frqb COG! begin watchIR pulseCount analizeIR 0 until ; \ --------------- \ Main \ --------------- \ Drive Rover to forward \ ( -- ) : REVERSE reverse outa COG! 100% dup pwmL L! pwmR L! ; : FORWARD forward outa COG! 100% dup pwmL L! pwmR L! ; : RIGHT 100% pwmL L! 100% rpcode W@ incremental * - pwmR L! ; : LEFT 100% pwmR L! 100% rpcode W@ incremental * - pwmL L! ; : STOP hFFFFFFFF dup pwmL L! pwmR L! ; : RIGHTTURN spinTurnR outa COG! 100% dup pwmL L! pwmR L! ; : LEFTTURN spinTurnL outa COG! 100% dup pwmL L! pwmR L! ; : case over = ; \ Operating Rover by IR-controller \ ( -- ) : Rover h79E dira COG! \ Set pin for direction to output STOP c" PWML" 0 cogx c" PWMR" 1 cogx c" irCheck" 2 cogx begin IRcode W@ 6 case if FORWARD else h47 case if LEFT else h40 case if RIGHT else h44 case if REVERSE else h45 case if LEFTTURN else h5F case if RIGHTTURN else STOP thens drop \ fkey? swap drop 0 until 0 cogreset 1 cogreset 2 cogreset ; \ Boot after Power-on : onreset5 onreset Rover ;