// Ansteuerung von RC-Servomotoren - LMA Quantec
// ATmega328P, 16MHz
// Arduino-Pro-Mini Clon
// Eclipse mit AVR Plugin
// erstellt am: 20.09.2015
// www.alex-handfest.de/avr/avr.html


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

void stellung0(void);
void stellung1(void);
void stellung2(void);
void stellung3(void);
void stellung4(void);

//Variablen
unsigned int high_count		= 0;
unsigned int  low_count		= 0;
unsigned char servo 		= 1;
unsigned char status 		= 0;
unsigned char stellung[10]	= {'\0'};

extern char uart_recv_buf[UART_BUF_SIZE];



int main(void) {

  // Initialisierung von Timer1
  TCCR1B |= (1<<WGM12);    // CTC Modus
  TCCR1B |= (1<<CS12) | (0<<CS11) | (1<<CS10);   //Setzen des Prescaler auf 1024
  OCR1A = 250;             // ( 16000000/1024 )/250 = 62,5 kHz -> 0,016s = 16ms
  TIMSK1 |= (1<<OCIE1A);   // Compare Interrupt erlauben

  stellung0();

  // Globale Interrupts einschalten
  sei();

  // Ausgaenge
  DDRB |= (1<<PB5);      // PB5    -> LED
  DDRC |= (1<<PC1);      // PC1    -> Power uC
  DDRC |= (1<<PC2);      // PC2    -> Power Servos
  DDRD  = 0b01111100;    // PD2-6  -> Signal Servos

  //PORTC |= (1<<PC2);   // high
  PORTC &= ~(1<<PC2);    // low    -> Servos aus
  PORTC |= (1<<PC1);     // high   -> uC an
  //PORTC &= ~(1<<PC1);  // low
  PORTB &= ~(1<<PB5);    // low    -> LED aus

  while(1){

    //leere Schleife
  }
  return 0;
}

// --------- Time Overflow alle 16ms --------

ISR(TIMER1_COMPA_vect){

  // *** PWM fuer Servos

  PORTD |= (1<<PD2);
  _delay_us(850);
  for(int i=0; i<=stellung[1]; i++) _delay_us(10);
  PORTD &= ~(1<<PD2);

  PORTD |= (1<<PD3);
  _delay_us(850);
  for(int i=0; i<=stellung[2]; i++) _delay_us(10);
  PORTD &= ~(1<<PD3);

  PORTD |= (1<<PD4);
  _delay_us(850);
  for(int i=0; i<=stellung[3]; i++) _delay_us(10);
  PORTD &= ~(1<<PD4);

  PORTD |= (1<<PD5);
  _delay_us(850);
  for(int i=0; i<=stellung[4]; i++) _delay_us(10);
  PORTD &= ~(1<<PD5);

  PORTD |= (1<<PD6);
  _delay_us(850);
  for(int i=0; i<=stellung[5]; i++) _delay_us(10);
  PORTD &= ~(1<<PD6);


  // *** Leave me alone - Logik

  if ( (PINC & (1<<PC0))){     // wenn high   -> Taster gedrueckt

    high_count++;
    low_count = 0;
    //PORTB |= (1<<PB5);       // PB5 high    -> LED an
    PORTC |= (1<<PC1);         // PC1 high    -> uC an

    if (high_count == 60){
      if (status != 0){  // falscher Start
        stellung0();
        high_count = 0;
      }
      PORTC |= (1<<PC2); // PC2 high    -> Servos an
    }
    if (high_count == 80){
      stellung[1] = 42;
    }
    if (high_count == 83){
      stellung1();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (high_count == 88){
      stellung[3] = 57;
    }
    if (high_count == 96){
      stellung2();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (high_count == 160){
      stellung3();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (high_count == 250){
      stellung2();
      //uart_putc(status);  uart_putc(UART_CR);
    }
  }
  else{

    low_count++;
    high_count = 0;
    //PORTB &= ~(1<<PB5);      // PB5 low     -> LED aus

    if (status == 3 && low_count == 3){
      stellung2();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (status == 2 && low_count == 40){
      stellung4();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (low_count == 50){
      stellung0();
      //uart_putc(status);  uart_putc(UART_CR);
    }
    if (low_count == 140){
      PORTC &= ~(1<<PC2);      // PC2 low     -> Servos aus
    }
    if (low_count >= 1500){    // nach 32s
      PORTC &= ~(1<<PC1);      // PC1 low     -> uC aus, Akku sparen
      _delay_ms(20);
    }
  }


  // *** Roboter per UART fahren
  /*
  if (uart_in() == 1){
    servo = uart_recv_buf[0];
    stellung[servo] = uart_recv_buf[1];
    uart_putc(uart_recv_buf[0]);
    uart_putc(stellung[servo]);
    uart_putc(UART_CR);
    uart_done();
  }*/
}

void stellung0(void){ 	// Kanonen Stellung
  stellung[1] = 62;
  stellung[2] = 115;
  stellung[3] = 74;
  stellung[4] = 13;
  stellung[5] = 42;
  status = 0;
}

void stellung1(void){ 	// zu Schalter1
  stellung[1] = 42;
  stellung[2] = 22;
  stellung[3] = 74;
  stellung[4] = 13;
  stellung[5] = 42;
  status = 1;
}

void stellung2(void){ 	// Schalter1
  stellung[1] = 42;
  stellung[2] = 22;
  stellung[3] = 57;
  stellung[4] = 105;
  stellung[5] = 130;
  status = 2;
}

void stellung3(void){ 	// Schalter2
  stellung[1] = 58;
  stellung[2] = 22;
  stellung[3] = 57;
  stellung[4] = 105;
  stellung[5] = 130;
  status = 3;
}

void stellung4(void){ 	// weg Schalter2
  stellung[1] = 42;
  stellung[2] = 115;
  stellung[3] = 57;
  stellung[4] = 105;
  stellung[5] = 130;
  status = 4;
}