Simple One-Step Move

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 #2 : SIMPLE ONE STEP MOVE

; SIMPLE ONE STEP MOVE
; *******************************************
; *                                         *
; *          Simple One-Step Move           *
; *                                         *
; *******************************************

; 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:

; Init the VPIA CHIP

        move.l #SERVO_OUTPUTPINS, r0     ; Pin directions
        move.l #VPIA_SERVO_DIRECTION,r1  ; Load address of VPIA direction register
        move.l r0,(r1)                   ; Put data in the direction register
        
; Put the servo engine on

        move.l #VPIA_SERVO_DATA,r13      ; Load address of VPIA data register
        move.l  (r13),r0                 ; Get the value in the data register
        movex.b #SERVO_XPOWER,r1         ; Value of the bit for Power of servo X.
        xor.l   r1,r0                    ; Invert value of the bit.
        move.l  r0,(r13)                 ; Store it to the register to put the servo on.
   
; Wait rotary encoder sensor change

        move.l (r13), r14                ; Get the value in the data register.
        move.l #SERVO_INPUTPINS,r0       ; Bits of the motion sensor pins.
        and.l  r0,r14                    ; Isolate the values of the sensor pins.
Loop:   move.l (r13),r1                  ; Get the value of the data register.
        and.l r0,r1                      ; Isolate the values of the sensor pins.
        cmp.l r1,r14
        beq Loop

; Put the servo engine off

        move.l  (r13),r0                 ; Get the value in the data register.
        move.l  #SERVO_XPOWER,r1         ; Bit for servo x power.
        or.l    r1,r0                    ; The or function will force the bit to 1.
        move.l  r0,(r13)                 ; Write new value to register to stop servo.

; Stop execution at the end of each loop

End:    brk                              ; Stop execution and wait for user to run again.
        bra Start;                       ; If user relaunch, go to start.