WebSVN - FlightCtrl - Diff - Rev 1477 and 1478 - /branches/V0.76g-acid/servoboard/uart.c


 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "servoboard.h"
#include "uart.h"
 
uint8_t rx_buffer[RX_BUFFER_SZ];
//uint8_t tx_buffer[TX_BUFFER_SZ];
uint8_t rx_pos, tx_pos;
uint8_t current_servo = 1;
 
void uart_init() {
 
        rx_pos = 0;
        tx_pos = 0;
 
        UCR=(1 << TXEN) | (1 << RXEN);
        USR   |= (1<<U2X);
        UCSRB |= (1<<RXCIE);
//      UCSRB |= (1<<TXCIE);
 
        UBRR = (F_CPU / (BAUD_RATE * 8L) - 1);
 
        fdevopen(uart_putchar, NULL);
 
        printf("servoboard %s\n> ", VERSION);
 
 
}
 
void display_cur_servo(uint8_t i) {
        printf("SERVO %d NEUTRAL %d LIMIT %d-%d\n", i + 1, pwm_neutral_position[i], (pwm_limit[i] & 0xff), (pwm_limit[i] >> 8));
}
 
 
ISR(USART_RXC_vect) {
 
        uint8_t i = UDR;
 
        if (rx_pos < RX_BUFFER_SZ) {
 
                if (i == '+' || i == '-') {
                        if (i == '+') {
                                if (pwm_neutral_position[current_servo] < 255) {
                                        pwm_neutral_position[current_servo]++;
                                }
                        } else if (i == '-') {
                                if (pwm_neutral_position[current_servo] > 0) {
                                        pwm_neutral_position[current_servo]--;
                                }
                        }
                        display_cur_servo(current_servo);
                        return;
                }
 
                uart_putchar(i);
 
                if (i == '\r') {
 
                        uart_putchar('\n');
 
                        rx_buffer[rx_pos] = 0;
                        rx_pos = 0;
 
                        if (!strcasecmp(rx_buffer, "HELP")) {
                                printf("DISPLAY         Display servo settings\n"
                                           "SET s=n[,l,u]   Set servo settings\n"
                                           "                   s = servo number (1-6)\n"
                                           "                   n = neutral position (0-255)\n"
                                           "                   l = lower limit (0-255)\n"
                                           "                   u = upper limit (0-255)\n"
                                           "CUR n           Set current servo. + = increase, - = decrease value\n"
                                           "SETLL           Set lower limit to servo position\n"
                                           "SETUL           Set upper limit\n"
                                           "LOAD            Load settings from eeprom\n"
                                           "SAVE            Write settings to eeprom\n");
                        } else if (!strcasecmp(rx_buffer, "DISPLAY")) {
                                for(i = 0; i < 6; i++) {
                                        display_cur_servo(i);
                                }
#if DEBUG_SIGNAL
                        } else if (!strcasecmp(rx_buffer, "TV")) {
                                display_values = !display_values;
                                printf("\n");
#endif
                        } else if (!strcasecmp(rx_buffer, "LOAD")) {
                                eeprom_init();
                        } else if (!strcasecmp(rx_buffer, "SAVE")) {
                                eeprom_write();
                        } else if (!strncasecmp(rx_buffer, "CUR ", 4)) {
                                char *s, *t;
                                s = strtok_r(rx_buffer, " ", &t);
                                if (s) {
                                        s = strtok_r(NULL, ",", &t);
                                        if (s) {
                                                i = atoi(s);
                                                if (i >= 1 && i <= 6) {
                                                        current_servo = i - 1;
                                                        printf("CURRENT SERVO %d\n", i);
                                                } else {
                                                        printf("Invalid servo\n");
                                                }
                                        }
                                }
                        } else if (!strcasecmp(rx_buffer, "SETLL")) {
                                uint8_t l, h;
                                l = pwm_neutral_position[current_servo];
                                h = (pwm_limit[current_servo] >> 8);
                                if (h < l) {
                                        h = l;
                                }
                                pwm_limit[current_servo] = (h << 8) | l;
                                display_cur_servo(current_servo);
                        } else if (!strcasecmp(rx_buffer, "SETUL")) {
                                uint8_t l, h;
                                h = pwm_neutral_position[current_servo];
                                l = (pwm_limit[current_servo] & 0xff);
                                if (l > h) {
                                        l = h;
                                }
                                pwm_limit[current_servo] = (h << 8) | l;
                                display_cur_servo(current_servo);
                        } else if (!strncasecmp(rx_buffer, "SET ", 4)) {
                                char *s, *t;
                                uint8_t servo;
                                s = strtok_r(rx_buffer, " ", &t);
                                if (s) {
                                        s = strtok_r(NULL, "=", &t);
                                        if (s) {
                                                servo = atoi(s);
                                                if (servo >= 1 && servo <= 6) {
                                                        servo--;
                                                        s = strtok_r(NULL, ",", &t);
                                                        if (s) {
                                                                uint8_t h = 0xff, l = 0;
                                                                pwm_neutral_position[servo] = atoi(s);
                                                                s = strtok_r(NULL, ",", &t);
                                                                if (s) {
                                                                        l = atoi(s);
                                                                        s = strtok_r(NULL, "", &t);
                                                                        if (s) {
                                                                                h = atoi(s);
                                                                        }
                                                                }
                                                                pwm_limit[servo] = (h << 8) | l;
                                                                set_pwm_active();
                                                                i = servo;
                                                                printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",
                                                                        (i + 1),
                                                                        (pwm_neutral_position[i]),
                                                                        (pwm_limit[i] & 0xff),
                                                                        (pwm_limit[i] >> 8)
                                                                );
                                                        }
                                                } else {
                                                        printf("Invalid servo\n");
                                                }
                                        }
                                }
                        } else {
                                printf("Invalid command, type HELP\n");
                        }
 
                        uart_putchar('>');
                        uart_putchar(' ');
 
                } else {
 
                        rx_buffer[rx_pos++] = i;
 
                }
 
        } else {
 
                printf("Receive buffer full.\n");
                rx_pos = 0;
 
        }
 
};
 
