X-Git-Url: http://git.droids-corp.org/?p=protos%2Frc_servos.git;a=blobdiff_plain;f=main.c;h=4b1bbc374a51ad532c6078059449c578b15c3fe5;hp=df8fb930b6c0e0f33b8e19d6b994a32faecd168d;hb=HEAD;hpb=25c9fe95c461d42a7c67f06f9267ce62436aaa0e diff --git a/main.c b/main.c index df8fb93..4b1bbc3 100644 --- a/main.c +++ b/main.c @@ -1,4 +1,10 @@ +/* +avrdude -p atmega168p -P usb -c avrispmkii -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xf9:m +-> it failed but I answered y, then make reset and it was ok +*/ + #include +#include struct servo { uint8_t bit; @@ -7,58 +13,89 @@ struct servo { static struct servo servo_table[] = { { - .bit = 0, - .command = 0, + .bit = 2, + .command = 300, + }, + { + .bit = 3, + .command = 700, }, { - .bit = 1, + .bit = 4, .command = 512, }, { - .bit = 2, - .command = 1023, + .bit = 5, + .command = 512, + }, + { + .bit = 6, + .command = 512, + }, + { + .bit = 7, + .command = 512, }, }; -#define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table)) +#define N_SERVO (sizeof(servo_table)/sizeof(*servo_table)) + +/* we use the first servo for PPM output if enabled */ +#define PPM (0) + +static uint8_t bypass; +static uint8_t ppm_enabled; +static volatile uint8_t done; +static uint8_t portval; +static uint8_t rxidx; -register uint8_t bypass asm("r2"); -register uint8_t done asm("r3"); -register uint8_t portval asm("r4"); -register uint8_t rxidx asm("r5"); -#define BYPASS_ENABLE 14 -#define BYPASS_DISABLE 15 +static uint8_t icp_idx = N_SERVO; +static uint16_t icp_servos[N_SERVO]; +static uint16_t icp_prev; + +static uint8_t spi_out_idx; /* current byte beeing sent */ + +#define PPM_BIT 0x01 +#define BYPASS_BIT 0x02 + +#define LED_ON() do { PORTB |= 0x02; } while(0) +#define LED_OFF() do { PORTB &= ~0x02; } while(0) /* * SPI protocol: * - * A command is stored on 2 bytes. The first one has its msb to 0, and the - * second one to 1. The first received byte contains the command number, and the - * msb of the servo value. The second byte contains the lsb of the servo value. + * A command is stored on 2 bytes (except command 0). The first byte + * has its most significant bit to 0, and the second one to 1. The + * first received byte contains the command number, and the msb of the + * servo value. The second byte contains the lsb of the servo value. * - * Commands 0 to NB_SERVO are to set the value of servo. - * Command 14 is to enable bypass mode. - * Command 15 is to disable bypass mode. + * Command 0 is only one byte long, it means "I have nothing to say". + * Commands 1 to N_SERVO (included) are to set the value of servo. + * Command N_SERVO+1 is: + * - to enable/disable ppm generation in place of first servo. + * - to enable/disable bypass mode */ -register union { +union byte0 { uint8_t u8; struct { - uint8_t zero:1; - uint8_t cmd_num:4; + /* inverted: little endian */ uint8_t val_msb:3; + uint8_t cmd_num:4; + uint8_t zero:1; }; -} byte0 asm("r6"); +}; -register union { +union byte1 { uint8_t u8; struct { - uint8_t one:1; + /* inverted: little endian */ uint8_t val_lsb:7; + uint8_t one:1; }; -} byte1 asm("r7"); +}; SIGNAL(TIMER1_COMPA_vect) { - PORTC = portval; + PORTD = portval; TIMSK1 &= ~_BV(OCIE1A); done = 1; } @@ -66,12 +103,38 @@ SIGNAL(TIMER1_COMPA_vect) static void poll_spi(void) { uint8_t c; + uint16_t servo; + static union byte0 byte0_rx; + union byte1 byte1_rx; + union byte0 byte0_tx; + static union byte1 byte1_tx; /* reception complete ? */ if (!(SPSR & (1<> 1]; + byte0_tx.val_msb = servo >> 7; + byte0_tx.cmd_num = (spi_out_idx >> 1) + 1; + byte0_tx.zero = 0; + byte1_tx.val_lsb = servo & 0x7f; + byte1_tx.one = 1; + SPDR = byte0_tx.u8; + } + else { + SPDR = byte1_tx.u8; + } + spi_out_idx ++; + if (spi_out_idx >= N_SERVO * 2) + spi_out_idx = 0; + + /* RX */ + if ((rxidx == 0) && (c & 0x80)) { rxidx = 0; return; /* drop */ @@ -82,99 +145,207 @@ static void poll_spi(void) } if (rxidx == 0) { - byte0.u8 = c; + byte0_rx.u8 = c; + + /* command num 0 is ignored */ + if (byte0_rx.cmd_num == 0) + return; } else { uint16_t val; - byte1.u8 = c; + byte1_rx.u8 = c; /* process command */ - if (byte0.cmd_num < NB_SERVO) { - val = (uint16_t)byte0.val_msb << 7; - val += byte1.val_lsb; - servo_table[byte0.cmd_num].command = val; - } - else if (byte0.cmd_num == BYPASS_ENABLE) { - bypass = 1; + val = (uint16_t)byte0_rx.val_msb << 7; + val += byte1_rx.val_lsb; + + if (byte0_rx.cmd_num < N_SERVO+1) { + servo_table[byte0_rx.cmd_num-1].command = val; } - else if (byte0.cmd_num == BYPASS_DISABLE) { - bypass = 0; + else if (byte0_rx.cmd_num == N_SERVO+1) { + if (val & PPM_BIT) + ppm_enabled = 1; + else + ppm_enabled = 0; + if (val & BYPASS_BIT) + bypass = 1; + else + bypass = 0; } } rxidx ^= 1; } +static void poll_input_capture(void) +{ + uint16_t icp, diff; + + /* no new sample, return */ + if ((TIFR1 & _BV(ICF1)) == 0) + return; + + cli(); + icp = ICR1; + sei(); + + /* clear the flag by writing a one */ + TIFR1 = TIFR1 | _BV(ICF1); + + diff = icp - icp_prev; + icp_prev = icp; + + /* a rising edge with at least 2ms of state 0 means that we + * get the first servo */ + if (diff > 3000) { + icp_idx = 0; + return; + } + + /* get the value for the servo */ + if (icp_idx < N_SERVO) { + if (diff < 1000) + icp_servos[icp_idx] = 0; + else if (diff > 2023) + icp_servos[icp_idx] = 1023; + else + icp_servos[icp_idx] = diff - 1000; + icp_idx++; + } +} + +static void poll(void) +{ + poll_spi(); + poll_input_capture(); +} + static void load_timer_at(uint16_t t) { OCR1A = t; TIMSK1 |= _BV(OCIE1A); } -static void do_one_servo(struct servo *s) +static void do_servos(void) { - uint16_t t; + uint8_t i; + uint16_t t, start; + + /* skip first servo if ppm is enabled */ + if (ppm_enabled) + i = 1; + else + i = 0; - /* set bit */ - done = 0; - //portval = PORTC | (1 << s->bit); - portval = (1 << s->bit); t = TCNT1; - load_timer_at(t + 150); - while (done == 0) - poll_spi(); + start = t + 100; + + for (; i < N_SERVO; i++) { + + /* set servo and PPM bit */ + portval = 1 << servo_table[i].bit; + if (ppm_enabled) + portval |= (1 << servo_table[PPM].bit); + + done = 0; + load_timer_at(start); + while (done == 0) + poll(); + + /* reset PPM bit after 300us */ + portval = 1 << servo_table[i].bit; + done = 0; + load_timer_at(start + 300); + while (done == 0) + poll(); + + start = start + 1000 + servo_table[i].command; + } + + /* set PPM bit only for last servo */ + portval = 0; + if (ppm_enabled) + portval |= (1 << servo_table[PPM].bit); - /* reset bit */ done = 0; + load_timer_at(start); + while (done == 0) + poll(); + + /* reset PPM bit after 300us */ portval = 0; - //portval = PORTC & (~(1 << s->bit)); - load_timer_at(t + 150 + 8000 + s->command * 8); + done = 0; + load_timer_at(start + 300); while (done == 0) - poll_spi(); + poll(); } int main(void) { - uint8_t i; uint8_t t, diff; + uint8_t tmp; + uint8_t cnt = 10; + + /* use pull-up for inputs */ + PORTC |= 0x3f; /* LED */ - DDRB = 0x20; + DDRB = 0x02; - /* servo outputs */ - DDRC = 0x7; + while (cnt > 0) { +#if 1 /* disable for LED debug only */ + cnt--; +#endif + LED_ON(); + wait_ms(100); + LED_OFF(); + wait_ms(100); + } + + /* servo outputs PD2-PD7 */ + DDRD = 0xfc; - /* start timer1 at clk/1 (8Mhz) */ + /* start timer1 at clk/8 (1Mhz), enable noise canceler on + * input capture, capture rising edge */ TCNT1 = 0; - TCCR1B = _BV(CS10); + TCCR1B = _BV(CS11) | _BV(ICNC1) | _BV(ICES1); /* start timer0 at clk/1024 (~8Khz) */ TCNT0 = 0; TCCR0B = _BV(CS02) | _BV(CS00); - /* enable spi (don't set unused MISO as output) */ + /* enable spi (set MISO as output) */ SPCR = _BV(SPE); + SPDR = 0; + DDRB |= _BV(4); sei(); bypass = 0; while (1) { t = TCNT0; - for (i = 0; i < NB_SERVO; i++) { - do_one_servo(&servo_table[i]); - } + do_servos(); /* wait 20 ms */ while (1) { diff = TCNT0 - t; if (diff >= 160) break; - poll_spi(); + poll(); } /* bypass mode */ - while (bypass == 1) { - PORTC = PORTB; + if (bypass == 1) { + LED_ON(); + + while (bypass == 1) { + tmp = PINC; + tmp &= 0x3f; + tmp <<= 2; + PORTD = tmp; + poll(); + } + LED_OFF(); } }