motor.c

足球机器人自动程序

#include <signal.h>
#include <io.h>

#define PERIOD_ACC 0xfff0
#define a0 5
#define LAG0 4
#define LAG1 8
#define LAG2 9

#include "st.h"
#include "cmd.h"

unsigned int period1, period2;	
char v1, v2;
char a1, a2;
char vo1, vo2;

char len_cmd;
char buf[3];
char *b_cur;

unsigned char set;
int d1, d2, dd;
void (*pf)();

unsigned char sn, bsn;

char tbuf[16];
unsigned int t_cur, t_end;

static inline unsigned int abs_c(char c) {
	return(c>=0 ? c : -c);
}

static inline void delay(volatile unsigned char t) {
	for(; t>0; --t);
}

int main() {
	// stop wdt
	WDTCTL = WDTPW | WDTHOLD;	

	// osc init
	BCSCTL1 |= XTS;		// ACLK = LFXT1 (HF mode)
	
	do {
		IFG1 &= ~OFIFG;
		delay(0xff);
	} while (IFG1 & OFIFG);

	BCSCTL2 |= SELM_3;	// MCLK = LFXT1

	// uart init
	P3SEL |= 0x30;		// p3 4-5: select option (uart 0)

	U0CTL |= (PENA | PEV | CHAR);	// even parity, 8 bit
	U0TCTL |= SSEL0;	// select clock ACLK

	U0BR0 = 0x90;		// 7680000 / 0x190 = 19200
	U0BR1 = 1;
	U0MCTL = 0;

	ME1 |= (UTXE0 | URXE0);	// enable transmit / recive
	UCTL0 &= ~SWRST;
	IE1 |= (URXIE0 | UTXIE0);	// enable receive / transmit interrupt
	IFG1 &= ~UTXIFG0;

	// ta init
	P1SEL |= 0xc0;	// p1 6,7 = ta 1,2
	P1DIR |= 0xc0;	// p1 6,7 output

	P2DIR |= 0x1b;	// p2 0,1,3,4 output
	P2OUT |= 0x1a;	// en 1,2 disable
			// dir 1,2 = 1,0

	CCTL1 |= OUTMOD_4;	// CCR1 toggle
	CCTL2 |= OUTMOD_4;	// CCR2 toggle
	TACTL |= (TASSEL_1 | ID_2 | MC_2);	// ACLK / 4, stop mode

	// adc init
	P6SEL |= 0xf0;	// select adc
	ADC12CTL0 = ADC12ON | MSC | SHT0_4 | REFON | REF2_5V;	// adc on, multi ad, sample and hold time 4, ref on, 2.5V
	ADC12CTL1 = SHP | ADC12SSEL_1 | CONSEQ_1 | ADC12DIV_1;	// sampling timer, aclk / 2, sequence of channels
	ADC12MCTL0 = SREF_1 | INCH_4;
	ADC12MCTL1 = SREF_1 | INCH_5;
	ADC12MCTL2 = SREF_1 | INCH_6 | EOS;
	ADC12IE = 0x4;
	ADC12CTL0 |= ENC;                     // Enable conversions

	_BIS_SR(LPM3_bits + GIE);
	return 0;
}

static inline void echo2() {
	if (t_end)
		tbuf[t_end++] = bsn;
	else
		TXBUF0 = bsn;
	tbuf[t_end++] = 2;
}

interrupt(TIMERA0_VECTOR) timer_a0() {

	CCR0 += PERIOD_ACC;

	if (a1) {
		char t = v1 + a1;
		if ( (a1 < 0 && t <= vo1) || (a1 > 0 && t >= vo1) ) {	// acc end
			a1 = 0;
			if (!a2) {
				CCTL0 &= ~CCIE;
				if (set == 2) {
					set = 0;
					echo2();
				}
			}

			t = vo1;
			// stop wheel1 if vo1 is 0
			if (!t) {	
				CCTL1 &= ~CCIE;
				P2OUT |= 0x02;
			}
		}

		if ( (v1 >= 0 && t < 0) || (v1 < 0 && t >= 0) ) {
			P2OUT ^= 0x10;
		}
		v1 = t;
		period1 = st[abs_c(v1)];
	}

	if (a2) {
		char t = v2 + a2;
		if ( (a2 < 0 && t <= vo2) || (a2 > 0 && t >= vo2) ) {
			a2 = 0;
			if (!a1) {
				CCTL0 &= ~CCIE;
				if (set == 2) {
					set = 0;
					echo2();
				}
			}

			t = vo2;
			// stop wheel2 if vo2 is 0
			if (!t) {
				CCTL2 &= ~CCIE;
				P2OUT |= 0x8;
			}
		}

		if ( (v2 >= 0 && t < 0) || (v2 < 0 && t >= 0) ) {
			P2OUT ^= 0x1;
		}
		v2 = t;
		period2 = st[abs_c(v2)];
	}
}

interrupt(TIMERA1_VECTOR) timer_a1() {

	switch(TAIV) {
		case  2:
			CCR1 += period1;
			if (v1 >= 0) 
				++d1;
			else
				--d1;
			if (set == 1)
				pf();
			return;
		case  4:
			CCR2 += period2;
			if (v2 >= 0) 
				++d2;
			else
				--d2;
			if (set == 1)
				pf();
	}
}

static inline void set_angle(int di) {
	// 4 * 29.8 / 12.5 / 0.225 = 42.3822222222222
	dd = di;
	dd += di << 8;
	dd += di << 7;
	dd += di << 2;
	dd += di << 1;
	dd += 1 << 9;
	dd >>= 10;

	dd += di << 5;
	dd += di << 3;
	dd += di << 1;
}

