Preamble from Somewhere, Yada Yada

Advanced Development and Demonstration of 3 DOF Parallel Architecture Joint for Key In-Space Applications

Project Status Report

Submitted by:

Stephen Canfield, Professor

Department of Mechanical Engineering

Tennessee Technological University

Cookeville, TN 38501

931-372-6359

Submitted to

Kirk Sorensen

Marshall Space Flight Center

Date:

November 25, 2007

This status report will describe developments in activities 1 & 2 as contained in the following sections. (section 1 begins on pg. 2, section 2 begins on pg. 7)

Section 1: Progress on Activity 1:

The first objective will provide the much needed enhancement of joint operation through the design and implementation of a capable control system. This control system will significantly improve overall performance of the gimbal manipulator, and is seen as a necessary component in its development. This control system will be designed based on current state of art in typical robot manipulators, providing real-time trajectory control with along with robust disturbance rejection. The controller will be designed as a joint-level scheme that is then integrated directly with the inverse kinematics to provide good performance over tool-space trajectories and motion planning. The primary tasks for this objective are as follows;

Real time control of the 3dof parallel architecture wrist manipulator under real-time trajectory generation

1. Overview:

Control of the 3 dof parallel architecture wrist (PAW) can take several forms: set point tracking, pre-determined trajectory specification or real-time trajectory generation. Conditions and examples of these forms are described as follows:

1.1 Set point tracking: In this command situation, the PAW is at one condition and desired to track to a second desired state, usually one with zero velocity and acceleration conditions. A typical application would be to point to a desired pose (position and orientation) and maintain that pose for a period of time.

1.2 Pre-determined trajectory specification: In this situation, path or trajectory is planned out apriori. Prescribed conditions on that path are specified and sufficient time is available to conduct inverse kinematic and dynamic analysis to determine an appropriate trajectory. This time-based trajectory is then stored in joint space configuration and recalled when operating a robotic system. A typical application is in training a manufacturing robot to perform in known environment.

1.3 Real-time trajectory generation: This third situation considers the case in which trajectory information is supplied to the manipulator in real-time. This information could come from human control (for example in a master-slave arrangement), or from a third-party navigation and control computer system, for example in a satellite guidance system or autonomous vehicle. Real-time events drive the trajectory planning such that the trajectory is dynamic and occurring at some fixed bandwidth. The manipulator’s job is to track this real-time trajectory in the presence of external disturbances.

2. Present Three Real-Time Trajectory Generation Schemes:

The third case of real-time trajectory generation is the expected mode of operation for the PAW and will be discussed more thoroughly here. First, consider a general desired trajectory for the manipulator as shown in figure 1. This trajectory consists of a set (perhaps two) goal conditions, the initial and final conditions. It also consists of a series of intermediate conditions called via points. These via points may be generated in real-time.

The motion is desired to be continuous with finite accelerations (and ideally finite jerk). The general approach is to satisfy the trajectory as a spline consisting of a series of segmented trajectories with matching boundary conditions. The segments most generally used are cubic splines, quintic splines, and linear segments with parabolic blends. Each of these methods is discussed briefly here:

2.1 cubic splines: The joint motion is defined as a cubic polynomial,

The cubic spline permits four boundary conditions, generally taken as the boundary conditions on joint position and velocity as:

The coefficients of the cubic are solved as:

The cubic polynomials match boundary conditions through velocity and guarantee finite acceleration but not finite jerk.

2.2 Quintic splines: To include boundary conditions on position, velocity and acceleration, a fifth-order polynomial can be used:

The boundary conditions are given as:

and the coefficients of the quintic polynomial are solved as

2.3 Linear splines with parabolic blends: These represent simplistic segments as a combination of quadratic and linear displacement functions (and corresponding constant and zero acceleration functions). These permit simplistic control for minimizing accelerations but do not permit velocity control or limit jerk.

3. Selected Real-Time Trajectory Generation Schemes:

For the initial implementation of the PAW prototype tracking a real-time trajectory (for example from a joystick), the cubic spline scheme will be used. The initial state is given and manipulator startup, while all future states are defined by via points which are generated in real-time by the input device (for example a joystick). The via points are generated at a rate or frequency defined by the rate at which navigation commands are updated, called wnav (Tnav as time between via points). This time is shown on figure 2. The current desired state of the manipulator is accessed by the manipulator controller at a rate of approx. 300-400 Hz (considered real time for most robotic applications). This rate is called wRTC (TRTC between updated control signals).

