| 1,5 → 1,4 |
|---|
| |
| #define F_CPU 8000000UL |
| |
| #include <string.h> |
| #include <avr/interrupt.h> |
| 7,25 → 6,16 |
|---|
| #include <util/delay.h> |
| #include "servoboard.h" |
| #include "twislave.h" |
| #include "uart.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_signal[6]; |
| 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 }; |
| uint8_t pwm_neutral_position[6]; |
| uint8_t pwm_position[6]; |
| uint16_t pwm_limit[6]; |
| |
| |
| 71,37 → 61,17 |
|---|
| |
| 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]); |
| } |
| */ |
| eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position)); |
| eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit)); |
| |
| 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; |
| |
| } |
| |
| 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]); |
| } |
| eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position)); |
| eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit)); |
| sei(); |
| |
| } |
| 117,12 → 87,6 |
|---|
| |
| int main(void) { |
| |
| #define PROGRAMMINGMODE 0 |
| #if PROGRAMMINGMODE |
| uint8_t current_servo = 0; |
| uint8_t keyrate = 0; |
| #endif |
| |
| DDRD = 0x80; |
| PORTD = 0x80; |
| |
| 130,22 → 94,13 |
|---|
| PORTC |= 0x0f; |
| |
| cli(); |
| uart_init(); |
| eeprom_init(); |
| InitIC2_Slave(0x82); |
| i2c_init(0x82); |
| pwm_init(); |
| set_pwm_neutral(); |
| sei(); |
| |
| #if PROGRAMMINGMODE |
| DDRD = 0xff; |
| PORTD = 0xff; |
| if (!(PINC & 0x01)) { |
| pwm_neutral_programming_mode = 1; |
| PORTD = ~(1 << current_servo); |
| delay(1000); |
| } |
| #endif |
| |
| TCNT1 = 0; |
| while(1) { |
| |
| 188,44 → 143,6 |
|---|
| 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; |