static inline void set_distance(int di) {
	// 2 * 360 / (pi *12.5 * 0.225) = 81.4873308630504
	dd = di;
	dd += di << 4;
	dd += di << 3;
	dd += di << 2;
	dd += di << 1;
	dd += 1 << 5;
	dd >>= 6;

	dd += di << 6;
	dd += di << 4;
	dd += di;
}

void process_angle() {
	if (dd == (d1 - d2)) {
		set = 0;
		d1 = d2 = 0;
		echo2();
	}
}

void process_distance() {
	if (dd == d1) {
		set = 0;
		d1 = d2 = 0;
		echo2();
	}
}

static inline void echo67() {

	if (t_end)
		tbuf[t_end++] = ++sn;
	else
		TXBUF0 = ++sn;
	if (set) {
		tbuf[t_end++] = 7;
		tbuf[t_end++] = bsn;
	}
	else
		tbuf[t_end++] = 6;
	tbuf[t_end++] = (d1 >> 8);
	tbuf[t_end++] = d1;
	tbuf[t_end++] = (d2 >> 8);
	tbuf[t_end++] = d2;
	d1 = d2 = 0;
}

static inline void start_acc() {

	vo1 = buf[0];
	vo2 = buf[1];

	buf[0] -= v1;
	buf[1] -= v2;

	if (!buf[0] && !buf[1]) {
		CCTL0 &= ~CCIE;
		if (set == 2) {
			set = 0;
			echo2();
		}
		return;
	}

	a1 = buf[0] > 0 ? a0 : buf[0] < 0 ? -a0 : 0;
	a2 = buf[1] > 0 ? a0 : buf[1] < 0 ? -a0 : 0;

	if (a1 && !(CCTL1 & CCIE)) {	// wheel 1 stop
		v1 = a1;
		// set dir1
		if (v1 >= 0)
			P2OUT |= 0x10;
		else
			P2OUT &= ~0x10;
		period1 = st[abs_c(v1)];

		// set en1
		P2OUT &= ~0x2;
		CCR1 = TAR + LAG1;
		CCTL1 |= CCIE;
	}

	if (a2 && !(CCTL2 & CCIE)) {
		v2 = a2;
		// set dir2
		if (v2 >= 0)
			P2OUT &= ~0x1;
		else
			P2OUT |= 0x1;
		period2 = st[abs_c(v2)];

		// set en2
		P2OUT &= ~0x8;
		CCR2 = TAR + LAG2;
		CCTL2 |= CCIE;
	}

	CCR0 = TAR + LAG0;
	CCTL0 |= CCIE;
}

interrupt(UART0RX_VECTOR) uart0_rx() {

	char t = RXBUF0;

	if (abs_c(t) <= DATA_MAX) {
		if (len_cmd > 0) {
			*(b_cur++) = t;
			--len_cmd;
		}
		return;
	}

	if (t > DATA_MAX) {
		switch (t) {
			case STOP:
				CCTL0 &= ~CCIE;
				CCTL1 &= ~CCIE;
				CCTL2 &= ~CCIE;
				P2OUT |= 0x0a;	// disable 1,2
				v1 = v2 = 0;
				set = 0;
				break;
			case HEAD2:
				len_cmd = 2;
				b_cur = buf;
				break;
			case HEAD3:
				len_cmd = 3;
				b_cur = buf;
				break;
			case IR:
				ADC12CTL0 |= ADC12SC;
				break;
			case RV:
				if (t_end)
					tbuf[t_end++] = ++sn;
				else
					TXBUF0 = ++sn;
				tbuf[t_end++] = 4;
				tbuf[t_end++] = v1;
				tbuf[t_end++] = v2;
				break;
			case RD:
				if (t_end)
					tbuf[t_end++] = ++sn;
				else
					TXBUF0 = ++sn;
				tbuf[t_end++] = M_RD | 6;
				tbuf[t_end++] = (d1 >> 8);
				tbuf[t_end++] = d1;
				tbuf[t_end++] = (d2 >> 8);
				tbuf[t_end++] = d2;
				break;
			case VER:
				if (t_end)
					tbuf[t_end++] = ++sn;
				else
					TXBUF0 = ++sn;
				tbuf[t_end++] = 3;
				tbuf[t_end++] = VERSION;
				break;
			case CLR:
				len_cmd = -1;
				t_cur = t_end = 0;
				sn = 0;
			}
			return;
	}

	// prevent premature cmd tail
	if (len_cmd) {
		len_cmd = -1;
		return;
	}

	echo67();
	switch (t) {
		case AA:
			set = 0;
			break;
		case AA_ANGLE:
			bsn = sn;
			set_angle(buf[2]);
			pf = process_angle;
			set = 1;
			break;
		case AA_DISTANCE:
			bsn = sn;
			set_distance(buf[2]);
			pf = process_distance;
			set = 1;
			break;
		case AA_ONLY:
			bsn = sn;
			set = 2;
	}
	start_acc();
	len_cmd = -1;
}

interrupt(ADC_VECTOR) adc12_isr() {
	if (t_end)
		tbuf[t_end++] = ++sn;
	else
		TXBUF0 = ++sn;
	tbuf[t_end++] = 5;
	tbuf[t_end++] = (ADC12MEM0 >> 4);
	tbuf[t_end++] = (ADC12MEM1 >> 4);
	tbuf[t_end++] = (ADC12MEM2 >> 4);
}

interrupt(UART0TX_VECTOR) usart0_tx() {
	if (t_cur >= t_end) {
		t_cur = t_end = 0;
		return;
	}

	TXBUF0 = tbuf[t_cur++];
}