/* Solomon Steplight and Tom Sugar */

/* Motor control with LM629 */

 

/****************************************************************************

* This code is designed for motion control using the LM629 chip for the *

* 1996-97 Gateway robot design project. This code was designed to output *

* all the necessary encoder and hexadecimal values for the motor's shaft *

* and desired position values to accurately monitor the motor's motion. *

* <<this particular variation was for multiple motor test beds>> *

****************************************************************************/

 

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <dos.h>

#include <conio.h>

 

#include <signal.h>

#include <ctype.h>

#include <string.h>

 

 

#define baseaddr 0x340

#define RESET 0x00

#define RSTI 0x1D

#define MSKI 0x1C

#define LFIL 0x1E

#define UDF 0x04

#define LTRJ 0x1F

#define STT 0x01

#define DFH 0x02

#define RDRP 0x0A

#define RDDP 0x08

#define RDRV 0x0B

#define RDDV 0x07

#define RDSIG 0x0C

 

char num [5];

long pos_val1, pos_val2, pos_val3, pos_val4, pos_val5, pos_val6;

unsigned char POSHI, POSLO, high, low;

int THETA,z,j,i;

long junk, count, position;

 

long desired_pos(int);

void wr_cmd(unsigned char, int);

unsigned char rd_st(int);

void chk_bsy(int);

void wr_data(unsigned char, unsigned char, int);

long rd_data(int);

void load_traj(int , long);

 

/* ********************************************* */

/* bit 0 CS */

/* bit 1 RD and STRB(8255) */

/* bit 2 WR */

/* bit 3 PS */

/* bit 4 ACK(8255) */

/* bit 5 RST */

 

void chk_bsy(int z)

{

int ST;

 

outp(baseaddr+(z*4+1),(0x36 & 0xff)); /* set ps and cs low */

outp(baseaddr+(z*4+1),(0x34 & 0xff)); /* set rd low */

ST = inp(baseaddr+(z*4)); /* read in value */

while (ST & 0x01) { /* check for busy bit low */

 

ST = inp(baseaddr+(z*4));

}

outp(baseaddr+(z*4+1),(0x36 & 0xff)); /* bring rd back up high */

outp(baseaddr+(z*4+1),(0x3e & 0xff)); /* keeping cs low */

}

 

unsigned char rd_st(int z)

{

unsigned char status;

 

outp(baseaddr+ (z*4+1),(0x36 & 0xff)); /* send ps and cs low */

outp(baseaddr+(z*4+1),(0x34 & 0xff)); /* send rd low */

status = (unsigned char) inp(baseaddr+(z*4)); /* read value */

outp(baseaddr+(z*4+1),(0x36 & 0xff)); /* send rd high */

outp(baseaddr+(z*4+1),(0x3e & 0xff)); /* send ps high and leave cs low */

return(status);

}

 

void wr_cmd(unsigned char CMD, int z)

{

 

outp(baseaddr+(z*4+1),0x36 & 0xff); /* cs and ps low */

outp(baseaddr+(z*4),CMD);

 

outp(baseaddr+(z*4+1),0x22 & 0xff); /* write and ack low */

chk_bsy(z);

 

outp(baseaddr+(z*4+1),0x26 & 0xff); /* bring write up */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* bring ack up and ps up at the same time */

/* outp(baseaddr+1,0x3e & 0xff); */ /* leaving cs low */

 

}

 

void wr_data(unsigned char HIdat, unsigned char LOdat, int z)

{ /* int jjunk; */

 

outp(baseaddr+(z*4+1),0x3e & 0xff); /* pull ps high and set cs low */

outp(baseaddr+(z*4), HIdat & 0xff);

outp(baseaddr+(z*4+1),0x2a & 0xff); /* send write and ack low */

chk_bsy(z);

outp(baseaddr+(z*4+1),0x2e & 0xff); /* bring the wr back up */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* bring ack up */

 

outp(baseaddr+(z*4), LOdat & 0xff);

outp(baseaddr+(z*4+1),0x2a & 0xff); /* send write and ack low */

 

chk_bsy(z);

outp(baseaddr+(z*4+1),0x2e & 0xff); /* bring the wr back up */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* bring ack up and leave cs low */

 

}

 

