00001 #include <stdio.h>
00002 #include <string.h>
00003 #include <signal.h>
00004 #include <stdlib.h>
00005 #include <ctype.h>
00006 #include <math.h>
00007 #include <unistd.h>
00008
00009 #include "stg.h"
00010
00011
00012 #ifndef __GNUC__
00013 #ifndef __attribute__
00014 #define __attribute__(x)
00015 #endif
00016 #endif
00017
00018 static char __attribute__((unused)) ident[] = "$Id: stgdiag.c,v 1.7 2001/10/26 14:31:01 wshackle Exp $";
00019
00020
00021
00022
00023
00024 #define NUM_AXES 3
00025
00026 static int done = 0;
00027 static void quit(int sig)
00028 {
00029 done = 1;
00030 }
00031
00032 enum {POS = 1, LIMIT, HOME, INDEX, LATCH, DAC, DIO, ADC};
00033
00034 int main(int argc, char *argv[])
00035 {
00036 int which = POS;
00037 int newline=0;
00038 double pos[NUM_AXES];
00039 double oldpos[NUM_AXES];
00040 int lim[NUM_AXES*2];
00041 int oldlim[NUM_AXES*2];
00042 int home[NUM_AXES];
00043 int oldhome[NUM_AXES];
00044 int index;
00045 int latch;
00046 int axis;
00047 double volts;
00048 int t;
00049 char buf[80];
00050 char *cptr;
00051 double adc[NUM_AXES];
00052 int argsused = 0;
00053
00054
00055 if (0 != iopl(3)) {
00056 printf("must be root to run\n");
00057 exit(1);
00058 }
00059
00060 if (argc > 1) {
00061 if (!strcmp(argv[1], "pos")) {
00062 which = POS;
00063 }
00064 else if (!strcmp(argv[1], "dio")) {
00065 which = DIO;
00066 }
00067 else if (!strcmp(argv[1], "dac")) {
00068 which = DAC;
00069 }
00070 else if (!strcmp(argv[1], "adc")) {
00071 which = ADC;
00072 }
00073 else if (!strcmp(argv[1], "limit")) {
00074 which = LIMIT;
00075 }
00076 else if (!strcmp(argv[1], "home")) {
00077 which = HOME;
00078 }
00079 else if (!strcmp(argv[1], "index")) {
00080 if(argc < 2)
00081 {
00082 which = 0;
00083 printf("No axis specified (using 0)\n");
00084 }
00085 else if (1 == sscanf(argv[2], "%d", &axis)) {
00086 argsused = 1;
00087 which = INDEX;
00088 }
00089 else {
00090 printf("syntax: %s {limit | home | index <axis> | latch <axis>}\n", argv[0]);
00091 exit(1);
00092 }
00093 }
00094 else if (!strcmp(argv[1], "latch")) {
00095 if (1 == sscanf(argv[2], "%d", &axis)) {
00096 argsused = 1;
00097 which = LATCH;
00098 }
00099 else {
00100 printf("syntax: %s {limit | home | index <axis> | latch <axis>}\n", argv[0]);
00101 exit(1);
00102 }
00103 }
00104 else {
00105 printf("syntax: %s {limit | home | index <axis> | latch <axis>}\n", argv[0]);
00106 exit(1);
00107 }
00108 }
00109
00110 if (argc > 2+argsused) {
00111 FIND_STG_BASE_ADDRESS = 0;
00112 if (1 != sscanf(argv[2], "%i", &STG_BASE_ADDRESS)) {
00113 printf("bad address: %s\n", argv[2]);
00114 exit(1);
00115 }
00116 else {
00117 printf("setting STG base address to 0x%X\n", STG_BASE_ADDRESS);
00118 }
00119 }
00120 else {
00121 STG_BASE_ADDRESS = BaseFind();
00122 if (STG_BASE_ADDRESS == 0) {
00123 printf("can't find STG board\n");
00124 exit(1);
00125 }
00126 else {
00127 printf("found STG at 0x%X\n", STG_BASE_ADDRESS);
00128 }
00129 }
00130
00131 signal(SIGINT, quit);
00132
00133 stgMotInit(NULL);
00134 stgDioInit(NULL);
00135 stgAioInit(NULL);
00136
00137 printf("STG Model =%d\n", stgModel());
00138
00139
00140 if (which == POS) {
00141 while (! done) {
00142 newline = 0;
00143 stgEncoderReadAll(NUM_AXES, pos);
00144 for (t = 0; t < NUM_AXES; t++) {
00145 if (fabs(pos[t]-oldpos[t]) > 1000.0 && !newline) {
00146 newline=1;
00147 printf("\n");
00148 }
00149 oldpos[t] = pos[t];
00150 }
00151 for (t = 0; t < NUM_AXES; t++) {
00152 printf("%+15.3f\t", pos[t]);
00153 }
00154 printf("\r");
00155 fflush(stdout);
00156 usleep(10000);
00157 }
00158 }
00159 else if (which == LIMIT) {
00160 while (! done) {
00161 newline=0;
00162 for (t = 0; t < NUM_AXES; t++) {
00163 stgMaxLimitSwitchRead(1, &lim[2*t]);
00164 if (oldlim[2*t] != lim[2*t] && !newline) {
00165 printf("\n");
00166 newline=1;
00167 }
00168 oldlim[2*t] = lim[2*t];
00169 stgMinLimitSwitchRead(1, &lim[2*t+1]);
00170 if (oldlim[2*t+1] != lim[2*t+1] && !newline) {
00171 printf("\n");
00172 newline=1;
00173 }
00174 oldlim[2*t+1] = lim[2*t+1];
00175 }
00176 if (newline) {
00177 for (t = 0; t < NUM_AXES; t++) {
00178 printf("%+d\t", lim[2*t]);
00179 printf("%+d\t", lim[2*t+1]);
00180 }
00181 printf("\r");
00182 fflush(stdout);
00183 }
00184 usleep(10000);
00185 }
00186 }
00187 else if (which == HOME) {
00188 while (! done) {
00189 newline=0;
00190 for (t = 0; t < NUM_AXES; t++) {
00191 stgHomeSwitchRead(t, &home[t]);
00192 if (oldhome[t] != home[t] && !newline) {
00193 printf("\n");
00194 newline=1;
00195 }
00196 oldhome[t] = home[t];
00197 }
00198 if (newline) {
00199 for (t = 0; t < NUM_AXES; t++) {
00200 printf("%d\t", home[t]);
00201 }
00202 printf("\r");
00203 fflush(stdout);
00204 }
00205 usleep(10000);
00206 }
00207 }
00208 else if (which == INDEX) {
00209 if (axis < 0 || axis >= NUM_AXES) {
00210 printf("syntax: %s {limit | home | index <axis> | latch <axis>}\n", argv[0]);
00211 exit(1);
00212 }
00213
00214 stgEncoderResetIndex(axis);
00215 while (! done) {
00216 stgEncoderReadLevel(axis, &index);
00217 printf("%d ", index);
00218 fflush(stdout);
00219 usleep(10000);
00220 }
00221 }
00222 else if (which == LATCH) {
00223 if (axis < 0 || axis >= NUM_AXES) {
00224 printf("syntax: %s {limit | home | index <axis> | latch <axis>}\n", argv[0]);
00225 exit(1);
00226 }
00227
00228 stgEncoderResetIndex(axis);
00229 while (! done) {
00230 stgEncoderReadLatch(axis, &latch);
00231 printf("%d ", latch);
00232 fflush(stdout);
00233 usleep(10000);
00234 }
00235 }
00236 else if (which == DAC) {
00237 printf("Axis? ");
00238 fgets(buf,80,stdin);
00239 axis = (int) strtol(buf,0,0);
00240 while (! done) {
00241 printf("DAC(%d)",axis);
00242 if (fgets(buf,80,stdin) == NULL) {
00243 break;
00244 }
00245 if (! isdigit(buf[0]) && buf[0] != '-') {
00246 break;
00247 }
00248 cptr = buf;
00249 while ((*cptr >= '0' && *cptr <= '9')
00250 || *cptr == '-' || *cptr == '.' || *cptr == 'E') {
00251 cptr++;
00252 }
00253 *cptr = 0;
00254 volts = strtod(buf,0);
00255 printf("volts=%f\n",volts);
00256 stgDacWrite(axis, volts);
00257 }
00258 }
00259 else if (which == ADC) {
00260 while (! done) {
00261 for (t = 0; t < NUM_AXES; t++) {
00262 stgAdcStart(t);
00263 stgAdcWait();
00264 stgAdcRead(t, &adc[t]);
00265 printf("%f\t", adc[t]);
00266 }
00267 printf("\r");
00268 fflush(stdout);
00269 usleep(10000);
00270 }
00271 }
00272 else if (which == DIO) {
00273 int val, i;
00274 for (i =0; i<24; i++) {
00275 printf("%d",i%8);
00276 }
00277 printf("\n");
00278 for (i = 0; i < 24; i++) {
00279 stgDioRead(i,&val);
00280 printf("%d",val);
00281 }
00282 printf("\n");
00283 }
00284
00285 stgAioQuit();
00286 stgDioQuit();
00287 stgMotQuit();
00288
00289 return 0;
00290 }