| 12,16 → 12,18 |
|---|
| uint16_t eeprom_pwm_limit[6] EEMEM; |
| |
| uint8_t pwm_signal[6]; |
| uint8_t pwm_neutral_position[6]; |
| uint8_t pwm_position[6]; |
| uint16_t pwm_limit[6]; |
| uint8_t volatile pwm_neutral_position[6]; |
| uint8_t volatile pwm_position[6]; |
| uint16_t volatile pwm_limit[6]; |
| uint8_t pwm_active; |
| uint8_t pwm_status = 1; |
| #if DEBUG_SIGNAL |
| uint8_t volatile display_values = 0; |
| #endif |
| |
| |
| void pwm_init() { |
| |
| SERVODDR = 0xff; |
| SERVODDR = (1<<SERVO1) | (1<<SERVO2) | (1<<SERVO3) | (1<<SERVO4) | (1<<SERVO5) | (1<<SERVO6); |
| |
| set_pwm(); |
| |
| 61,12 → 63,12 |
|---|
| void pwm_check_active() { |
| |
| // check if pwm is configured |
| pwm_active = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0; |
| pwm_active |= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0; |
| pwm_active |= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0; |
| pwm_active |= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0; |
| pwm_active |= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0; |
| pwm_active |= (pwm_limit[5] != 0xffff) ? (1 << SERVO6) : 0; |
| pwm_active = (pwm_limit[0] != 0xffff) ? (1<<SERVO1) : 0; |
| pwm_active |= (pwm_limit[1] != 0xffff) ? (1<<SERVO2) : 0; |
| pwm_active |= (pwm_limit[2] != 0xffff) ? (1<<SERVO3) : 0; |
| pwm_active |= (pwm_limit[3] != 0xffff) ? (1<<SERVO4) : 0; |
| pwm_active |= (pwm_limit[4] != 0xffff) ? (1<<SERVO5) : 0; |
| pwm_active |= (pwm_limit[5] != 0xffff) ? (1<<SERVO6) : 0; |
| |
| } |
| |
| 89,6 → 91,7 |
|---|
| |
| } |
| |
| /* |
| void delay(int ms) { |
| |
| while(ms--) { |
| 96,8 → 99,8 |
|---|
| } |
| |
| } |
| */ |
| |
| |
| int main(void) { |
| |
| uint8_t blink_counter = 0; |
| 104,12 → 107,9 |
|---|
| uint8_t blink = 0; |
| |
| // intialize |
| DDRD = 0x80; |
| PORTD = 0x80; |
| DDRD = (1<<PD7); |
| PORTD = (1<<PD7); |
| |
| DDRC &= ~0x0f; |
| PORTC |= 0x0f; |
| |
| cli(); |
| uart_init(); |
| eeprom_init(); |
| 118,13 → 118,16 |
|---|
| set_pwm_neutral(); |
| sei(); |
| |
| // start pwm |
| TCNT1 = 0; |
| while(1) { |
| |
| // start pwm |
| cli(); |
| TCNT1 = 0; |
| SERVOPORT = pwm_active; |
| |
| // update signals |
| set_pwm(); |
| |
| // show status |
| blink_counter++; |
| |
| 135,7 → 138,7 |
|---|
| |
| } else { |
| |
| if (I2C_timeout) { |
| if (I2C_timeout) { |
| I2C_timeout--; |
| if (I2C_timeout == 0) { // no i2c signal |
| blink = 0; |
| 160,10 → 163,12 |
|---|
| } |
| } |
| |
| // wait with high pwm signal |
| // till here the code execution takes less than 70ys, plenty enough time for some more calculations |
| |
| // wait till 300ys are reached |
| while(TCNT1 < 300) ; |
| |
| // check servos setting pwm to low |
| // check servo settings and set pwm to low |
| for(uint8_t i = 0; i < 255; i++) { |
| |
| TCNT0 = 0; |
| 190,18 → 195,34 |
|---|
| while (TCNT0 < 60) ; |
| |
| } |
| sei(); |
| |
| // set all servos to low |
| SERVOPORT = 0; |
| |
| // update signals |
| set_pwm(); |
| // enable interrupts and receive data while waiting till 20ms are reached and the next pulse has to be sent |
| sei(); |
| |
| // wait till 20ms are reached |
| #if DEBUG_SIGNAL |
| if (display_values) { |
| |
| cli(); |
| uart_putchar('\r'); |
| for(uint8_t i = 0; i < 6; i++) { |
| uart_putchar(i+49); |
| uart_putchar(':'); |
| uart_putchar((pwm_signal[i]/100)%10+48); |
| uart_putchar((pwm_signal[i]/10)%10+48); |
| uart_putchar((pwm_signal[i])%10+48); |
| uart_putchar(' '); |
| } |
| sei(); |
| |
| } |
| #endif |
| |
| while(TCNT1 < 20000) ; |
| TCNT1 = 0; |
| |
| |
| } |
| |
| return 0; |