/* * muovi.c * * Stefano Rosa - Sergio Sigala * * VERSIONE: 1.0 - 04 JUL 2001 * * DESCRIZIONE: programma di prova per comunicazione seriale con il MARMOT. * Illustra come aprire il canale seriale e come procedere nella trasmissione * e nella ricezione dei pacchetti. * * ATTENZIONE: il programma suppone che la porta a cui il MARMOT e' collegato * sia la `/dev/ttyS0'. * * ATTENZIONE: per ulteriori informazioni su come gestire a basso livello la * comunicazione seriale, consultare `The GNU C Library Reference Manual', * attraverso il comando `info libc'. Cercare la voce * `Low-Level Terminal Interface' nel menu che compare. */ #include #include #include #include #include #include #include #include #include #include #include /* * Identificatori dei pacchetti: devono corrispondere a quelli definiti nel * programma che gira sul MARMOT. */ #define ID_SETSPEED 0 #define ID_GETSPEED 1 #define ID_GETADC 2 #define ID_BADPKT 3 #define ID_BADCHKSUM 4 #define ID_TIMEOUT 5 #define ID_SETMOTORS 6 #define ID_GETMOTORS 7 #define ID_DEADCONN 8 #define ID_BADPARMS 9 /* * Struttura utilizzata per memorizzare informazioni sulla porta seriale. */ typedef struct { char *device; int fd; struct termios now; struct termios old; } TTYH; /* * Apre un canale seriale. */ TTYH *tty_open(char *device) { TTYH *h; printf("tty_open() called\n"); printf("provo ad aprire %s\n", device); if ((h = (TTYH *) malloc(sizeof(TTYH))) == NULL) { fprintf(stderr, "malloc() reports out of memory\n"); return NULL; } if ((h->fd = open(device, O_RDWR)) < 0) { fprintf(stderr, "open() failed\n"); return NULL; } if (tcgetattr(h->fd, &h->old) != 0) { close(h->fd); free(h); fprintf(stderr, "tcgetattr() failed\n"); return NULL; } h->now = h->old; /* input modes */ h->now.c_iflag &= ~INPCK; /* parity checking disabled */ h->now.c_iflag &= ~IGNPAR; /* don't ignore bytes with bad parity */ h->now.c_iflag &= ~PARMRK; /* don't mark bytes with bad parity */ h->now.c_iflag &= ~ISTRIP; /* use 8 bit bytes */ h->now.c_iflag |= IGNBRK; /* ignore break conditions */ h->now.c_iflag &= ~BRKINT; /* don't generate signals on break cond. */ h->now.c_iflag &= ~IGNCR; /* don't ignore carriage return */ h->now.c_iflag &= ~ICRNL; /* don't translate \r to \n */ h->now.c_iflag &= ~INLCR; /* don't translate \n to \r */ h->now.c_iflag &= ~IXOFF; /* disable */ h->now.c_iflag &= ~IXON; /* disable */ h->now.c_iflag &= ~IXANY; /* disable */ h->now.c_iflag &= ~IMAXBEL; /* disable */ /* output modes */ h->now.c_oflag &= ~OPOST; /* don't process output characters */ h->now.c_oflag &= ~ONLCR; /* disable \n conversion to \r\n */ // h->now.c_oflag &= ~OXTABS; /* don't convert tabs */ // h->now.c_oflag &= ~ONOEOT; /* don't discard C-d */ /* control modes */ h->now.c_cflag |= CLOCAL; /* ignore modem status lines */ h->now.c_cflag &= ~HUPCL; /* don't generate a modem disconnect */ h->now.c_cflag |= CREAD; /* input can be read from the terminal */ h->now.c_cflag &= ~CSTOPB; /* use only one stop bit */ h->now.c_cflag &= ~PARENB; /* disable parity bit */ h->now.c_cflag &= ~PARODD; /* use even parity */ // h->now.c_cflag &= ~CSIZE; h->now.c_cflag |= CS8; /* use 8 bits per byte */ // h->now.c_cflag &= ~CCTS_OFLOW; // h->now.c_cflag &= ~CRTS_IFLOW; // h->now.c_cflag &= ~MDMBUF; // h->now.c_cflag &= ~CIGNORE; /* local modes */ h->now.c_lflag &= ~ICANON; /* disable canonical input process. mode */ h->now.c_lflag &= ~ECHO; /* disable echoing of input characters */ h->now.c_lflag &= ~ECHOE; /* don't echo erase */ h->now.c_lflag &= ~ECHOPRT; h->now.c_lflag &= ~ECHOK; /* disable kill character */ h->now.c_lflag &= ~ECHOKE; /* disable kill character */ h->now.c_lflag &= ~ECHONL; /* don't echo \n */ h->now.c_lflag &= ~ECHOCTL; /* don't echo control characters */ h->now.c_lflag &= ~ISIG; /* don't generate INTR, QUIT and SUSP */ h->now.c_lflag &= ~IEXTEN; /* disable other special characters */ h->now.c_lflag &= ~NOFLSH; /* don't flush buffers on signals */ h->now.c_lflag &= ~TOSTOP; h->now.c_lflag &= ~FLUSHO; h->now.c_lflag &= ~PENDIN; if (cfsetspeed(&h->now, B9600) != 0) { close(h->fd); free(h); fprintf(stderr, "tcsetspeed() failed\n"); return NULL; } h->now.c_cc[VMIN] = 0; /* non-blocking read() */ h->now.c_cc[VTIME] = 1; /* timeout di 100 ms */ if (tcsetattr(h->fd, TCSANOW, &h->now) != 0) { close(h->fd); free(h); fprintf(stderr, "tcsetattr() failed\n"); return NULL; } h->device = strdup(device); return h; } /* * Chiude un canale seriale. */ int tty_close(TTYH *h) { printf("tty_close() called\n"); if (h == NULL) { fprintf(stderr, "invalid handle\n"); return -1; } if (tcsetattr(h->fd, TCSANOW, &h->old) != 0) { close(h->fd); free(h->device); free(h); fprintf(stderr, "tcsetattr() failed\n"); return -1; } close(h->fd); free(h->device); free(h); return 1; } static TTYH *h = NULL; /* * Chiamata automaticamente alla conclusione del programma. */ void release_serial() { if (h != NULL) tty_close(h); } /* * Calcola il byte di checksum affinche' la somma di tutti i byte del * pacchetto sia nulla. */ void fix_checksum(unsigned char *buf) { int i; int size = buf[0]; /* legge lunghezza del pacchetto */ unsigned char value = 0; for (i = 0; i < size - 1; i++) { value += buf[i]; } buf[size - 1] = -(value & 0xff); } #define LO(a) ((a) & 0xff) #define HI(a) (((a) >> 8) & 0xff) /* * Controlla che il checksum del pacchetto sia nullo. */ int test_chksum(unsigned char *buf) { int i = buf[0]; /* legge lunghezza del pacchetto */ int j; unsigned char sum = 0; for (j = 0; j < i; j++) sum = (sum + buf[j]) & 0xff; if (sum != 0) { fprintf(stderr, "checksum errato: %d\n", sum); } return sum; } /* * Setta i periodi (in microsecondi) dei tre oscillatori che pilotano i * motori. Il segno di ciascun periodo identifica il verso di rotazione * richiesto. Alcuni valori sono: * * periodo frequenza * * 200 -> 5KHz * 300 -> 3333Hz * 400 -> 2.5KHz */ int set_speed(int p1, int p2, int p3) { int l; unsigned char ack; unsigned char buf[100]; unsigned cw1 = p1 >= 0 ? 1 : 0; //direzioni per i tre motori unsigned cw2 = p2 >= 0 ? 1 : 0; unsigned cw3 = p3 >= 0 ? 1 : 0; unsigned per_oc2 = p1 >= 0 ? p1 : -p1; //periodi per i tre motori unsigned per_oc3 = p2 >= 0 ? p2 : -p2; unsigned per_oc4 = p3 >= 0 ? p3 : -p3; buf[0] = 10; //size buf[1] = ID_SETSPEED; //id buf[2] = HI(per_oc2); //high oc2 buf[3] = LO(per_oc2); //low oc2 buf[4] = HI(per_oc3); //high oc3 buf[5] = LO(per_oc3); //low oc3 buf[6] = HI(per_oc4); //high oc4 buf[7] = LO(per_oc4); //low oc4 buf[8] = cw1 + cw2 * 2 + cw3 * 4; //direzione buf[9] = 0; //checksum fix_checksum(buf); write(h->fd, buf, 10); //send data /* * Per ottimizzare la comunicazione, sarebbe meglio che la gestione * della ricezione venisse fatta in modo asincrono rispetto alla * trasmissione, in modo da evitare l'introduzione di ritardi come il * seguente. Questo ritardo ha lo scopo di dare tempo al 68HC11 di * ricevere il pacchetto, elaborarlo e trasmettere completamente la * risposta al PC. */ usleep(10000); //aspetta un tantino if ((l = read(h->fd, buf, 3)) == 0) { fprintf(stderr, "no data from 68HC11\n"); return 0; } else if (l != 3) { fprintf(stderr, "pacchetto di dimensione errata dal 68HC11\n"); fprintf(stderr, "%d byte ricevuti\n", l); return 0; } if (test_chksum(buf) != 0) return 0; ack = buf[1]; if (ack == ID_BADPKT) { fprintf(stderr, "68HC11 notifica pacchetto non riconosciuto\n"); return 0; } if (ack == ID_BADCHKSUM) { fprintf(stderr, "68HC11 notifica errore di checksum\n"); return 0; } if (ack == ID_TIMEOUT) { fprintf(stderr, "68HC11 notifica errore di timeout\n"); return 0; } if (ack != ID_SETSPEED) { fprintf(stderr, "68HC11 ha mandato pacchetto non riconosciuto\n"); return 0; } printf("Aggiornamento velocita' riuscito:\n"); printf("M1: cw/ccw# = %d per_oc2 = %d\n", cw1, per_oc2); printf("M2: cw/ccw# = %d per_oc3 = %d\n", cw2, per_oc3); printf("M3: cw/ccw# = %d per_oc4 = %d\n", cw3, per_oc4); return 1; } /* * Legge i periodi (in microsecondi) dei tre oscillatori che pilotano i * motori. Il segno di ciascun periodo identifica il verso di rotazione * del rispettivo motore. */ int get_speed(int *p1, int *p2, int *p3) { int l; unsigned char buf[100]; unsigned per_oc2; unsigned per_oc3; unsigned per_oc4; unsigned dir; buf[0] = 3; //size buf[1] = ID_GETSPEED; //id buf[2] = 0; //checksum fix_checksum(buf); write(h->fd, buf, 3); //send data /* * Per ottimizzare la comunicazione, sarebbe meglio che la gestione * della ricezione venisse fatta in modo asincrono rispetto alla * trasmissione, in modo da evitare l'introduzione di ritardi come il * seguente. Questo ritardo ha lo scopo di dare tempo al 68HC11 di * ricevere il pacchetto, elaborarlo e trasmettere completamente la * risposta al PC. */ usleep(50000); //aspetta un tantino if ((l = read(h->fd, buf, 10)) == 0) { fprintf(stderr, "no data from 68HC11\n"); return 0; } else if (l == 3) { unsigned char ack; if (test_chksum(buf) != 0) return 0; ack = buf[1]; if (ack == ID_BADPKT) { fprintf(stderr, "68HC11 notifica pacchetto non riconosciuto\n"); return 0; } if (ack == ID_BADCHKSUM) { fprintf(stderr, "68HC11 notifica errore di checksum\n"); return 0; } if (ack == ID_TIMEOUT) { fprintf(stderr, "68HC11 notifica errore di timeout\n"); return 0; } fprintf(stderr, "68HC11 ha mandato pacchetto non riconosciuto\n"); return 0; } else if (l != 10) { fprintf(stderr, "pacchetto di dimensione errata dal 68HC11\n"); fprintf(stderr, "%d byte ricevuti\n", l); return 0; } if (test_chksum(buf) != 0) return 0; per_oc2 = (buf[2] << 8) | buf[3]; per_oc3 = (buf[4] << 8) | buf[5]; per_oc4 = (buf[6] << 8) | buf[7]; dir = buf[8]; if ((dir & 0x01) == 0) per_oc2 = -per_oc2; if ((dir & 0x02) == 0) per_oc3 = -per_oc3; if ((dir & 0x04) == 0) per_oc4 = -per_oc4; if (p1 != NULL) *p1 = per_oc2; if (p2 != NULL) *p2 = per_oc3; if (p3 != NULL) *p3 = per_oc4; printf("Lettura velocita' riuscita:\n"); printf("M1: per_oc2 = %d\n", per_oc2); printf("M2: per_oc3 = %d\n", per_oc3); printf("M3: per_oc4 = %d\n", per_oc4); return 1; } /* * Campiona le tensioni presenti ai quattro ingressi analogici del 68HC11. */ int get_adc(unsigned char *data) { int l; unsigned char buf[100]; buf[0] = 3; //size buf[1] = ID_GETADC; //id buf[2] = 0; //checksum fix_checksum(buf); write(h->fd, buf, 3); //send data /* * Per ottimizzare la comunicazione, sarebbe meglio che la gestione * della ricezione venisse fatta in modo asincrono rispetto alla * trasmissione, in modo da evitare l'introduzione di ritardi come il * seguente. Questo ritardo ha lo scopo di dare tempo al 68HC11 di * ricevere il pacchetto, elaborarlo e trasmettere completamente la * risposta al PC. */ usleep(10000); //aspetta un tantino if ((l = read(h->fd, buf, 7)) == 0) { fprintf(stderr, "no data from 68HC11\n"); return 0; } else if (l == 3) { unsigned char ack; if (test_chksum(buf) != 0) return 0; ack = buf[1]; if (ack == ID_BADPKT) { fprintf(stderr, "68HC11 notifica pacchetto non riconosciuto\n"); return 0; } if (ack == ID_BADCHKSUM) { fprintf(stderr, "68HC11 notifica errore di checksum\n"); return 0; } if (ack == ID_TIMEOUT) { fprintf(stderr, "68HC11 notifica errore di timeout\n"); return 0; } fprintf(stderr, "68HC11 ha mandato pacchetto non riconosciuto\n"); return 0; } else if (l != 7) { fprintf(stderr, "pacchetto di dimensione errata dal 68HC11\n"); fprintf(stderr, "%d byte ricevuti\n", l); return 0; } if (test_chksum(buf) != 0) return 0; if (data != NULL) { data[0] = buf[2]; data[1] = buf[3]; data[2] = buf[4]; data[3] = buf[5]; } printf("Lettura convertitori A/D riuscita:\n"); printf("adc1: %d\n", buf[2]); printf("adc2: %d\n", buf[3]); printf("adc3: %d\n", buf[4]); printf("adc4: %d\n", buf[5]); return 1; } /* * Legge lo stato del segnale ENABLE# di ciascun motore. */ int get_motors(int *stato) { int enaM1, enaM2, enaM3; int l; unsigned char buf[100]; buf[0] = 3; //size buf[1] = ID_GETMOTORS; //id buf[2] = 0; //checksum fix_checksum(buf); write(h->fd, buf, 3); //send data /* * Per ottimizzare la comunicazione, sarebbe meglio che la gestione * della ricezione venisse fatta in modo asincrono rispetto alla * trasmissione, in modo da evitare l'introduzione di ritardi come il * seguente. Questo ritardo ha lo scopo di dare tempo al 68HC11 di * ricevere il pacchetto, elaborarlo e trasmettere completamente la * risposta al PC. */ usleep(10000); //aspetta un tantino if ((l = read(h->fd, buf, 4)) == 0) { fprintf(stderr, "no data from 68HC11\n"); return 0; } else if (l == 3) { unsigned char ack; if (test_chksum(buf) != 0) return 0; ack = buf[1]; if (ack == ID_BADPKT) { fprintf(stderr, "68HC11 notifica pacchetto non riconosciuto\n"); return 0; } if (ack == ID_BADCHKSUM) { fprintf(stderr, "68HC11 notifica errore di checksum\n"); return 0; } if (ack == ID_TIMEOUT) { fprintf(stderr, "68HC11 notifica errore di timeout\n"); return 0; } fprintf(stderr, "68HC11 ha mandato pacchetto non riconosciuto\n"); return 0; } else if (l != 4) { fprintf(stderr, "pacchetto di dimensione errata dal 68HC11\n"); fprintf(stderr, "%d byte ricevuti\n", l); return 0; } if (test_chksum(buf) != 0) return 0; if (stato != NULL) *stato = buf[2]; enaM1 = (buf[2] & 0x01) != 0 ? 1 : 0; enaM2 = (buf[2] & 0x02) != 0 ? 1 : 0; enaM3 = (buf[2] & 0x04) != 0 ? 1 : 0; printf("Lettura stato dei motori riuscita:\n"); printf("M1: segnale ENABLE#: %d\n", enaM1); printf("M2: segnale ENABLE#: %d\n", enaM2); printf("M3: segnale ENABLE#: %d\n", enaM3); return 1; } /* * Altera lo stato del segnale ENABLE# di ciascun motore. */ int set_motors(int stato) { int enaM1, enaM2, enaM3; int l; unsigned char buf[100]; buf[0] = 4; //size buf[1] = ID_SETMOTORS; //id buf[2] = stato & 0x07; buf[3] = 0; //checksum fix_checksum(buf); write(h->fd, buf, 4); //send data /* * Per ottimizzare la comunicazione, sarebbe meglio che la gestione * della ricezione venisse fatta in modo asincrono rispetto alla * trasmissione, in modo da evitare l'introduzione di ritardi come il * seguente. Questo ritardo ha lo scopo di dare tempo al 68HC11 di * ricevere il pacchetto, elaborarlo e trasmettere completamente la * risposta al PC. */ usleep(10000); //aspetta un tantino if ((l = read(h->fd, buf, 3)) == 0) { fprintf(stderr, "no data from 68HC11\n"); return 0; } else if (l == 3) { unsigned char ack; if (test_chksum(buf) != 0) return 0; ack = buf[1]; if (ack == ID_BADPKT) { fprintf(stderr, "68HC11 notifica pacchetto non riconosciuto\n"); return 0; } if (ack == ID_BADCHKSUM) { fprintf(stderr, "68HC11 notifica errore di checksum\n"); return 0; } if (ack == ID_TIMEOUT) { fprintf(stderr, "68HC11 notifica errore di timeout\n"); return 0; } if (ack != ID_SETMOTORS) { fprintf(stderr, "68HC11 ha mandato pacchetto non riconosciuto\n"); return 0; } } else if (l != 3) { fprintf(stderr, "pacchetto di dimensione errata dal 68HC11\n"); fprintf(stderr, "%d byte ricevuti\n", l); return 0; } enaM1 = (stato & 0x01) != 0 ? 1 : 0; enaM2 = (stato & 0x02) != 0 ? 1 : 0; enaM3 = (stato & 0x04) != 0 ? 1 : 0; printf("Settaggio stato motori riuscito:\n"); printf("M1: segnale ENABLE#: %d\n", enaM1); printf("M2: segnale ENABLE#: %d\n", enaM2); printf("M3: segnale ENABLE#: %d\n", enaM3); return 1; } int main(int argc, char **argv) { int i; int perM1, perM2, perM3; int zerox, zeroy; /* apre canale */ h = tty_open("/dev/ttyS0"); if (h == NULL) { fprintf(stderr, "impossibile aprire il canale seriale\n"); return EXIT_FAILURE; } atexit(release_serial); /* * Se vengono passati tre parametri il programma funziona in * modalita' `command line'. I tre parametri identificano i * periodi dei segnali di clock dei motori. Il senso di rotazione * dei motori dipende dal segno dei valori passati. * * I range di valori ammissibili sono [300, 16001], oppure * [-16001, -300]. I valori 300 e -300 corrispondono alle velocita' * massime, mentre 16000 e -16000 a quelle minime; le costanti 16001 * e -16001 fermano i motori. * * Valori esterni a questi due intervalli verranno forzati ad uno * degli estremi dal 68HC11. */ if (argc == 4) { perM1 = atoi(argv[1]); perM2 = atoi(argv[2]); perM3 = atoi(argv[3]); printf("Imposto i periodi M1 = %d, M2 = %d, M3 = %d\n", perM1, perM2, perM3); set_speed(perM1, perM2, perM3); return EXIT_SUCCESS; } /* * Se il numero di parametri passati non e' ne' 0 ne' 4 allora * il programma ferma i motori. */ if (argc != 1 && argc != 4) { printf("Fermo i motori\n"); set_speed(16001, 16001, 16001); return EXIT_SUCCESS; } /* * Questo blocco esercita il sistema di comunicazione a pacchetti, * provando tutti i comandi disponibili. */ #if 0 while (1) { set_speed(300, 300, 300); get_speed(NULL, NULL, NULL); get_adc(NULL); set_motors(0); get_motors(NULL); } #endif /* * Se non vengono passati parametri il programma funziona in modo * `interattivo', leggendo la posizione del joystick ad azionando di * conseguenza i motori di MARMOT. * * In questa modalita' il programma non termina. * * ATTENZIONE: strani ed imprevedibili spostamenti del robot * possono verificarsi se il joystick non e' collegato al sistema. * Cio' e' dovuto alla fluttuazione dei segnali analogici, a causa * dell'alta impedenza degli ingressi. */ zerox = 0; zeroy = 0; for (i = 0; i < 4; i++) /* esegue quattro letture */ { unsigned char buf[4]; get_adc(buf); zerox += buf[0]; zeroy += buf[1]; } zerox /= 4; /* e media i risultati */ zeroy /= 4; printf("posizione dello zero: %d, %d\n", zerox, zeroy); while (1) { int componentex; int componentey; int deltax; int deltay; unsigned char buf[4]; get_adc(buf); deltax = buf[0] - zerox; deltay = buf[1] - zeroy; if (deltax >= -2 && deltax <= 2) { componentex = 16001; } else { componentex = 10000 / deltax; printf("speed x: %d\n", componentex); } if (deltay >= -2 && deltay <= 2) { componentey = 16001; } else { componentey = 10000 / deltay; printf("speed y: %d\n", componentey); } if (componentex == 16001 && componentey != 16001) { set_speed(-componentey, 16001 , componentey); } if (componentex != 16001 && componentey == 16001) { set_speed(componentex, componentex, componentex); } if (componentex != 16001 && componentey != 16001) { set_speed(componentex + componentey, componentex, componentex - componentey); } if (componentex == 16001 && componentey == 16001) { set_speed(16001, 16001, 16001); } } return EXIT_SUCCESS; }