/*
ISR(USART_TXC_vect) {
}*/
 
int uart_putchar (char c) {
        if (c == '\n') {
                uart_putchar('\r');
        }
        loop_until_bit_is_set(USR, UDRE);
        UDR = c;
        return 0;
}
 
 

Rev 1477	Rev 1478
1		1
2	#include <stdio.h>	2	#include <stdio.h>
3	#include <stdlib.h>	3	#include <stdlib.h>
4	#include <string.h>	4	#include <string.h>
5	#include <avr/io.h>	5	#include <avr/io.h>
6	#include <avr/interrupt.h>	6	#include <avr/interrupt.h>
7	#include "servoboard.h"	7	#include "servoboard.h"
8	#include "uart.h"	8	#include "uart.h"
9		9
10	uint8_t rx_buffer[RX_BUFFER_SZ];	10	uint8_t rx_buffer[RX_BUFFER_SZ];
11	//uint8_t tx_buffer[TX_BUFFER_SZ];	11	//uint8_t tx_buffer[TX_BUFFER_SZ];
12	uint8_t rx_pos, tx_pos;	12	uint8_t rx_pos, tx_pos;
13	uint8_t current_servo = 1;	13	uint8_t current_servo = 1;
14		14
15	void uart_init() {	15	void uart_init() {
16		16
17	rx_pos = 0;	17	rx_pos = 0;
18	tx_pos = 0;	18	tx_pos = 0;
19		19
20	UCR=(1 << TXEN) \| (1 << RXEN);	20	UCR=(1 << TXEN) \| (1 << RXEN);
21	USR \|= (1<<U2X);	21	USR \|= (1<<U2X);
22	UCSRB \|= (1<<RXCIE);	22	UCSRB \|= (1<<RXCIE);
23	// UCSRB \|= (1<<TXCIE);	23	// UCSRB \|= (1<<TXCIE);
24		24
25	UBRR = (F_CPU / (BAUD_RATE * 8L) - 1);	25	UBRR = (F_CPU / (BAUD_RATE * 8L) - 1);
26		26
27	fdevopen(uart_putchar, NULL);	27	fdevopen(uart_putchar, NULL);
28		28
29	printf("servoboard %s\n> ", VERSION);	29	printf("servoboard %s\n> ", VERSION);
30		30
31		31
32	}	32	}
33		33
34	void display_cur_servo(uint8_t i) {	34	void display_cur_servo(uint8_t i) {
35	printf("SERVO %d NEUTRAL %d LIMIT %d-%d\n", i + 1, pwm_neutral_position[i], (pwm_limit[i] & 0xff), (pwm_limit[i] >> 8));	35	printf("SERVO %d NEUTRAL %d LIMIT %d-%d\n", i + 1, pwm_neutral_position[i], (pwm_limit[i] & 0xff), (pwm_limit[i] >> 8));
36	}	36	}
37		37
38		38
39	ISR(USART_RXC_vect) {	39	ISR(USART_RXC_vect) {
40		40
41	uint8_t i = UDR;	41	uint8_t i = UDR;
42		42
43	if (rx_pos < RX_BUFFER_SZ) {	43	if (rx_pos < RX_BUFFER_SZ) {
44		44
45	if (i == '+' \|\| i == '-') {	45	if (i == '+' \|\| i == '-') {
46	if (i == '+') {	46	if (i == '+') {
47	if (pwm_neutral_position[current_servo] < 255) {	47	if (pwm_neutral_position[current_servo] < 255) {
48	pwm_neutral_position[current_servo]++;	48	pwm_neutral_position[current_servo]++;
49	}	49	}
50	} else if (i == '-') {	50	} else if (i == '-') {
51	if (pwm_neutral_position[current_servo] > 0) {	51	if (pwm_neutral_position[current_servo] > 0) {
52	pwm_neutral_position[current_servo]--;	52	pwm_neutral_position[current_servo]--;
53	}	53	}
54	}	54	}
55	display_cur_servo(current_servo);	55	display_cur_servo(current_servo);
56	return;	56	return;
57	}	57	}
58		58
59	uart_putchar(i);	59	uart_putchar(i);
60		60
61	if (i == '\r') {	61	if (i == '\r') {
62		62
63	uart_putchar('\n');	63	uart_putchar('\n');
64		64
65	rx_buffer[rx_pos] = 0;	65	rx_buffer[rx_pos] = 0;
66	rx_pos = 0;	66	rx_pos = 0;
67		67
68	if (!strcasecmp(rx_buffer, "HELP")) {	68	if (!strcasecmp(rx_buffer, "HELP")) {
69	printf("DISPLAY Display servo settings\n"	69	printf("DISPLAY Display servo settings\n"
70	"SET s=n[,l,u] Set servo settings\n"	70	"SET s=n[,l,u] Set servo settings\n"
71	" s = servo number (1-6)\n"	71	" s = servo number (1-6)\n"
72	" n = neutral position (0-255)\n"	72	" n = neutral position (0-255)\n"
73	" l = lower limit (0-255)\n"	73	" l = lower limit (0-255)\n"
74	" u = upper limit (0-255)\n"	74	" u = upper limit (0-255)\n"
75	"CUR n Set current servo. + = increase, - = decrease value\n"	75	"CUR n Set current servo. + = increase, - = decrease value\n"
76	"SETLL Set lower limit to servo position\n"	76	"SETLL Set lower limit to servo position\n"
77	"SETUL Set upper limit\n"	77	"SETUL Set upper limit\n"
78	"LOAD Load settings from eeprom\n"	78	"LOAD Load settings from eeprom\n"
79	"SAVE Write settings to eeprom\n");	79	"SAVE Write settings to eeprom\n");
80	} else if (!strcasecmp(rx_buffer, "DISPLAY")) {	80	} else if (!strcasecmp(rx_buffer, "DISPLAY")) {
81	for(i = 0; i < 6; i++) {	81	for(i = 0; i < 6; i++) {
82	display_cur_servo(i);	82	display_cur_servo(i);
83	}	83	}
84	#if DEBUG_SIGNAL	84	#if DEBUG_SIGNAL
85	} else if (!strcasecmp(rx_buffer, "TV")) {	85	} else if (!strcasecmp(rx_buffer, "TV")) {
86	display_values = !display_values;	86	display_values = !display_values;
87	printf("\n");	87	printf("\n");
88	#endif	88	#endif
89	} else if (!strcasecmp(rx_buffer, "LOAD")) {	89	} else if (!strcasecmp(rx_buffer, "LOAD")) {
90	eeprom_init();	90	eeprom_init();
91	} else if (!strcasecmp(rx_buffer, "SAVE")) {	91	} else if (!strcasecmp(rx_buffer, "SAVE")) {
92	eeprom_write();	92	eeprom_write();
93	} else if (!strncasecmp(rx_buffer, "CUR ", 4)) {	93	} else if (!strncasecmp(rx_buffer, "CUR ", 4)) {
94	char s, t;	94	char s, t;
95	s = strtok_r(rx_buffer, " ", &t);	95	s = strtok_r(rx_buffer, " ", &t);
96	if (s) {	96	if (s) {
97	s = strtok_r(NULL, ",", &t);	97	s = strtok_r(NULL, ",", &t);
98	if (s) {	98	if (s) {
99	i = atoi(s);	99	i = atoi(s);
100	if (i >= 1 && i <= 6) {	100	if (i >= 1 && i <= 6) {
101	current_servo = i - 1;	101	current_servo = i - 1;
102	printf("CURRENT SERVO %d\n", i);	102	printf("CURRENT SERVO %d\n", i);
103	} else {	103	} else {
104	printf("Invalid servo\n");	104	printf("Invalid servo\n");
105	}	105	}
106	}	106	}
107	}	107	}
108	} else if (!strcasecmp(rx_buffer, "SETLL")) {	108	} else if (!strcasecmp(rx_buffer, "SETLL")) {
109	uint8_t l, h;	109	uint8_t l, h;
110	l = pwm_neutral_position[current_servo];	110	l = pwm_neutral_position[current_servo];
111	h = (pwm_limit[current_servo] >> 8);	111	h = (pwm_limit[current_servo] >> 8);
112	if (h < l) {	112	if (h < l) {
113	h = l;	113	h = l;
114	}	114	}
115	pwm_limit[current_servo] = (h << 8) \| l;	115	pwm_limit[current_servo] = (h << 8) \| l;
116	display_cur_servo(current_servo);	116	display_cur_servo(current_servo);
117	} else if (!strcasecmp(rx_buffer, "SETUL")) {	117	} else if (!strcasecmp(rx_buffer, "SETUL")) {
118	uint8_t l, h;	118	uint8_t l, h;
119	h = pwm_neutral_position[current_servo];	119	h = pwm_neutral_position[current_servo];
120	l = (pwm_limit[current_servo] & 0xff);	120	l = (pwm_limit[current_servo] & 0xff);
121	if (l > h) {	121	if (l > h) {
122	l = h;	122	l = h;
123	}	123	}
124	pwm_limit[current_servo] = (h << 8) \| l;	124	pwm_limit[current_servo] = (h << 8) \| l;
125	display_cur_servo(current_servo);	125	display_cur_servo(current_servo);
126	} else if (!strncasecmp(rx_buffer, "SET ", 4)) {	126	} else if (!strncasecmp(rx_buffer, "SET ", 4)) {
127	char s, t;	127	char s, t;
128	uint8_t servo;	128	uint8_t servo;
129	s = strtok_r(rx_buffer, " ", &t);	129	s = strtok_r(rx_buffer, " ", &t);
130	if (s) {	130	if (s) {
131	s = strtok_r(NULL, "=", &t);	131	s = strtok_r(NULL, "=", &t);
132	if (s) {	132	if (s) {
133	servo = atoi(s);	133	servo = atoi(s);
134	if (servo >= 1 && servo <= 6) {	134	if (servo >= 1 && servo <= 6) {
135	servo--;	135	servo--;
136	s = strtok_r(NULL, ",", &t);	136	s = strtok_r(NULL, ",", &t);
137	if (s) {	137	if (s) {
138	uint8_t h = 0xff, l = 0;	138	uint8_t h = 0xff, l = 0;
139	pwm_neutral_position[servo] = atoi(s);	139	pwm_neutral_position[servo] = atoi(s);
140	s = strtok_r(NULL, ",", &t);	140	s = strtok_r(NULL, ",", &t);
141	if (s) {	141	if (s) {
142	l = atoi(s);	142	l = atoi(s);
143	s = strtok_r(NULL, "", &t);	143	s = strtok_r(NULL, "", &t);
144	if (s) {	144	if (s) {
145	h = atoi(s);	145	h = atoi(s);
146	}	146	}
147	}	147	}
148	pwm_limit[servo] = (h << 8) \| l;	148	pwm_limit[servo] = (h << 8) \| l;
149	set_pwm_active();	149	set_pwm_active();
150	i = servo;	150	i = servo;
151	printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",	151	printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",
152	(i + 1),	152	(i + 1),
153	(pwm_neutral_position[i]),	153	(pwm_neutral_position[i]),
154	(pwm_limit[i] & 0xff),	154	(pwm_limit[i] & 0xff),
155	(pwm_limit[i] >> 8)	155	(pwm_limit[i] >> 8)
156	);	156	);
157	}	157	}
158	} else {	158	} else {
159	printf("Invalid servo\n");	159	printf("Invalid servo\n");
160	}	160	}
161	}	161	}
162	}	162	}
163	} else {	163	} else {
164	printf("Invalid command, type HELP\n");	164	printf("Invalid command, type HELP\n");
165	}	165	}
166		166
167	uart_putchar('>');	167	uart_putchar('>');
168	uart_putchar(' ');	168	uart_putchar(' ');
169		169
170	} else {	170	} else {
171		171
172	rx_buffer[rx_pos++] = i;	172	rx_buffer[rx_pos++] = i;
173		173
174	}	174	}
175		175
176	} else {	176	} else {
177		177
178	printf("Receive buffer full.\n");	178	printf("Receive buffer full.\n");
179	rx_pos = 0;	179	rx_pos = 0;
180		180
181	}	181	}
182		182
183	};	183	};
184		184
185	/*	185	/*
186	ISR(USART_TXC_vect) {	186	ISR(USART_TXC_vect) {
187	}*/	187	}*/
188		188
189	int uart_putchar (char c) {	189	int uart_putchar (char c) {
190	if (c == '\n') {	190	if (c == '\n') {
191	uart_putchar('\r');	191	uart_putchar('\r');
192	}	192	}
193	loop_until_bit_is_set(USR, UDRE);	193	loop_until_bit_is_set(USR, UDRE);
194	UDR = c;	194	UDR = c;
195	return 0;	195	return 0;
196	}	196	}
197		197
198		198

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-acid/servoboard/uart.c - Rev 1477 → 1478