Rev 1466 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#include <string.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <util/delay.h>
#include "servoboard.h"
#include "twislave.h"
#include "uart.h"
uint8_t eeprom_neutral_pos[6] EEMEM;
uint16_t eeprom_pwm_limit[6] EEMEM;
uint8_t pwm_signal[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 = (1<<SERVO1) | (1<<SERVO2) | (1<<SERVO3) | (1<<SERVO4) | (1<<SERVO5) | (1<<SERVO6);
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 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;
}
void eeprom_init() {
eeprom_busy_wait();
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_check_active();
}
void eeprom_write() {
cli();
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();
}
/*
void delay(int ms) {
while(ms--) {
_delay_ms(1);
}
}
*/
int main(void) {
uint8_t blink_counter = 0;
uint8_t blink = 0;
// intialize
DDRD = (1<<PD7);
PORTD = (1<<PD7);
cli();
uart_init();
eeprom_init();
i2c_init(I2C_ADDRESS);
pwm_init();
set_pwm_neutral();
sei();
while(1) {
// start pwm
cli();
TCNT1 = 0;
SERVOPORT = pwm_active;
// update signals
set_pwm();
// show status
blink_counter++;
if (pwm_active == 0) { // not configured
blink = 50;
pwm_status = 1;
} else {
if (I2C_timeout) {
I2C_timeout--;
if (I2C_timeout == 0) { // no i2c signal
blink = 0;
pwm_status = 2;
set_pwm_neutral();
set_pwm();
}
blink = 5;
pwm_status = 0;
}
}
if (blink == 0) {
LED_ON;
} else if (blink_counter % blink == 0) {
blink_counter = 1;
if (LED_IS_ON) {
LED_OFF;
} else {
LED_ON;
}
}
// 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 servo settings and set pwm to low
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) ;
}
// set all servos to low
SERVOPORT = 0;
// enable interrupts and receive data while waiting till 20ms are reached and the next pulse has to be sent
sei();
#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) ;
}
return 0;
}