In this scheme, the manipulator controller will first generate the cubic polynomial (calculate coefficients) for the upcoming Tnav time period, and then calculate desired position and velocity information every TRTC seconds for the control module. When a new via point is received, the current manipulator position is taken as the starting state and the new position the ending state for an upcoming segment. In this manner, the manipulator lag time should be limited to Tnav.

Section II: Progress on Activity 2:

The second objective will incorporate all joint control algorithms into the embedded microcontroller. This will eliminate the current need for a high-level computer (laptop) in any of the demos, and will demonstrate how the joint (both mechanical hardware and all control algorithms) can be delivered as a single unit (controller embedded in the system). As the next step up the TRL ladder, the microcontroller hardware could be selected as ready for an in-space environment. Further, this would result in a system ready to interface with other flight control hardware.

Developing embedded control system for the PAW joint:

Two methods of code writing are available to us: a low level language (Assembly) and a high level language (C/C++). Some benefits of programming in Assembly are typically faster more streamlined code execution and a smaller end code footprint. However, doing more sophisticated operations such as working with fractional numbers, trigonometric functions, or matrices are rather unintuitive. This is when it becomes more practical to write the program code in C and take advantage of the compiler to handle the advanced operations.

For the case of the wrist kinematics, it is desired that the entire kinematics reside onboard the microcontroller. For this task, it is wise to choose to program in the C environment to make easy use of the routines already developed for the wrist and to allow the compiler to handle the conversions of the complicated math functions that are needed.

The board we have chosen to embed into the wrist system is the MiniDRAGON+. This board employs the Motorola MC9S12DP256 microcontroller chip with 12 KB of RAM and 256 KB of FLASH memory and is shown here.

For development purposes we are using the DRAGON12 development board pictured below.

This board utilizes the same hardware as the board we will embed, but offers a few output options that make debugging more streamlined process (i.e. the 2 line LCD display and the 7 segment displays).

For the previous version of the wrist controller, we programmed in Assembly. This prevented us from easily incorporating suitable PID control algorithms on board the microcontroller. In addition, we needed a PC to communicate with the controller real time in order to calculate the joint kinematics and take input from the user. The entire Assembly code from this version is attached at the end of this document.

For the upgrade we are performing here, the entire control code for the wrist will be composed in C and loaded entirely on the microcontroller eliminating the need for a separate PC. To begin the process, we need the ability to flash the any code developed on to the microcontroller. The previous HC12 motorola boards were flashed in a manner requiring the use of a separate microcontroller unit. This allowed for completely erasing the contents of the chip’s FLASH memory for programming. The newer edition boards have the capability to be programmed independent of an external board. By utilizing the onboard Bootloader on the controller, any developed code can be uploaded straight from a simple terminal program on a PC.

The procedure first involves writing the compatible C code. This is accomplished through the use of Freescale CodeWarrior. Programming in C for the MC9S12 is not very different from standard programming other than matching up appropriate registers that are special to the microcontroller. Below is a short code sample containing startup code for the microcontroller.

#include <hidef.h> /* common defines and macros */

#include <mc9s12dp256.h> /* derivative information */

#include <stdio.h>

#include <math.h>

#include "lcd.h"

#include "kinematics.h"

#pragma LINK_INFO DERIVATIVE "mc9s12dp256b"

#define pi 3.14159265

#define dtr 0.01745329

// Function Declarations

__interrupt void RealTimeInterrupt( void );

__interrupt void TC1Interrupt( void );

void main(void) {

float aa = 0;//, thetas[3];

long i = 0;

char string [17];

short funError;

/* Create a look-up table for the 7-segment display digits*/

unsigned char digits_array[10] = { 0x3F, /* %00111111 */

0x06, /* %00000110 */

0x5B, /* %01011011 */

0x4F, /* %01001111 */

0x66, /* %01100110 */

0x6D, /* %01101101 */

0x7C, /* %01111100 */

0x07, /* %00000111 */

0x7F, /* %01111111 */

0x6F };/* %01101111 */

/* put your own code here */

EnableInterrupts;

RTICTL = 0x50; /* 1/(4MHz/2^14) = 4.096 ms per RTI */

CRGINT |= 0x80; /* enable RTI interrupt */

/* Enable the LCD display */

// LCD_init();

DDRB = 0xff;

PORTB = 0x00;

DDRH = 0x00;

DDRP = 0xff;

PORTB = 0x00;

PTP = 0x0E;

PORTB = digits_array[0];

i = 1;

for(;;) {} /* wait forever */

/* please make sure that you never leave this function */

}

