DPRG
DPRG List  



[DPRG] Isopod or isomax

Subject: [DPRG] Isopod or isomax
From: Randy M. Dumse rmd at newmicros.com
Date: Wed Dec 11 22:51:01 CST 2002

> Does anybody have any example code in isomax for an isopod?
> Something that controls a couple of servos would be very
helpful.

Complete listing for a RC Servo based robot which can do a dead
reconning quick trip. First section of code deals with hardware
servo registers.

Second section of the code adds motion commands, LFWD (left
wheel forward) LBCK (left wheel backward) LSTP (left wheel stop)
RFWD (right wheel forward) RBCK (right wheel backward) RSTP
(right wheel stop).

The last section of code is a simple state machine sequencer
that runs quick trip. States are W.F.B. (wait for button) QTINIT
(do initialization) QTDOWN (count while it runs down) QTTURN
(count while one wheel is stopped and it pivots around) QTBACK
(count while it runs back) QTENDIT (count while one wheel is
stopped and it pivots around to return to starting position).

This is basically the program Michael Zhang ran at the September
Roborama, althought his statemachine was larger, including a
similar sequence for T-Time etc.

Rather than run on about it, I'd suggest, ask questions if
there's anything in particular you would like to have clarified.

Randy
www.newmicros.com


SCRUB

HEX
0 RAM !

: HALFSPEEDCPU F413 FA1 ! 82 F00 ! ; EEWORD ( DIV BY 2

HALFSPEEDCPU ( GOTTA RUN HALFSPEED TO GET PWM DOWN TO 50HZ FOR
RC SERVO


 4E2 CONSTANT LOSTOP EEWORD ( 1 MS 1250 decimal COUNTS OF .8uS
 9C4 CONSTANT HISTOP EEWORD ( 2 MS 2500 decimal COUNTS OF .8uS
HISTOP LOSTOP + 2/ CONSTANT MIDWAY EEWORD ( 1.5 MS


: PWMLOAD E00 C@ DROP C3 E00 ! ; EEWORD ( FORCE VALUES TO PWM
HARDWARE

: INIT
  HALFSPEEDCPU
  0      FC3 ! ( ENABLE PB0 - 7 AS INPUTS
  8000   E03 ! ( PWM HARDWARE SETUP
  61A8   E05 ! ( 25000 decimal COUNTS OF .8uS FOR 20 MS UPDATE
RATE
  0      E0D ! ( PWM HARDWARE SETUP
  0      E0E ! ( PWM HARDWARE SETUP
  000E   E0F ! ( CENTER ALIGN FOR DIV BY 2 PWM HARDWARE SETUP
  MIDWAY E06 ! ( CHOSE MIDWAY SO STOPPED
  MIDWAY E07 ! ( CHOSE MIDWAY SO STOPPED
  PWMLOAD      ( START PWM OUTPUT
; EEWORD



( NEXT WORDS TO MAKE MOTION COMMANDS
: LFWD HISTOP E06 ! PWMLOAD ; EEWORD
: LBCK LOSTOP E06 ! PWMLOAD ; EEWORD
: LSTP MIDWAY E06 ! PWMLOAD ; EEWORD
: RFWD HISTOP 20 - E07 ! PWMLOAD ; EEWORD (TRIMMED TO MAKE RUN
STRAIGHT
: RBCK LOSTOP E07 ! PWMLOAD ; EEWORD
: RSTP MIDWAY E07 ! PWMLOAD ; EEWORD



: -LOOPVAR
  <BUILDS
   HERE P, 1- DUP , ,
  DOES>
    P@
    DUP @ 0=
    IF DUP 1 + @ SWAP ! TRUE ELSE 1-! FALSE THEN
; EEWORD

DECIMAL
  100 -LOOPVAR CNT EEWORD
 1500 -LOOPVAR 16FT EEWORD
  750 -LOOPVAR 8FT EEWORD
   86 -LOOPVAR 180DEG EEWORD
   43 -LOOPVAR 90DEG EEWORD
HEX

( FOLLOWS A SIMPLE STATE MACHINE SEQUENCER

MACHINE CONTEST EEWORD
 ON-MACHINE CONTEST
   APPEND-STATE W.F.B.    EEWORD ( WAIT FOR BUTTON

   APPEND-STATE QTINIT    EEWORD ( QUICK TRIP
   APPEND-STATE QTDOWN    EEWORD
   APPEND-STATE QTTURN    EEWORD
   APPEND-STATE QTBACK    EEWORD
   APPEND-STATE QTENDIT   EEWORD



IN-STATE
  W.F.B.
CONDITION
  PB7 OFF?
CAUSES
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  PB0 OFF
THEN-STATE
  QTINIT
TO-HAPPEN IN-EE


IN-STATE
  QTINIT
CONDITION
  CNT
CAUSES
  LFWD RFWD
  REDLED OFF
  YELLED OFF
  GRNLED ON
  PB0 ON
THEN-STATE
  QTDOWN
TO-HAPPEN IN-EE

IN-STATE
  QTDOWN
CONDITION
  16FT
  PB7 OFF? OR
CAUSES
  RSTP
  REDLED ON
  YELLED OFF
  GRNLED OFF
THEN-STATE
  QTTURN
TO-HAPPEN IN-EE

IN-STATE
  QTTURN
CONDITION
  180DEG
  PB7 OFF? OR
CAUSES
  LFWD RFWD
  REDLED OFF
  YELLED ON
  GRNLED OFF
THEN-STATE
  QTBACK
TO-HAPPEN IN-EE

IN-STATE
  QTBACK
CONDITION
  16FT
  PB7 OFF? OR
CAUSES
  RSTP
  REDLED ON
  YELLED OFF
  GRNLED OFF
THEN-STATE
  QTENDIT
TO-HAPPEN IN-EE

IN-STATE
  QTENDIT
CONDITION
  180DEG
  PB7 OFF? OR
CAUSES
  RSTP LSTP
  REDLED ON
  YELLED ON
  GRNLED OFF
THEN-STATE
  W.F.B.
TO-HAPPEN IN-EE



: STARTUP
  HALFSPEEDCPU
  W.F.B. SET-STATE
  INIT
  EVERY C350 CYCLES SCHEDULE-RUNS CONTEST
; EEWORD


( HEX 7C00 AUTOSTART STARTUP
( SAVE-RAM


More information about the DPRG mailing list