X-Git-Url: http://git.droids-corp.org/?p=protos%2Frc_servos.git;a=blobdiff_plain;f=main.c;h=4b1bbc374a51ad532c6078059449c578b15c3fe5;hp=9c368d5aa27b08f19da25d12c75249adc95f99ba;hb=HEAD;hpb=3be0a345c40d7ef9b20e7749a0c73db14f2927e0 diff --git a/main.c b/main.c index 9c368d5..4b1bbc3 100644 --- a/main.c +++ b/main.c @@ -1,3 +1,8 @@ +/* +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 @@ -32,19 +37,25 @@ static struct servo servo_table[] = { .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; -static uint8_t icp_idx = NB_SERVO; -static uint16_t icp_servos[NB_SERVO]; +static uint8_t icp_idx = N_SERVO; +static uint16_t icp_servos[N_SERVO]; static uint16_t icp_prev; -#define BYPASS_ENABLE 14 -#define BYPASS_DISABLE 15 +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) @@ -58,11 +69,12 @@ static uint16_t icp_prev; * servo value. The second byte contains the lsb of the servo value. * * Command 0 is only one byte long, it means "I have nothing to say". - * Commands 1 to NB_SERVO+1 are to set the value of servo. - * Command 14 is to enable bypass mode. - * Command 15 is to disable bypass mode. + * 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 */ -static union { +union byte0 { uint8_t u8; struct { /* inverted: little endian */ @@ -70,16 +82,16 @@ static union { uint8_t cmd_num:4; uint8_t zero:1; }; -} byte0; +}; -static union { +union byte1 { uint8_t u8; struct { /* inverted: little endian */ uint8_t val_lsb:7; uint8_t one:1; }; -} byte1; +}; SIGNAL(TIMER1_COMPA_vect) { @@ -91,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 */ @@ -107,29 +145,34 @@ static void poll_spi(void) } if (rxidx == 0) { - byte0.u8 = c; + byte0_rx.u8 = c; /* command num 0 is ignored */ - if (byte0.cmd_num == 0) + 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+1) { - val = (uint16_t)byte0.val_msb << 7; - val += byte1.val_lsb; - servo_table[byte0.cmd_num-1].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; } } @@ -139,22 +182,16 @@ static void poll_spi(void) static void poll_input_capture(void) { uint16_t icp, diff; - uint8_t rising; /* no new sample, return */ if ((TIFR1 & _BV(ICF1)) == 0) return; - sei(); - icp = ICR1; cli(); + icp = ICR1; + sei(); - rising = TCCR1B & _BV(ICES1); - - /* change the edge type */ - TCCR1B ^= _BV(ICES1); - - /* clear the flag */ + /* clear the flag by writing a one */ TIFR1 = TIFR1 | _BV(ICF1); diff = icp - icp_prev; @@ -162,15 +199,17 @@ static void poll_input_capture(void) /* a rising edge with at least 2ms of state 0 means that we * get the first servo */ - if (rising == 1 && diff > 2000) { + if (diff > 3000) { icp_idx = 0; return; } /* get the value for the servo */ - if (rising == 0 && icp_idx < NB_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++; @@ -189,33 +228,65 @@ static void load_timer_at(uint16_t 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 + 20); + 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); + + done = 0; + load_timer_at(start); while (done == 0) poll(); - /* reset bit */ - done = 0; + /* reset PPM bit after 300us */ portval = 0; - //portval = PORTC & (~(1 << s->bit)); - load_timer_at(t + 20 + 1000 + s->command); + done = 0; + load_timer_at(start + 300); while (done == 0) 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; @@ -223,14 +294,15 @@ int main(void) /* LED */ DDRB = 0x02; -#if 0 /* LED debug */ - while (1) { + while (cnt > 0) { +#if 1 /* disable for LED debug only */ + cnt--; +#endif LED_ON(); wait_ms(100); LED_OFF(); wait_ms(100); } -#endif /* servo outputs PD2-PD7 */ DDRD = 0xfc; @@ -244,17 +316,17 @@ int main(void) 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;