| 0,0 → 1,230 |
|---|
| |
| #define F_CPU 8000000UL |
| |
| #include <string.h> |
| #include <avr/interrupt.h> |
| #include <avr/eeprom.h> |
| #include <util/delay.h> |
| #include "servoboard.h" |
| #include "twislave.h" |
| |
| |
| #define SERVODDR DDRB |
| #define SERVOPORT PORTB |
| #define SERVO1 PB0 |
| #define SERVO2 PB1 |
| #define SERVO3 PB2 |
| #define SERVO4 PB3 |
| #define SERVO5 PB4 |
| #define SERVO6 PB5 |
| |
| |
| uint8_t pwm_neutral_programming_mode = 0; |
| uint8_t eeprom_neutral_pos[6] EEMEM = { 127, 127, 127, 127, 127, 127 }; |
| uint16_t eeprom_pwm_limit[6] EEMEM = { 0, 0, 0, 0, 0, 0 }; |
| uint8_t pwm_signal[6] = { 0, 0, 0, 0, 0, 0 }; |
| uint8_t pwm_counter = 0; |
| uint8_t pwm_neutral_position[6] = { 127, 127, 127, 127, 127, 127 }; |
| uint8_t pwm_position[6] = { 127, 127, 127, 127, 127, 127 }; |
| uint16_t pwm_limit[6]; |
| |
| |
| |
| void pwm_init() { |
| |
| SERVODDR = 0xff; |
| |
| set_pwm(); |
| |
| TCCR1B = (1 << CS11); |
| TCCR0 = (1 << CS00); |
| |
| } |
| |
| void set_pwm() { |
| |
| for(uint8_t n = 0; n < 6; n++) { |
| pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n]; |
| if (pwm_limit[n] != 0) { |
| uint8_t l, h; |
| l = pwm_limit[n] & 0xff; |
| h = pwm_limit[n] >> 8; |
| if (pwm_signal[n] < l) { |
| pwm_signal[n] = l; |
| } |
| if (pwm_signal[n] > h) { |
| pwm_signal[n] = h; |
| } |
| } |
| } |
| |
| } |
| |
| void set_pwm_neutral() { |
| |
| for(uint8_t i = 0; i < 6; i++) { |
| pwm_position[i] = 127; |
| } |
| set_pwm(); |
| |
| } |
| |
| void eeprom_init() { |
| |
| /* |
| eeprom_busy_wait(); |
| for(uint8_t i = 0; i < 6; i++) { |
| pwm_neutral_position[i] = eeprom_read_byte(&eeprom_neutral_pos[i]); |
| pwm_limit[i] = eeprom_read_byte(&eeprom_pwm_limit[i]); |
| } |
| */ |
| |
| pwm_neutral_position[0] = 0b10101000; |
| pwm_neutral_position[1] = 0b01101101; |
| pwm_neutral_position[2] = 0b10000000; |
| pwm_neutral_position[3] = 0b10010101; |
| pwm_neutral_position[4] = 0; |
| pwm_neutral_position[5] = 127; |
| |
| pwm_limit[0] = 0; |
| pwm_limit[1] = 0; |
| pwm_limit[2] = 0; |
| pwm_limit[3] = 0b1011001101110101; |
| pwm_limit[4] = 0; |
| pwm_limit[5] = 0; |
| |
| set_pwm_neutral(); |
| |
| } |
| |
| void eeprom_write() { |
| |
| cli(); |
| for(uint8_t i = 0; i < 6; i++) { |
| eeprom_write_byte(&eeprom_neutral_pos[i], pwm_position[i]); |
| eeprom_write_byte(&eeprom_pwm_limit[i], pwm_limit[i]); |
| } |
| sei(); |
| |
| } |
| |
| void delay(int ms) { |
| |
| while(ms--) { |
| _delay_ms(1); |
| } |
| |
| } |
| |
| |
| int main(void) { |
| |
| uint8_t current_servo = 0; |
| uint8_t keyrate = 0; |
| |
| DDRD = 0xff; |
| PORTD = 0xff; |
| |
| DDRC &= ~0x0f; |
| PORTC |= 0x0f; |
| |
| cli(); |
| eeprom_init(); |
| InitIC2_Slave(0x82); |
| pwm_init(); |
| sei(); |
| |
| #define PROGRAMMINGMODE 0 |
| #if PROGRAMMINGMODE |
| if (!(PINC & 0x01)) { |
| pwm_neutral_programming_mode = 1; |
| PORTD = ~(1 << current_servo); |
| delay(1000); |
| } |
| #endif |
| |
| TCNT1 = 0; |
| while(1) { |
| |
| cli(); |
| SERVOPORT = 0xff; |
| while(TCNT1 < 300) ; |
| |
| for(uint8_t i = 0; i < 255; i++) { |
| |
| TCNT0 = 0; |
| |
| if (i == pwm_signal[0]) { |
| SERVOPORT &= ~(1<<SERVO1); |
| } |
| if (i == pwm_signal[1]) { |
| SERVOPORT &= ~(1<<SERVO2); |
| } |
| if (i == pwm_signal[2]) { |
| SERVOPORT &= ~(1<<SERVO3); |
| } |
| if (i == pwm_signal[3]) { |
| SERVOPORT &= ~(1<<SERVO4); |
| } |
| if (i == pwm_signal[4]) { |
| SERVOPORT &= ~(1<<SERVO5); |
| } |
| if (i == pwm_signal[5]) { |
| SERVOPORT &= ~(1<<SERVO6); |
| } |
| |
| while (TCNT0 < 60) ; |
| |
| } |
| sei(); |
| |
| SERVOPORT = 0; |
| |
| set_pwm(); |
| |
| while(TCNT1 < 20000) ; |
| TCNT1 = 0; |
| |
| #if PROGRAMMINGMODE |
| |
| if (pwm_neutral_programming_mode) { |
| |
| if (keyrate == 0) { |
| |
| if (!(PINC & 0x01)) { |
| if (pwm_position[current_servo] < 255) { |
| pwm_position[current_servo]++; |
| keyrate = 5; |
| set_pwm(); |
| } |
| } |
| if (!(PINC & 0x02)) { |
| if (pwm_position[current_servo] > 0) { |
| pwm_position[current_servo]--; |
| keyrate = 5; |
| set_pwm(); |
| } |
| } |
| if (!(PINC & 0x04)) { |
| current_servo++; |
| current_servo %= 6; |
| PORTD = ~(1 << current_servo); |
| keyrate = 25; |
| } |
| if (!(PINC & 0x08)) { |
| //keyrate = 50; |
| PORTD = ~pwm_position[current_servo]; |
| } |
| |
| } else { |
| keyrate--; |
| } |
| |
| } |
| #endif |
| |
| } |
| |
| return 0; |
| } |
| |