long rd_data(int z)

{ long lmvalue;

long dataval;

 

lmvalue = 0;

dataval = 0;

 

chk_bsy(z);

outp(baseaddr+(z*4+1),0x3e & 0xff); /* pull ps high and set cs low */

outp(baseaddr+(z*4+1),0x3c & 0xff); /* send rd low */

dataval = (long) inp(baseaddr+(z*4)); /* read high byte value */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* send rd high */

lmvalue = (dataval << 24);

 

chk_bsy(z);

outp(baseaddr+(z*4+1),0x3e & 0xff); /* pull ps high and set cs low */

outp(baseaddr+(z*4+1),0x3c & 0xff); /* send rd low */

dataval = (long) inp(baseaddr+(z*4)); /* read low byte value */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* send rd high */

lmvalue = lmvalue + (dataval << 16);

 

chk_bsy(z);

outp(baseaddr+(z*4+1),0x3e & 0xff); /* pull ps high and set cs low */

outp(baseaddr+(z*4+1),0x3c & 0xff); /* send rd low */

dataval = (long) inp(baseaddr+(z*4)); /* read high byte value */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* send rd high */

lmvalue = lmvalue + (dataval << 8);

 

chk_bsy(z);

outp(baseaddr+(z*4+1),0x3e & 0xff); /* pull ps high and set cs low */

outp(baseaddr+(z*4+1),0x3c & 0xff); /* send rd low */

dataval = (long) inp(baseaddr+(z*4)); /* read high byte value */

outp(baseaddr+(z*4+1),0x3e & 0xff); /* send rd high */

lmvalue = lmvalue + dataval;

 

return(lmvalue);

 

}

 

long desired_pos(int THETA)

{

double pos;

 

pos = (THETA * 218.42 * 500.0 * 4.0)/360.0; /* Given an angle theta, compute */

/* the number of counts (pos) the motor*/

/* should turn. 5.9 equals the gear */

/* ratio of the motor used for testing*/

 

printf("Position value to load: %f\r\n", pos);

 

return((long) pos); /* Return the new position value in */

/* terms of counts */

}

 

 

void load_traj(int z, long j)

{

unsigned char stat;

 

chk_bsy(z);

/*wr_cmd(DFH,z);*/

wr_cmd(LTRJ,z);

high = 0x00;

low = 0x2b;

chk_bsy(z);

 

chk_bsy(z);

wr_data(high,low,z); /* trajectory data will be loaded */

/* 6 pairs of data should follow */

chk_bsy(z);

wr_data(0x00,0x00,z); /* acceleration integer */

chk_bsy(z);

wr_data(0x02,0xff,z); /* acceleration fractional word */

high = 0x03 >> 24;

low = 0x00 >> 16;

chk_bsy(z);

wr_data(high,low,z); /* velocity integer */

high = 0xff;

low = 0xff;

chk_bsy(z);

wr_data(high,low,z); /* velocity fractional word */

/* load position 32 byte word */

POSHI = (unsigned char) (j >> 24);

POSLO = (unsigned char) (j >> 16);

high =POSHI;

low = POSLO;

/*printf("high and low value %d\n\r %d\n\r ", high, low);*/

chk_bsy(z);

wr_data(high,low,z); /* load position byte */

POSHI = (unsigned char) (j >> 8);

POSLO = (unsigned char) (j);

high = POSHI;

low = POSLO;

/*printf("High and low value %d\n\r %d\n\r", high, low);*/

chk_bsy(z);

wr_data(high,low,z);

chk_bsy(z);

wr_cmd(STT,z);

 

 

 

junk = 0;

for (i=0;i<2000;i++)

junk = junk + 1;

 

 

}

 

 

void main(void)

