Forward and Back

A simple one step move.

The robot will move one step each time the program is run.

This example code show a realistic example on how a controller in a real world robot would handle a move including the feedback management.

Listing #3 : FORWARD AND BACK

; FORWARD AND BACK
; *******************************************
; *                                         *
; *          FORWARD AND BACK               *
; *                                         *
; *******************************************

; Definitions for the Servo VPIA registers

VPIA_SERVO_DATA      = 0x80000000
VPIA_SERVO_DIRECTION = 0x80000004

; SERVO BIT ASSIGNMENT

SERVO_XPOWER     = 1
SERVO_XDIRECTION = 2
SERVO_YPOWER     = 4
SERVO_YDIRECTION = 8
SERVO_ZPOWER     = 16
SERVO_ZDIRECTION = 32
SERVO_X_RETURN   = 64
SERVO_Y_RETURN   = 128
SERVO_Z_RETURN   = 256

SERVO_INPUTPINS  = SERVO_X_RETURN | SERVO_Y_RETURN | SERVO_Z_RETURN
SERVO_OUTPUTPINS = SERVO_XPOWER | SERVO_XDIRECTION | SERVO_YPOWER 
                   | SERVO_YDIRECTION | SERVO_ZPOWER | SERVO_ZDIRECTION

; Starting address

        .org 0

Start:
        ; Call the VPIA init subroutine

        move.l #InitPIA,r1 ; Load address of the subroutine
        jsr (r1)            ; Call it

        ; Loops for square and sides of the square
        
        movex.n #0,r0       ; Direction counter.
        movex.n #2,r3       ; Working value.
Loop1:  movex.n #5,r2       ; Number of steps for a side.
Loop2:  move.l #Step,r1     ; Load address of the Step subroutine
        jsr(r1)             ; Call subroutine to make robot moving
        dec.l #1,r2         ; Decrement remaining side step counter.
        bne Loop2           ; If not zero, continue with this edge.
        xor.l r3,r0         ; Change the direction
        bra Loop1           ; Loop forever.

; *** Step subroutine, perform one step in the direction indicated
;     by the r0 register.

Step:
        pushregs r0-r1,r8,r13       ; Save registers used in this subroutine.
        move.l #VPIA_SERVO_DATA,r13 ; Load address of the VPIA data register.       
        move.l #Table,r1            ; Load address of combination table
        add.l  r0,r0                ; Multiply 2x (Asl was buggy in 1.39)
        add.l  r0,r0                ; Again.
        add.l  r0,r1                ; Get offset to value in the table.
        move.l (r1),r8              ; Load right value from the table.
        move.l r8,(r13)             ; Activate servo

        move.l #WaitMoved,r0        ; Load address of the waiting routine
        jsr (r0)                    ; Wait until the robot has moved.

        move.l #0xFFFFFFFF,r0       ; Load the value for stopping servo
        move.l r0,(r13)             ; Stop servo by putting it in VPIA register.
        popregs r0-r1,r8,r13        ; Restore registers used in this subtoutine.
        rts                         ; Return to caller.

; *** Init the VPIA CHIP subroutine

InitPIA:pushregs r0-r1
        move.l #SERVO_OUTPUTPINS, r0     ; Pin directions
        move.l #VPIA_SERVO_DIRECTION,r1  ; 
        move.l r0,(r1)
        popregs r0-r1
        rts
        
; *** Wait Moved subroutine : wait until the robot has moved.
          
WaitMoved:pushregs r0-r1,r13-r14

        ; Save the state of the rotary sensors

        move.l #VPIA_SERVO_DATA,r13 
        move.l (r13), r14
        move.l #SERVO_INPUTPINS,r0
        and.l  r0,r14
   
        ; Wait rotary encoder sensor change

        move.l #SERVO_INPUTPINS,r0
Loop:   move.l (r13),r1
        and.l r0,r1
        cmp.l r1,r14
        beq Loop
          
        popregs r0-r1,r13-r14
        rts

; *** Data table containing the right VPIA values for the various directions.

Table: .data.l ( 0xFFFFFFFF ^ 1, 0xFFFFFFFF ^ 48, 0xFFFFFFFF ^ 3, 0xFFFFFFFF ^ 16 )