/* A program to demonstrate how to use Baseboard peripherals */ /* in an event driven program using select(). */ /* The program uses two peripherals, the buttons and LEDs */ /* on the Baseboard (bb4io), and the dual quadrature decoder, */ /* quad2. The program configures the quad2 to report every */ /* 50 milliseconds and prints to standard output the counts */ /* and frequency of the quadrature inputs. Pressing button */ /* one on the Baseboard turns on or off quadratures output. */ /* The LEDs on the Baseboard are incremented on each reading */ /* from the quadrature decoder. */ /* gcc -o quad_demo quad_demo.c */ /* ./quad_demo */ /* This program is more complete than the first sample but is */ /* still not production ready. Please use or refactor this */ /* code as you see fit for your application. */ #include #include #include #include #include #include #include #include #include /************************* Defines *****************************/ #define DPSRVADDR "127.0.0.1" #define DPSRVPORT 8870 #define MXCMD 80 /* limits dpdaemon command size */ #define MXBUF 99 /* limits size of response from dpdaemon */ /********************** Global Variables ***********************/ int fd_cmd; // fd to send commands to dpdaemon int outenable; // set ==1 to print quad2 readings on stdout void sndcmd(int, char *, int); // write a string down an FD void do_quad(char *); // process input line of quadrature data int main (int argc, char *argv[]) { int fd_quad; // fd to stream of quadrature readings int fd_button; // fd to buttons on the FPGA card fd_set rfds; // bit masks for select statement int mxfd; // Maximum FD for the select statement struct timeval tv; // for the one second timer int ret; // return value for select() call char cmd[MXCMD]; // print commands to dpdaemon here char inbuf[MXBUF]; // read from dpdaemon goes here char qbuf[MXBUF]; // data from the quadrature sensor goes here int qinx; // index into qbuf int nread; // return value for read() int gotline; // ==1 if found a full line in the data stream int cmdlen; // length of command to send int i; // generic loop counter int value; // value read from bb4io button int count; // number of quad2 readings struct sockaddr_in skt; // network address for dpdaemon int adrlen; /* Initialize the state */ count = 0; outenable = 1; qinx = 0; // Open connections to dpdaemon adrlen = sizeof(struct sockaddr_in); (void) memset((void *) &skt, 0, (size_t) adrlen); skt.sin_family = AF_INET; skt.sin_port = htons(8870); if ((inet_aton("127.0.0.1", &(skt.sin_addr)) == 0) || ((fd_cmd = socket(AF_INET, SOCK_STREAM, 0)) < 0) || (connect(fd_cmd, (struct sockaddr *) &skt, adrlen) < 0) || ((fd_quad = socket(AF_INET, SOCK_STREAM, 0)) < 0) || (connect(fd_quad, (struct sockaddr *) &skt, adrlen) < 0) || ((fd_button = socket(AF_INET, SOCK_STREAM, 0)) < 0) || (connect(fd_button, (struct sockaddr *) &skt, adrlen) < 0)) { printf("Error: unable to connect to dpdaemon.\n"); exit(-1); } /* Clear the LEDs */ cmdlen = snprintf(cmd, MXCMD, "dpset bb4io leds 0\n"); sndcmd(fd_cmd, cmd, cmdlen); /* Configure quad2 for an update every 50 milliseconds */ /* Then start the stream of quad2 readings */ // (note that we send the dpcat command on fd_quad. This is // means the quadrature readings will be available on fd_quad cmdlen = snprintf(cmd, MXCMD, "dpset quad2 update_period 50\n"); sndcmd(fd_cmd, cmd, cmdlen); cmdlen = snprintf(cmd, MXCMD, "dpcat quad2 counts\n"); sndcmd(fd_quad, cmd, cmdlen); /* Start the data stream of button presses from the Baseboard */ cmdlen = snprintf(cmd, MXCMD, "dpcat bb4io buttons\n"); sndcmd(fd_button, cmd, cmdlen); /* Watch for button press events, quadrature readings, and */ /* timeout each second */ mxfd = (fd_quad > fd_button) ? (fd_quad+1) : (fd_button+1); while (1) { FD_ZERO(&rfds); FD_SET(fd_quad, &rfds); FD_SET(fd_button, &rfds); tv.tv_sec = 1; tv.tv_usec = 0; ret = select(mxfd, &rfds, (fd_set *)NULL, (fd_set *)NULL, &tv); /* if select error -- bail out on all but EINTR and EAGAIN */ if ((ret < 0) && ((errno != EINTR) && (errno != EAGAIN))) { perror("Failure in select() "); exit(-1); } if (ret == 0) { // timeout // this timeout does not occur on one second boundaries // but one second after the last time we processed a // read-ready file descriptor. To make a _periodic_ // timer you would use gettimeofday to intelligently // set tv_sec and tv_usec before each call to select(). } if (FD_ISSET(fd_button, &rfds)) { ret = read(fd_button, inbuf, MXBUF); if (0 > ret) { if ((errno != EINTR) && (errno != EAGAIN)) perror("Error reading button press from bb4io"); continue; } // While tempting in its simplicity, the code below has a bug. // There is no guarantee that read() will return the newline // at the end of a button sensor reading. The next read would // see the newline from the previous reading and not see the new // value. This case is handled properly for quad2 readings. if (sscanf(inbuf, "%d", &value) == 1) { if (1 == value) // output quadrature readings on first button outenable = 1; else if (2 == value) outenable = 0; // suppress readings on second button } } if (FD_ISSET(fd_quad, &rfds)) { // increment count and send to LEDs count++; cmdlen = snprintf(cmd, MXCMD, "dpset bb4io leds %x\n", count & 0x00ff); sndcmd(fd_cmd, cmd, cmdlen); // read() is not guaranteed to return full lines of text. Since we may // get part of a line, we need to store the partial line (qbuf) while // waiting for the next read. Worse, if the processor is busy we may // find more than one line of input in the buffer. We need to scan the // collected characters looking for a newline. If found we process the // line and move any remaining characters to the beginning of the buffer. // The code below would normally go in a subroutine that is used for // reading sensor streams. /* Get data from quadrature sensor. There may already be characters */ /* in the buffer so add to end of qbuf */ nread = read(fd_quad, &(qbuf[qinx]), (MXBUF - qinx)); if (0 >= nread) { if ((errno != EINTR) && (errno != EAGAIN)) perror("Error reading the quadrature peripheral"); continue; } qinx += nread; /* Scan for a complete lines. */ do { gotline = 0; // Scan for a newline. If found, replace it with a null for (i = 0; i < qinx; i++) { if (qbuf[i] == '\n') { qbuf[i] = (char) 0; gotline = 1; do_quad(qbuf); // move any remaining characters to start of buffer (void) memmove(qbuf, &(qbuf[i+1]), (qinx - (i+1))); qinx -= i+1; break; } } } while ((gotline == 1) && (qinx > 0)); } } } /* sndcmd() : send a command to the dpdaemon. Report */ /* write errors to stdout but try to continue. */ void sndcmd(int fd, char *cmd, int length) { int nwrt; // number of bytes written if (0 >= length) { printf("Error sending command of length %d\n", length); return; } nwrt = write(fd, cmd, length); if (nwrt != length) { printf("Error sending command to dpdaemon. Wrote only %d of %d bytes\n", nwrt, length); } return; } /* do_quad() : get quadrature readings from a line of sensor data. */ void do_quad(char *qline) { int ret; // return value for sscan() call int tick0, tick1; // quadrature ticks float period0, period1; // seconds to get ticks float freq0, freq1; // tick frequency ret = sscanf(qline, "%d %f %d %f", &tick0, &period0, &tick1, &period1); if (4 != ret) { printf("error reading quadrature ticks and periods\n"); return; } if ((0 == tick0) || (0 == period0)) freq0 = 0.0; else freq0 = (float) tick0 / period0; if ((0 == tick1) || (0 == period1)) freq1 = 0.0; else freq1 = (float) tick1 / period1; // print the counts and their frequency if (outenable) printf("%d %f %d %f\n", tick0, freq0, tick1, freq1); // In a fully event driven system, this is where we // would do the PID loop to control motor speed. // do_pid(freq0, freq1); // and where we could do odometry // do_odometry(tick0, tick1); return; }