__interrupt void RealTimeInterrupt( void ) {

CRGFLG = 0x80; /* clear rti flag */

}

__interrupt void TC1Interrupt( void ) {

TFLG1 = 0x02; /* clear TC1 flag */

}

This code setups up the controller and initializes the LCD display and interrupt vectors for the platform. From this beginning further code is easily incorporated to perform more advanced functions (i.e. wrist kinematics).

CodeWarrior will compile and assemble the C code into an S record file (.S19 extension). This file can be written directly to RAM via the onboard debugger if desired and tested in that manor. A sample S19 file is shown here:

S1238000FE803DFD803B270E35ED31EC3169700434FB310326F2FE803FEC31270BED3118AA

S12380200A30700434F920F13DCF1100790011CC09395B105A1207C8068143000180419FCA

S1238040DA110000087F8000007FC00000000000010000000A00000064000003E80000276A

S123806010000186A0000F42400098968005F5E1003B9ACA000000000100000010000001FA

……..

S123EFC000000000000000000000000000000000000000000000000000000000000000002D

S123EFE000000000000000000000000085BF000085BA0000000000000000000000008029E1

S9030000FC

In order to write this record file to FLASH and have the microcontroller execute it every time it’s powered up, the S19 record file must be converted to a 24-bit S29 record file. A utility exists that converts the 16-bit addressed S19 to an S29 file. For demonstration, the conversion of the sample S19 shown above is shown below in its S29 format:

S2240C0000FE803DFD803B270E35ED31EC3169700434FB310326F2FE803FEC31270BED31181D

S2240C00200A30700434F920F13DCF1100790011CC09395B105A1207C8068143000180419F3D

S2240C0040DA110000087F8000007FC00000000000010000000A00000064000003E8000027DD

S2240C006010000186A0000F42400098968005F5E1003B9ACA0000000001000000100000016D

…….

S2241BEFBF000000000000000000000000000000000000000000000000000000000000000012

S2241BEFDF00000000000000000000000085BF000085BA0000000000000000000000008029C6

S9030000FC

ASSEMBLY Code From First Generation Wrist Controller

;*********************************************************************

;**********************MAIN CONTROL PROGRAM***************************

;*********************************************************************

;using bdm mode...............wrist driver PROGRAM 09-21-05

;************************************************

;************EQUATE STATEMENTS*******************

;************************************************

INCLUDE 'EQUATESB32.ASM' ;Equates for all registers

;EQUATES

RDIR EQU %00000010

LDIR EQU %00000001

LD EQU %00000001

RD EQU %00000001

;***SCI EQUATES***

RDRF EQU %00100000 ;READ REGISTER FULL FLAG

;***SPI EQUATES***

SS EQU %10000000

SPIF EQU %10000000

;***VECTOR2X EQUATES***

RESET EQU %00000010

EOC EQU %00010000

VECPC EQU %00000001

DEADLO EQU $70

DEADHI EQU $80

MIDDLE EQU $7F

;***ATD EQUATES***

ORG $FFD0

DC.W $8000

ORG $FFEE

DC.W ENCODER_INTERRUPT

ORG $FFF0

DC.W $8000

ORG $8000

NOP

MOVB #$08,$0016 ;DISABLE COP

LDS #$0A00

MOVB #%00001111,DDRA ;PORTA = OUTPUTS

MOVB #%00000000,DDRB ;PORTB = INPUTS

BSET TSCR,#%10000000 ;START CLOCK

BSET TMSK2,#%00000000 ;PRESCALE CLOCK

MOVB #$00,TIOS

MOVB #%01010101,TCTL3

MOVB #%01010101,TCTL4

MOVB #%00000001,TMSK1

;-----------------------------------------------------------------------

; Initialize the PWM

