ctrl1.c

足球机器人自动程序

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

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

int fd;
struct termios tio;
char lspeed, rspeed;

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

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;

	printf("control program version %d\n", VERSION);

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

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

	// 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 = 0;
	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(n);
					exit(-1);
				}
				if (i + k >= n + 2) {	// current len not enough
					printf("notice: complex \n");
					print(n);
					res_n = n + 1 - i;
					memmove(rbuf, rbuf+i-1, res_n);
					printf("move trailing %d bytes\n", res_n);
					print(res_n);
					break;
				}

				switch (k) {
					case 2:
						if (type != M_DONE) {
							fprintf(stderr, "echo error 2\n");
							print(n);
							exit(-1);
						}
						printf("echo 2: %x\n", sn);
						break;
					case 6:
						if (!(type == 0 || type == M_IR)) {
							fprintf(stderr, "echo error 6\n");
							print(n);
							exit(-1);
						}
						if (type == M_IR) {
							printf("IR: %x\n", sn);
							//printf("IR: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);
						}
						else {
							bsn = sn;
							printf("echo 6: %x\n", sn);
							//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(n);
							exit(-1);
						}
						bsn = sn;
						printf("echo 7: %x\n", sn);
						//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(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");
				lspeed = rspeed = 0;
				buf[0] = STOP;
				msend(buf, 1);
				break;
			case 'k':
				printf("k\n");
				lspeed += 20;
				rspeed += 20;
				buf[0] = HEAD3;
				buf[1] = lspeed;
				buf[2] = rspeed;
				buf[3] = 2;
				buf[4] = AA_DISTANCE;
				msend(buf, 5);
				++sn;
				break;
			case 'j':
				printf("j\n");
				lspeed -= 20;
				rspeed -= 20;
				buf[0] = HEAD3;
				buf[1] = lspeed;
				buf[2] = rspeed;
				buf[3] = -2;
				buf[4] = AA_DISTANCE;
				msend(buf, 5);
				++sn;
				break;
			case 'h':
				printf("h\n");
				lspeed -= 20;
				rspeed += 20;
				buf[0] = HEAD3;
				buf[1] = lspeed;
				buf[2] = rspeed;
				buf[3] = -5;
				buf[4] = AA_ANGLE;
				msend(buf, 5);
				++sn;
				break;
			case 'l':
				printf("l\n");
				lspeed += 20;
				rspeed -= 20;
				buf[0] = HEAD3;
				buf[1] = lspeed;
				buf[2] = rspeed;
				buf[3] = 5;
				buf[4] = AA_ANGLE;
				msend(buf, 5);
				++sn;
				break;
			case 'i':
				printf("i\n");
				buf[0] = IR;
				msend(buf, 1);
				++sn;
				break;
			default:
				printf("key undefined: %c\n", c);
			}
		}
	}

	return 0;
}