/**
 * Seweryn Zeman
 * (C)2011
 * Sterowanie robota:
 * 4x serwo
 * 2x krokowy
 * LCD + RS232
 * SET mode
 * sterowanie all-motors-at-once
 *
 * Napędy:
 * J1, J2, D3, J4, GRIP
 *
 * Timer1 - przepełnienie co 2,5ms (2,5ms z 20000 krokami)
 * Timer2 - przepełnienie co 4,75ms (19ms/4serwa)
 *
 * Kierunek (dir):
 * 0 - lewo
 * 1 - prawo
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

// serwa: zestaw portów
#define SERVO0_PIN              PD0
#define SERVO1_PIN              PD1
#define SERVO2_PIN              PD2
#define SERVO3_PIN              PD3
#define SERVOS                  (1<<SERVO0_PIN)|(1<<SERVO1_PIN)|(1<<SERVO2_PIN)|(1<<SERVO3_PIN)

// serwa: ilość kroków na 1deg
#define SERVO_STEPS_PER_DEG     68      // 68 kroków/deg
#define SERVO_STEP              68      // speed

// serwa: wartości położeń
#define SERVO_MIN               5200
#define SERVO_NULL              9150
#define SERVO_MAX               13100

#define SERVO_MIN_DEG           0
#define SERVO_MAX_DEG           115

// serwa: położenie domyślne
#define SERVO_DEFAULT           9150

uint16_t volatile servo_pos[4] = {5200,8400,11600,13100};
uint8_t volatile current_servo = 0;

void init_ports(void) {
    DDRB &= ~((1<<PB0)|(1<<PB1));
    PORTB |= (1<<PB0)|(1<<PB1);
    DDRD |= SERVOS;
    PORTD &= ~(SERVOS);
}

void init_timer2(void) {
    TCCR2 |= (1<<WGM21);            // CTC
    TCCR2 |= (1<<CS21)|(1<<CS22);   // prescaler 256
    OCR2 = 0x93;                    // ~5ms
    TIMSK |= (1<<OCIE2);            // interrupt on compare match
}

void init_timer1(void) {
    TCCR1B |= (1<<WGM12);           // CTC
    TIMSK |= (1<<OCIE1A);           // interrupt on compare match
}

void toggle_servo_signal(uint8_t servo) {
    switch(servo) {
        case 0:
            PORTD ^= (1<<SERVO0_PIN);
            break;
        case 1:
            PORTD ^= (1<<SERVO1_PIN);
            break;
        case 2:
            PORTD ^= (1<<SERVO2_PIN);
            break;
        case 3:
            PORTD ^= (1<<SERVO3_PIN);
            break;
    }
}

void start_timer1(void) {
    OCR1A = servo_pos[current_servo];
    TCCR1B |= (1<<CS10);
}

void stop_timer1(void) {
    TCCR1B &= ~(1<<CS10);
    TCNT1 = 0;
    TIFR &= ~(1<<OCF1A);
    if(current_servo<4) {
        current_servo++;
    } else {
        current_servo = 0;
    }
}

ISR(TIMER2_COMP_vect) {
    toggle_servo_signal(current_servo);
    start_timer1();
}

ISR(TIMER1_COMPA_vect) {
    toggle_servo_signal(current_servo);
    stop_timer1();
}

int servo_move(uint8_t servo, uint8_t dir) {
    _delay_us(4750);
    if(dir==1 && servo_pos[servo]<SERVO_MAX) {
        servo_pos[servo] += SERVO_STEP;
        return 1;
    } else if(dir==0 && servo_pos[servo]>SERVO_MIN) {
        servo_pos[servo] -= SERVO_STEP;
        return 1;
    }
    return 0;
}

int servo_to_deg(uint8_t servo, uint8_t deg) {
    if(deg>=SERVO_MIN_DEG && deg<=SERVO_MAX_DEG) {
        servo_pos[servo] = SERVO_MIN + (SERVO_STEPS_PER_DEG*deg);
        return 1;
    }
    return 0;
}

int main (void) {
    init_ports();
    init_timer2();
    init_timer1();
    sei();
    while(1) {
        if(!(PINB&(1<<PB0))) {
            servo_to_deg(0,0);
        } else if(!(PINB&(1<<PB1))) {
            servo_to_deg(0,90);
        }
    }
}