;-----------------------------------------------------------------------

MOVB #$FF,DDRP ;set port P to all output

MOVB #%00000000,PWCLK ;initialize PWCLK($40) register

MOVB #%11111111,PWPOL ;initialize PWPOL($41) register

MOVB #%00001111,PWEN ;initialize PWEN($42) register

MOVB #$FF,PWSCAL0 ;PRESCALE CLOCK S0

MOVB #$FF,PWSCAL1 ;PRESCALE CLOCK S1

MOVB #$FF,PWPER0 ;set period for YAW

MOVB #$FF,PWPER1 ;set period for PITCH1

MOVB #$FF,PWPER2 ;set period for PITCH2

MOVB #$FF,PWPER3 ;set period for PITCH3

;set duty cycle

MOVB #$00,PWDTY0 ;set duty cycle for YAW

MOVB #$00,PWDTY1 ;set duty cycle for PITCH1

MOVB #$00,PWDTY2 ;set duty cycle for PITCH2

MOVB #$00,PWDTY3 ;set duty cycle for PITCH3

;***SERIAL INIT***

MOVB #%11101111,DDRS ;Serial interface intitialization (SCI)

MOVB #$0D,SC0BDL

MOVB #%00000100,SC0CR1 ; NO PARITIY, NO LOOP MODE

MOVB #%00001100,SC0CR2 ; NO INTERUPTS, Tx AND Rx ENABLED

MOVB #%01011100,SP0CR1 ;SERIAL INITIALIZATION (SPI)

MOVB #%00000000,SP0CR2

MOVB #%00000111,SP0BR

MOVB #$80,ATDCTL2

JSR DELAYS

; MOVB #$00,ATDCTL3

; MOVB #$01,ATDCTL4

CLI

;******************************************

;************** MAIN ********************

;******************************************

MOVB #$00,DDRA

MOVB #$FF,DDRB

MOVB #$00,PORTB

MOVB #%10010000,PEAR

MOVB #%11111111,DDRE

MOVB #%00001111,PORTB

MOVW #$00A0,MAXM1

MOVW #$00A0,MAXM2

MOVW #$00A0,MAXM3

MOVW #$1900,DES1 ;INITIALIZE DESIRED VALUES

MOVW #$1900,DES2

MOVW #$1900,DES3

MOVB #$01,KP

MOVW #$1900,ENCOFFSET

;FIRST TWO BITS OF PORTB SELECT ENC

;00 = ENC1

;10 = ENC2

;01 = ENC3

;11 = ENC4

;THIRD BIT OF PORTB IS RESET FOR 2020 ALL CHIPS

;PULL BIT LOW FOR A RESET

;FOURTH BIT OF PORTB IS OUTPUT ENABLE FOR 2020

;THIS MUST BE PULLED LOW WHEN THE ENC TO BE POLLED IS SELECTED

MAIN BRCLR SC0SR1,#RDRF,NEXT ;CHECK FOR MASTER CONTROL BYTE

MOVB SC0DRL,MSTRCTRL

BIT1 BRCLR MSTRCTRL,#%00000001,BIT2

JSR READ_ENC

JSR ENCOUT

BRA NEXT

BIT2 BRCLR MSTRCTRL,#%00000010,BIT3

JSR RX_VALUES

BRA NEXT

BIT3 BRCLR MSTRCTRL,#%00000100,BIT4

JSR GAINS

BRA NEXT

BIT4 BRCLR MSTRCTRL,#%00001000,BIT5

JSR MOTSPEEDS

BRA NEXT

BIT5 BRCLR MSTRCTRL,#%00010000,NEXT

JSR SETENCODER

NEXT JSR DRIVE

BRA MAIN

;******************************************

;******** CYCLE THROUGH ENC READS *********

;******************************************

READ_ENC ;ENC1

MOVB #%00000100,PORTB ;GRAB HI BYTE FROM 2020

LDAA PORTA

MOVB #%00001100,PORTB ;GRAB LOW BYTE FROM 2020

LDAB PORTA

ADDD ENCOFFSET

STD ENC1BYTES

;ENC2

MOVB #%00000110,PORTB ;GRAB HI BYTE FROM 2020

LDAA PORTA

MOVB #%00001110,PORTB ;GRAB LOW BYTE FROM 2020