Rev 1460 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#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
= 0;
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
);
}
ISR
(USART_RXC_vect
) {
if (rx_pos
< RX_BUFFER_SZ
) {
uint8_t i
= UDR
;
if (i
== '+') {
if (pwm_neutral_position
[current_servo
- 1] < 255) {
pwm_neutral_position
[current_servo
- 1]++;
}
printf("SERVO %d VALUE %d\n", current_servo
, pwm_neutral_position
[current_servo
- 1]);
return;
} else if (i
== '-') {
if (pwm_neutral_position
[current_servo
- 1] > 0) {
pwm_neutral_position
[current_servo
- 1]--;
}
printf("SERVO %d VALUE %d\n", current_servo
, pwm_neutral_position
[current_servo
- 1]);
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"
"EXPORT Export srvo 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"
"LOAD Load settings from eeprom\n"
"SAVE Write settings to eeprom\n");
} else if (!strcasecmp
(rx_buffer
, "DISPLAY")) {
for(i
= 0; i
< 6; i
++) {
printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",
(i
+ 1),
(pwm_neutral_position
[i
]),
(pwm_limit
[i
] & 0xff),
(pwm_limit
[i
] >> 8)
);
}
} else if (!strcasecmp
(rx_buffer
, "EXPORT")) {
for(i
= 0; i
< 6; i
++) {
printf("SET %d=%d,%d,%d\n",
(i
+ 1),
(pwm_neutral_position
[i
]),
(pwm_limit
[i
] & 0xff),
(pwm_limit
[i
] >> 8)
);
}
} 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
) + 1;
if (i
>= 1 && i
<= 6) {
current_servo
= i
;
printf("CURRENT SERVO %d\n", i
);
} else {
printf("Invalid servo\n");
}
}
}
} 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
= 0, 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
);
} else {
h
= 0xff;
}
}
pwm_limit
[servo
] = (h
<< 8) | l
;
pwm_check_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");
}
};
/*
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;
}