switched to fast PWM, added workaround for UART transmission errors
This commit is contained in:
Christian Kroll
parent
1752f5a0a1
commit
39796a93be
|
|
@ -8,57 +8,67 @@
|
|||
#include "main.h"
|
||||
#include "uart.h"
|
||||
|
||||
#define POWER_MIN 0
|
||||
#define POWER_MAX 400
|
||||
#define SERVO_STEPS 125
|
||||
#define POWER_MIN 0
|
||||
#define POWER_MAX 400
|
||||
#define SERVO_STEPS 125
|
||||
|
||||
#define BUFSIZE 10
|
||||
#define BUFSIZE 10
|
||||
|
||||
/* ugly workaround for massive out of sequence power values,
|
||||
* USART code seems to forget characters from time to time */
|
||||
#define MAX_ALLOWED_DEVIATION 50
|
||||
#define MAX_FAILS 3
|
||||
|
||||
#define DEBUG2
|
||||
|
||||
volatile uint16_t syscounter = 0;
|
||||
volatile uint16_t power;
|
||||
uint8_t data_count = 0;
|
||||
char data_in[BUFSIZE];
|
||||
|
||||
volatile uint16_t current_pulse_width = 250;
|
||||
|
||||
void reset_input_buffer(void) {
|
||||
data_count = 0;
|
||||
memset(data_in, 0, BUFSIZE);
|
||||
data_count = 0;
|
||||
memset(data_in, 0, BUFSIZE);
|
||||
}
|
||||
|
||||
static void timer_init(void) {
|
||||
// CTC Mode for Timer 1 (16Bit) with prescale of 64
|
||||
TCCR1B |= _BV(WGM12) | _BV(CS11) | _BV(CS10);
|
||||
OCR1A = 2250; // set Servo to max. position
|
||||
TIMSK |= _BV(OCIE1A); // enable timer interrupt
|
||||
TCCR1A |= _BV(COM1A0); // toggle OCR1A on overflow
|
||||
// set Timer 1 to fast PWM mode, with prescale of 64, clear OC1A as soon as
|
||||
// TCNT1 reaches OCR1A, set OC1A as soon as it reaches ICR1
|
||||
// NOTE: one tick is equivalent to 8 usecs
|
||||
TCCR1A = _BV(COM1A1) | _BV(WGM11);
|
||||
TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11) | _BV(CS10);
|
||||
ICR1 = 2500; // period of 20 msecs
|
||||
OCR1A = 250; // inital pulse width of 2 msecs (10% duty cycle)
|
||||
TIMSK |= _BV(TOIE1); // enable overflow interrupt
|
||||
|
||||
// CTC Mode for Timer 0 (8Bit) with prescale of 1024
|
||||
TCCR0B |= _BV(CS02) | _BV(CS00); // prescaler
|
||||
TCCR0A |= _BV(WGM01); // CTC mode
|
||||
TIMSK |= _BV(OCIE0A); // enable timer interrupt
|
||||
TCCR0B |= _BV(CS02) | _BV(CS00); // prescaler
|
||||
TCCR0A |= _BV(WGM01); // CTC mode
|
||||
TIMSK |= _BV(OCIE0A); // enable timer interrupt
|
||||
|
||||
OCR0A = 78; // gives us ~100ms interval
|
||||
}
|
||||
|
||||
static void ports_init(void) {
|
||||
DDRB |= _BV(PB3);
|
||||
DDRB |= _BV(PB3);
|
||||
}
|
||||
|
||||
static void work_uart() {
|
||||
uint16_t c = uart_getc();
|
||||
static uint16_t power;
|
||||
static uint8_t fail_counter = 0;
|
||||
uint16_t new_power = 0, diff = 0;
|
||||
uint16_t c = uart_getc();
|
||||
|
||||
if ( !(c & UART_NO_DATA) ) {
|
||||
if ( !(c & UART_NO_DATA) ) {
|
||||
char cur = c & 0xff;
|
||||
|
||||
data_in[data_count] = cur;
|
||||
data_in[data_count] = cur;
|
||||
//uart_print_uint8(cur);
|
||||
|
||||
data_count++;
|
||||
if(data_count >= BUFSIZE) { // buffer overflow
|
||||
reset_input_buffer();
|
||||
}
|
||||
if(data_count >= BUFSIZE) { // buffer overflow
|
||||
reset_input_buffer();
|
||||
}
|
||||
|
||||
#ifdef DEBUG1
|
||||
for(uint8_t i=0;i<BUFSIZE;i++) {
|
||||
|
|
@ -69,107 +79,84 @@ static void work_uart() {
|
|||
uart_puts_P(" -- \r\n");
|
||||
#endif
|
||||
|
||||
if (cur == 13 || cur == '\n') {
|
||||
if (cur == 13 || cur == '\n') {
|
||||
new_power = atoi(data_in);
|
||||
new_power = new_power <= POWER_MAX ? new_power : POWER_MAX;
|
||||
diff = new_power > power ? new_power - power : power - new_power;
|
||||
|
||||
power = atol(data_in);
|
||||
if(power > POWER_MAX) power = POWER_MAX;
|
||||
|
||||
#ifdef DEBUG
|
||||
uart_puts_P("power = ");
|
||||
uart_print_uint16(power);
|
||||
uart_puts_P("\r\n");
|
||||
if(diff < MAX_ALLOWED_DEVIATION || fail_counter > MAX_FAILS) {
|
||||
#ifdef UART_DEBUG
|
||||
uart_puts_P("Transmitted power = ");
|
||||
uart_print_uint16(power);
|
||||
uart_puts_P("\r\n");
|
||||
#endif
|
||||
if(power > 0) {
|
||||
power = new_power;
|
||||
fail_counter = 0;
|
||||
set_servo(power);
|
||||
}
|
||||
|
||||
reset_input_buffer();
|
||||
}
|
||||
}
|
||||
else {
|
||||
#ifdef UART_DEBUG
|
||||
uart_puts_P("Oooooooops!!! Transmitted power = ");
|
||||
uart_print_uint16(new_power);
|
||||
uart_puts_P("\r\n");
|
||||
#endif
|
||||
fail_counter++;
|
||||
}
|
||||
reset_input_buffer();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief set the servo to a position calculated to given power
|
||||
* \brief set the servo to a position calculated to given power
|
||||
*
|
||||
* \param display The power value from 0 to 400 (including bounds)
|
||||
* \param display The power value from 0 to 400 (including bounds)
|
||||
*/
|
||||
void set_servo(uint16_t display) {
|
||||
int diff;
|
||||
|
||||
if( display > POWER_MAX ) display = POWER_MAX;
|
||||
|
||||
display = POWER_MAX-display; // invert the value / servo
|
||||
|
||||
display = display * 10; // *10 otherwise we need float
|
||||
display = display / ((POWER_MAX * 10) / SERVO_STEPS);
|
||||
display = display + SERVO_STEPS;
|
||||
// invert value and ensure that we don't exceed the limit
|
||||
display = POWER_MAX - (display < POWER_MAX ? display : POWER_MAX);
|
||||
// *10 otherwise we need float
|
||||
display = (display * 10u) / ((POWER_MAX * 10u) / SERVO_STEPS) + SERVO_STEPS;
|
||||
|
||||
#ifdef DEBUG
|
||||
uart_puts_P("display = ");
|
||||
uart_print_uint16(display);
|
||||
uart_puts_P("\r\n");
|
||||
uart_puts_P("display = ");
|
||||
uart_print_uint16(display);
|
||||
uart_puts_P("\r\n");
|
||||
#endif
|
||||
|
||||
if( display < 125 ) display = 125; // just make sure, the timer
|
||||
if( display > 250 ) display = 250; // is never out ouf 20ms grid
|
||||
|
||||
|
||||
// check if timer is currently in the small pulse, then sleep here 2ms
|
||||
// and do again
|
||||
|
||||
|
||||
if(OCR1A <= 2250) {
|
||||
_delay_ms(10);
|
||||
}
|
||||
|
||||
//cli(); // read and write atomic
|
||||
TIMSK &= ~(_BV(OCIE1A));
|
||||
diff = OCR1A - (2500-display);
|
||||
if(diff <=20 && diff >= -20) {
|
||||
OCR1A = 2500-display;
|
||||
}
|
||||
else {
|
||||
if(diff <=20) {
|
||||
OCR1A = OCR1A+5;
|
||||
}
|
||||
else if ( diff >= -20 ) {
|
||||
OCR1A = OCR1A-5;
|
||||
}
|
||||
}
|
||||
//sei();
|
||||
TIMSK |= _BV(OCIE1A);
|
||||
cli();
|
||||
current_pulse_width = display;
|
||||
sei();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief the method moves the servo one complete cycle
|
||||
* \brief the method moves the servo one complete cycle
|
||||
*/
|
||||
static void demo_display(void) {
|
||||
set_servo(0);
|
||||
wait(100);
|
||||
set_servo(400);
|
||||
wait(100);
|
||||
set_servo(0);
|
||||
wait(100);
|
||||
set_servo(0);
|
||||
wait(100);
|
||||
set_servo(400);
|
||||
wait(100);
|
||||
set_servo(0);
|
||||
wait(100);
|
||||
}
|
||||
|
||||
|
||||
int main(void) {
|
||||
sei();
|
||||
|
||||
sei();
|
||||
ports_init();
|
||||
timer_init();
|
||||
uart_init(UART_BAUD_SELECT(38400,F_CPU));
|
||||
reset_input_buffer();
|
||||
//demo_display();
|
||||
ports_init();
|
||||
timer_init();
|
||||
uart_init(UART_BAUD_SELECT(38400,F_CPU));
|
||||
|
||||
reset_input_buffer();
|
||||
// demo_display();
|
||||
|
||||
while(1) {
|
||||
work_uart();
|
||||
while(1) {
|
||||
work_uart();
|
||||
|
||||
if(syscounter >= 10) {
|
||||
reset_input_buffer();
|
||||
uart_putc('a'); // send a to receive values from master box
|
||||
if(syscounter >= 10) {
|
||||
reset_input_buffer();
|
||||
uart_putc('a'); // send a to receive values from master box
|
||||
syscounter = 0;
|
||||
|
||||
#ifdef DEBUG
|
||||
|
|
@ -177,20 +164,14 @@ int main(void) {
|
|||
uart_print_uint16(OCR1A);
|
||||
uart_puts_P("\r\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return(0);
|
||||
return(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief this is our timer for PWM generation and system clock
|
||||
* the system clock varies a bit, but this does not matter
|
||||
*/
|
||||
ISR(TIMER1_COMPA_vect) {
|
||||
OCR1A = 2500-OCR1A; // Das Servosignal wird aus der Differenz von
|
||||
// Periodenlänge (2500*0,008ms=20ms) und letztem
|
||||
// Vergleichswert (OCR1A) gebildet
|
||||
ISR(TIMER1_OVF_vect) {
|
||||
OCR1A = current_pulse_width;
|
||||
}
|
||||
|
||||
ISR(TIMER0_COMPA_vect) {
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ while 1:
|
|||
tdata = ser.read()
|
||||
|
||||
if tdata == 'a':
|
||||
print power
|
||||
print 'Local power = ' + str(power)
|
||||
ser.write(str(power) + '\x0d')
|
||||
#ser.write('100\n')
|
||||
ser.flush()
|
||||
|
|
@ -30,7 +30,9 @@ while 1:
|
|||
if power < 0:
|
||||
power = 0
|
||||
incdec = 0
|
||||
#else:
|
||||
else:
|
||||
sys.stdout.write(tdata)
|
||||
sys.stdout.write(ser.readline())
|
||||
#data_left = ser.inWaiting()
|
||||
#tdata += ser.read(data_left)
|
||||
#print tdata
|
||||
|
|
|
|||
Loading…
Reference in New Issue