/* 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. * *

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

 

#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_val;

unsigned char POSHI, POSLO;

int THETA;

long desired_pos(int);

void wr_cmd(unsigned char);

unsigned char rd_st(void);

void chk_bsy(void);

void wr_data(unsigned char, unsigned char);

long rd_data(void);

 

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

/* 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(void)

{

int ST;

 

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

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

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

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

ST = inp(baseaddr);

}

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

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

}

 

unsigned char rd_st(void)

{

unsigned char status;

 

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

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

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

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

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

return(status);

}

 

void wr_cmd(unsigned char CMD)

{

 

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

outp(baseaddr,CMD);

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

 

/* chk_bsy(); */

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

outp(baseaddr+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 jjunk; */

 

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

outp(baseaddr, HIdat & 0xff);

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

/* chk_bsy(); */

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

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

 

outp(baseaddr, LOdat & 0xff);

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

 

/* chk_bsy(); */

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

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

 

}

 

long rd_data(void)

{ long lmvalue;

long dataval;

 

lmvalue = 0;

dataval = 0;

 

chk_bsy();

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

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

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

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

lmvalue = (dataval << 24);

 

chk_bsy();

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

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

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

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

lmvalue = lmvalue + (dataval << 16);

 

chk_bsy();

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

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

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

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

lmvalue = lmvalue + (dataval << 8);

 

chk_bsy();

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

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

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

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

lmvalue = lmvalue + dataval;

 

return(lmvalue);

 

}

 

long desired_pos(int THETA)

{

double pos;

 

pos = (THETA * 218.4 * 500.0 * 4.0)/360; /* 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 main(void)

{ long i, junk, count, position;

unsigned char stat, high, low;

long dataenc[260];

FILE *fptr;

 

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

dataenc[i] = 0;

 

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

 

junk = 0;

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

junk = junk + 1;

 

outp(baseaddr+1,0x3f);

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

junk = 0;

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

junk = junk + 1;

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

 

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

junk = 0;

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

junk = junk + 1;

 

stat = rd_st();

printf("RESETING CHIP \n");

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

chk_bsy();

stat = rd_st();

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

count = 0;

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

{

chk_bsy();

stat = rd_st();

count ++;

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

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

}

 

chk_bsy();

wr_cmd(RESET);

chk_bsy();

stat = rd_st();

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();

stat = rd_st();

count ++;

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

}

 

chk_bsy();

wr_cmd(MSKI);

chk_bsy();

wr_data(0x0,0x0);

chk_bsy();

wr_cmd(RSTI);

chk_bsy();

wr_data(0x0,0x0);

/* chk_bsy(); */

stat = rd_st();

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

count = 0;

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

{

chk_bsy();

stat = rd_st();

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

count++;

}

 

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

 

chk_bsy(); /* 8 bit port */

wr_cmd(0x05);

 

chk_bsy(); /* setting home position */

wr_cmd(DFH);

 

 

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

junk = 0;

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

junk = junk + 1;

 

chk_bsy();

wr_cmd(LFIL);

chk_bsy();

stat = rd_st();

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

chk_bsy();

wr_data(0x00,0x0a); /* 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();

stat = rd_st();

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

 

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

low = 0x00af;

chk_bsy();

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

high = 0x00; /*Kd values loaded*/

low = 0x01;

chk_bsy();

wr_data(high,low);

chk_bsy();

stat = rd_st();

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

 

chk_bsy();

wr_cmd(UDF);

chk_bsy();

stat = rd_st();

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

 

junk = 0;

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

junk = junk + 1;

 

chk_bsy();

wr_cmd(MSKI);

chk_bsy();

wr_data(0x0,0x0);

chk_bsy();

wr_cmd(RSTI);

chk_bsy();

wr_data(0x0,0x0);

chk_bsy();

stat = rd_st();

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

 

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

printf("Enter an angle:");

gets(num);

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

/* to an integer */

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

pos_val = (long) desired_pos(THETA);

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

chk_bsy();

wr_cmd(LTRJ);

high = 0x00;

low = 0x2A;

chk_bsy();

 

chk_bsy();

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

/* 6 pairs of data should follow */

chk_bsy();

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

chk_bsy();

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

 

high = 0x00 >> 24;

low = 0x00 >> 16;

chk_bsy();

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

high = 0xff;

low = 0xff;

chk_bsy();

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

/* load position 32 byte word */

 

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

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

high =POSHI;

low = POSLO;

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

chk_bsy();

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

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

POSLO = (unsigned char) (pos_val);

high = POSHI;

low = POSLO;

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

chk_bsy();

wr_data(high,low);

chk_bsy();

wr_cmd(STT);

 

 

 

junk = 0;

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

junk = junk + 1;

 

#ifdef asdfasf

count = 0;

while (count < 10)

{

chk_bsy();

wr_cmd(RDRP);

chk_bsy;

position = (long) rd_data();

printf("the motor position is: %ld\n",position);

 

junk = 0;

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

junk = junk + 1;

 

/* added a time delay so that we don't */

/* write to the chip too much */

/* maximum writes are 100 Hz */

count ++;

}

#endif

 

count = 0;

chk_bsy();

stat = rd_st();

while (!(stat & 0x4)) /* checking if trajectory complete bit goes high */

{

chk_bsy();

wr_cmd(RDRP);

chk_bsy();

position = (long) rd_data();

/* dataenc[count] = position; */

count++;

printf("the motor position is: %ld\n",position);

 

junk = 0;

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

 

junk = junk + 1;

 

chk_bsy();

 

stat = rd_st();

}

printf("the trajectory is complete, status byte: %d\n",stat);

printf("the number of data points is: %ld\n",count);

 

junk = 0;

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

junk = junk + 1;

 

count = 0;

while (count < 10)

{

chk_bsy();

wr_cmd(RDRP);

chk_bsy();

position = (long) rd_data();

printf("the motor position is: %ld\n",position);

 

junk = 0;

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

junk = junk + 1;

count ++;

}

 

 

 

/* turn motor off by sending 1 to the high byte */

chk_bsy();

wr_cmd(LTRJ);

high = 0x01;

low = 0x00;

chk_bsy();

wr_data(high,low);

chk_bsy();

wr_cmd(STT);

 

junk = 0;

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

junk = junk + 1;

 

chk_bsy();

stat = rd_st();

printf("the motor should be off, status byte: %d\n",stat);

 

count = 0;

while (count < 10)

{

chk_bsy();

wr_cmd(RDRP);

chk_bsy();

position = (long) rd_data();

printf("the motor position is: %ld\n",position);

 

junk = 0;

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

junk = junk + 1;

 

/* added a time delay so that we don't */

/* write to the chip too much */

/* maximum writes are 100 Hz */

count ++;

}

 

fptr = fopen("position.dat","w");

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

fprintf(fptr,"%ld \n",dataenc[i]);

fclose(fptr);

 

}