Rev 1455 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#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;
}
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) {
#define PROGRAMMINGMODE 0
#if PROGRAMMINGMODE
uint8_t current_servo = 0;
uint8_t keyrate = 0;
#endif
DDRD = 0x80;
PORTD = 0x80;
DDRC &= ~0x0f;
PORTC |= 0x0f;
cli();
eeprom_init();
InitIC2_Slave(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) {
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;
}