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; |
} |
|