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