{

unsigned char stat;

 

printf("Enter an angle for motor 1:");

gets(num);

THETA = atoi(num); /* Convert angle from an argument */

/* to an integer */

printf("Theta = %d\n\r", THETA);

pos_val1 = (long) desired_pos(THETA);

 

printf("position value after function %++ld\n",pos_val1);

 

printf("Enter an angle for motor 2:");

gets(num);

THETA = atoi(num);

printf("Theta = %d\n\r", THETA);

pos_val2 = (long) desired_pos(THETA);

 

 

z=0;

while (z<=1)

{

 

outp(baseaddr+(z*4+3),0xC1); /* sets up the parallel port */

 

junk = 0;

for (i=0;i<500;i++)

junk = junk + 1;

 

outp(baseaddr+(z*4+1),0x3f);

outp(baseaddr+(z*4+1),0x1f); /* set reset bit to low */

junk = 0;

for (i=0;i<500;i++)

junk = junk + 1;

outp(baseaddr+(z*4+1),0x3e); /* always keeping the cs bit low */

 

/* ****************************** */

junk = 0;

for (i=0;i<20;i++)

junk = junk + 1;

 

stat = rd_st(z);

printf("RESETING CHIP \n");

printf("status byte immediately after reset by pin: %d\n",stat);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after reset by pin: %d\n",stat);*/

count = 0;

while ((stat != 0x84) & (stat!=0xC4))

{

chk_bsy(z);

stat = rd_st(z);

count ++;

/* if ((stat == 0x84) || (stat == 0xC4))

printf("status byte after reset by pin: %d and number of/* tries: %ld\n",stat,count);*/

}

 

chk_bsy(z);

wr_cmd(RESET, z);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after reset command: %d\n",stat);*/

count = 0;

 

junk = 0;

for (i=0;i<20000;i++)

junk = junk + 1;

 

while ((stat != 0x84) & (stat!=0xC4))

{

chk_bsy(z);

stat = rd_st(z);

count ++;

/*printf("status byte after reset command: %d and number of tries %ld\n",stat,count);*/

}

 

chk_bsy(z);

wr_cmd(MSKI, z);

chk_bsy(z);

wr_data(0x0,0x0, z);

chk_bsy(z);

wr_cmd(RSTI, z);

chk_bsy(z);

wr_data(0x0,0x0, z);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after reseting interrupts, 1: %d\n",stat);*/

count = 0;

while ((stat != 0x80) & (stat!=0xC0) & (count < 10))

{

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after reseting interrupts, 2: %d\n",stat);*/

count++;

}

 

/******** now ready to load the filter coefficients and trajectory */

 

chk_bsy(z); /* 8 bit port */

wr_cmd(0x05,z);

 

chk_bsy(z); /* setting home position */

wr_cmd(DFH, z);

 

 

/* ************* Load the Filter Parameters ******************** */

junk = 0;

for (i=0;i<2000;i++)

junk = junk + 1;

 

chk_bsy(z);

wr_cmd(LFIL,z);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after LFIL, 1: %d\n",stat);*/

chk_bsy(z);

wr_data(0x00,0x0a,z); /* Hi byte loads the derivative sampling rate */

/* Hi byte of 0 = 256us sampling rate */

/* Lo byte informs data to be loaded, load Kp data */

/* bit 3 load Kp data

bit 2 load Ki data

bit 1 load Kd data

bit 0 load il data */

 

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after LFIL data loading, 1: %d\n",stat);*/

 

high = 0x00af >> 8; /*Kp values loaded*/

low = 0x00af;

chk_bsy(z);

wr_data(high,low,z); /* load Kp data MSB first */

high = 0x0001 >> 8; /*Kd values loaded*/

low = 0x0001;

chk_bsy(z);

wr_data(high,low,z);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after Kp and Kd data loading, 1: %d\n",stat);*/

 

chk_bsy(z);

wr_cmd(UDF,z);

chk_bsy(z);

stat = rd_st(z);

/*printf("status byte after UDF command, 1: %d\n",stat);*/

 

junk = 0;

for (i=0;i<2000;i++)

junk = junk + 1;

 

chk_bsy(z);

wr_cmd(MSKI,z);

chk_bsy(z);

wr_data(0x0,0x0,z);

chk_bsy(z);

wr_cmd(RSTI,z);

chk_bsy(z);

wr_data(0x0,0x0,z);

chk_bsy(z);

 

stat = rd_st(z);

/*printf("status byte after reseting interrupts : %d\n",stat);*/

/* z++;

}*/

/* ************* Load the Trajectory Parameters ************** */

 

z++;

}

 

 

 

load_traj(0, pos_val1);

load_traj(1, pos_val2);

 

}