ctrl2.c

足球机器人自动程序

#include "cmd.h"
#include "tty.h"
#include "ctrl.h"

#include <stdlib.h>
#include <sys/poll.h>
#include <signal.h>

#define V 10
#define DIS 3
#define ANG 10

int fd;
struct termios tio;

char buf[20];
unsigned char rbuf[20];

char bbuf[4] = {HEAD2, 0, 0, AA_ONLY};
int bbsn;

inline void cleanup() {
	buf[0] = STOP;
	write(fd, buf, 1);

	tcsetattr(0, TCSANOW, &tio);
	printf("\nclean up\n");
}

void msend(void *buf, int n) {
	int t;
	if ((t = write(fd, buf, n)) != n) {
		if (t == -1)
			perror("write");
		else
			fprintf(stderr, "write error %d\n", t);
		exit(-1);
	}
}

int main() {

	unsigned char sn, bsn;
	int res_n;

	// init ttys
	if ((fd = ttys_init(0)) < 0) {
		return -1;
	}

	// init ctrl
	if (ctrl_init(fd) < 0) {
		return -1;
	}

	// init tty (key control)
	tty_set(&tio);

	// clean up at exit
	if (atexit(cleanup)) {
		fprintf(stderr, "atexit error\n");
		return -1;
	}

	// clean up at ^c exit
	if (signal(SIGINT, exit) == SIG_ERR) {
		fprintf(stderr, "signal error\n");
		return -1;
	}

	struct pollfd ufds[2] = {{0, POLLIN}, {fd, POLLIN}};

	sn = bsn = 1;
	res_n = 0;
	for (;;) {
		if (poll(ufds, 2, -1) <= 0) {
			fprintf(stderr, "poll error\n");
			exit(-1);
		}

		if (ufds[1].revents & POLLIN) {
			int n;
			if ((n = read(fd, rbuf + res_n, sizeof(rbuf) - res_n)) == -1) {
				perror("read");
				exit(-1);
			}

			n += res_n;
			res_n = 0;
			int i = 1;
			for (;;) {

				int type = rbuf[i] & 0xc0;
				int k = rbuf[i] & 0x3f;
				if (k > 7) {
					fprintf(stderr, "echo len too long\n");
					print_buf(rbuf, n);
					exit(-1);
				}
				if (i + k >= n + 2) {	// current len not enough
					printf("notice: complex \n");
					print_buf(rbuf, n);
					res_n = n + 1 - i;
					copy_buf(rbuf, rbuf+i-1, res_n);
					printf("move trailing %d bytes\n", res_n);
					print_buf(rbuf, res_n);
					break;
				}

				switch (k) {
					case 2:
						if (type != 0) {
							fprintf(stderr, "echo error 2\n");
							print_buf(rbuf, n);
							exit(-1);
						}
						printf("%x done\n", rbuf[i-1]);

						// stop on demand
						if (bbsn == bsn) {
							msend(bbuf, 4);
							++sn;
							printf("stop on done\n");
						}
						break;
					case 3:
						if (rbuf[i-1] != sn) {
							fprintf(stderr, "echo error 3\n");
							print_buf(rbuf, n);
							exit(-1);
						}
						break;
					case 5:
						printf("IR: %x\n", sn);
						//printf("IR: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);
						break;
					case 6:
						if (!(type == 0 || type == M_RD)) {
							fprintf(stderr, "echo error 6\n");
							print_buf(rbuf, n);
							exit(-1);
						} 
						if (type == M_RD) {
							printf("%x RD\n", rbuf[i-1]);
							//printf("RD: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);
						}
						else {
							bsn = rbuf[i-1];
							printf("%x begin clean\n", bsn);
							//printf("clear: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);
						}
						break;
					case 7:
						if (type != 0) {
							fprintf(stderr, "echo error 7\n");
							print_buf(rbuf, n);
							exit(-1);
						}
						bsn = rbuf[i-1];
						printf("%x begin dirty(%x)\n", bsn, rbuf[i+1]);
						//printf("dirty: %d %d %d %d\n", rbuf[i+2], rbuf[i+3], rbuf[i+4], rbuf[i+5]);
						break;
					default:
						printf("what is this?\n");
						print_buf(rbuf, n);
						exit(-1);
				}
				
				i += k;
				if (i > n) break;
				else if (i == n) {	// trailing 1
					res_n = 1;
					rbuf[0] = rbuf[n-1];
					break;
				}
			}
		}

		if (ufds[0].revents & POLLIN) {
			char c;
			if (read(0, &c, 1) != 1) {
				fprintf(stderr, "read error\n");
				exit(-1);
			}
				
			switch (c) {
			case 4:
			case 'q':
				exit(0);
			case 'p':
			case ' ':
				printf("stop\n");
				buf[0] = STOP;
				msend(buf, 1);
				break;
			case 'k':
				buf[0] = HEAD3;
				buf[1] = V;
				buf[2] = V;
				buf[3] = DIS;
				buf[4] = AA_DISTANCE;
				msend(buf, 5);
				++sn;
				printf("k %x\n", sn);
				bbsn = sn;
				break;
			case 'j':
				buf[0] = HEAD3;
				buf[1] = -V;
				buf[2] = -V;
				buf[3] = -DIS;
				buf[4] = AA_DISTANCE;
				msend(buf, 5);
				++sn;
				printf("j %x\n", sn);
				bbsn = sn;
				break;
			case 'h':
				buf[0] = HEAD3;
				buf[1] = -V;
				buf[2] = V;
				buf[3] = -ANG;
				buf[4] = AA_ANGLE;
				msend(buf, 5);
				++sn;
				printf("h %x\n", sn);
				bbsn = sn;
				break;
			case 'l':
				buf[0] = HEAD3;
				buf[1] = V;
				buf[2] = -V;
				buf[3] = ANG;
				buf[4] = AA_ANGLE;
				msend(buf, 5);
				++sn;
				printf("l %x\n", sn);
				bbsn = sn;
				break;
			case 'i':
				buf[0] = IR;
				msend(buf, 1);
				++sn;
				printf("i %x\n", sn);
				break;
			default:
				printf("key undefined: %c\n", c);
			}
		}
	}

	return 0;
}