/* 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 #include #include #include #include #include #include #include #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); }