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