Rev 1477 |
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
= 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;
}