From 39796a93be70b7e17a188d745e5506484d22a644 Mon Sep 17 00:00:00 2001 From: Christian Kroll Date: Sun, 15 Jun 2014 04:27:26 +0200 Subject: [PATCH] switched to fast PWM, added workaround for UART transmission errors --- .../displayboard_servo/software/src/main.c | 225 ++++++++---------- schaltungen/displayboard_servo/tester.py | 6 +- 2 files changed, 107 insertions(+), 124 deletions(-) diff --git a/schaltungen/displayboard_servo/software/src/main.c b/schaltungen/displayboard_servo/software/src/main.c index 18eb5e8..4879a8c 100644 --- a/schaltungen/displayboard_servo/software/src/main.c +++ b/schaltungen/displayboard_servo/software/src/main.c @@ -8,58 +8,68 @@ #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 POWER_MAX) power = POWER_MAX; + 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; -#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 - * - * \param display The power value from 0 to 400 (including bounds) + * \brief set the servo to a position calculated to given power + * + * \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(); - ports_init(); - timer_init(); - uart_init(UART_BAUD_SELECT(38400,F_CPU)); - reset_input_buffer(); - //demo_display(); - - - while(1) { - work_uart(); + sei(); - if(syscounter >= 10) { - reset_input_buffer(); - uart_putc('a'); // send a to receive values from master box + ports_init(); + timer_init(); + uart_init(UART_BAUD_SELECT(38400,F_CPU)); + + reset_input_buffer(); + // demo_display(); + + while(1) { + work_uart(); + + if(syscounter >= 10) { + reset_input_buffer(); + uart_putc('a'); // send a to receive values from master box syscounter = 0; - -#ifdef DEBUG + +#ifdef DEBUG uart_puts_P("OCR1A = "); 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) { diff --git a/schaltungen/displayboard_servo/tester.py b/schaltungen/displayboard_servo/tester.py index 0a3420f..53489de 100755 --- a/schaltungen/displayboard_servo/tester.py +++ b/schaltungen/displayboard_servo/tester.py @@ -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