/*
Ersteller: Frank Pelzhause
Datum: 18.11.2009
*/

/* Dieses Programm vereint eine Kombination aus Kalibrierung der Line Sensoren
und ein Prüfprogramm für die Fahrt über eine Linie.
Einige Hilfsfunktionen sind aus bestehenden Programmen übernommen,
aber der Ablauf ist versuchsweise nur in dieser Datei hinterlegt.
Für weitere Verwendung können die Funktionen auch in einzelne Dateien
und Headerdateien ausgelagert werden.
Das Ganze ist natürlich ausbaufähig.

Eine Kurzbeschreibung der Funktionen ist immer am Anfang dokumentiert,
weitere Informationen sind jeweils vor der Funktion beschrieben.

Spezielle Anmerkungen sind inline Dokumentiert. */


/* Benötigte Include Dateien */
#include <nibobee/iodefs.h>          /* muss immer als erste Datei 
										eingebunden werden */

#include <nibobee/led.h>			/* LED Funktionen */
#include <nibobee/delay.h>			/* Verzögerungen, Includiert
										die avr/delay.h
										(achtung: Zusatzfunktionen) */
#include <nibobee/sens.h>			/* Aktivierung der Fühler */
#include <nibobee/analog.h>			/* ????? */
#include <nibobee/line.h>			/* Liniensensoren (Infrarot vorne) */
#include <nibobee/motpwm.h> 		/* Motorsteuerung */

/* Spezielle Includes, keine Ahnung warum (Kommentare erwünscht) */

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

/* Definieren der verschiedenen Modes,
um die Folgereaktionen auswerten zu können */

enum {
	MODE_KALIBRIEREN,
	MODE_DRIVE,
	MODE_LAUFLICHT,
};


/* Funktion um ein Lauflicht zu starten.
Interuptsteuerung wird aktiviert, um auch während der Funktion
reagieren zu können und ein Ereignis zurückzuliefern */

uint8_t Lauflicht()
{
	led_init();
	sens_init();

	while(1==1)
	{
		enable_interrupts();
		int ledNr;
		for (ledNr=0; ledNr<4; ledNr++)
		{
			led_set(ledNr, 1);
			delay(350);
			led_set(ledNr, 0);
			delay(150);
		}
		/* Test auf Fühlerbetätigung */
		/* Wenn beide Fühler nach hinten gedrückt wurden
		   wird ein Status MODE_KALIBRIEREN zurückgegeben
		   Die Fühler müssen allerdings für eine ganze 
		   Umdrehung der LEDs festgehalten werden, 
		   da die Abfrage erst nach der For-Schleife erfolgt */
		if ((sens_getLeft()==-1) && (sens_getRight()==-1)) 
		{
			while ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
				delay(1);
			}
			led_init();
			return MODE_KALIBRIEREN;
		} 
	}
	return 0;
}


/* Calibrierung */
/*
  Hier werden die Linesensoren kalibriert.
  Die Werte werden in das EEPROM geschrieben, wenn der Befehl dazu gegeben wurde.
  - Ablauf der Kalibrierung:
  	1. Linker Fühler nach hinten drücken: Weißabgleich, Dazu Nibobee auf ein weißes
  		Blatt Papier stellen. Dioden blinken.
  	2. Rechter Fühler nach hinten drücken: Schwarzabgleich, Dazu Nibobee auf ein schwarzes
  		Blatt Papier stellen. Dioden blinken.
	3. Zum speichern der Daten beide Fühler nach Vorne drücken.
	 	Die Daten werden dann dauerhaft im EEPROM gespeichert. 
		Dioden Blinken abermals und das linienverfolgungsprogramm wird gestartet.
*/

/* Deklarationen */
void line_read_persistent();
uint16_t line_get(uint8_t idx);
void line_calibrate_white();
void line_calibrate_black();

/* Blinkfunktion */
void do_blink(uint8_t mask) {
  for (int i=0; i<5; ++i) {
    led_set(LED_L_RD, mask&0x01);
    led_set(LED_R_RD, mask&0x01);
    led_set(LED_L_YE, mask&0x02);
    led_set(LED_R_YE, mask&0x02);
    delay(200);
    led_set(LED_L_RD, 0);
    led_set(LED_R_RD, 0);
    led_set(LED_L_YE, 0);
    led_set(LED_R_YE, 0);
    delay(200);
  }
}

/* Kalibrieren im eigentlichen Sinn. 
Besonderheit: Erst nach betätigung Beider Fühler nach Vorne
wird nach dem speichern in den Modus "Linienvervolgung umgeschaltet */

uint8_t Kalibrieren() {
  led_init();
  motpwm_init();
  sens_init();
  analog_init();
  activate_output_bit(IO_LINE_EN);

  line_readPersistent();

  while(1==1) {                      // Endlosschleife (1==1 ist immer wahr!)
    enable_interrupts();
    delay(10);


    if (sens_getLeft()==-1) {
      while (sens_getLeft()==-1) delay(1); /* Linker Fühler nach hinten --> Weißabgleich */
      line_calibrateWhite();
      do_blink(2);
    }
     
    if (sens_getRight()==-1) {
      while (sens_getRight()==-1) delay(1); /* Linker Fühler nach hinten --> Weißabgleich */
      line_calibrateBlack();
      do_blink(1);
    }

    set_output_groupbitval(IO_LEDS, L_YE, line_get(LINE_L)>160);
    set_output_groupbitval(IO_LEDS, L_RD, line_get(LINE_L)>240);
    set_output_groupbitval(IO_LEDS, R_YE, line_get(LINE_R)>160);
    set_output_groupbitval(IO_LEDS, R_RD, line_get(LINE_R)>240);

    /* Beide Fühler nach vorne = Speichern der Werte und weiter
	   zum Linienprogramm */
	if ((sens_getLeft()==1) && (sens_getRight()==1)) {
      while ((sens_getLeft()==1) && (sens_getRight()==1)) {
        delay(1);
      }
      line_writePersistent();
      do_blink(3);
	  return MODE_DRIVE;
    }

  }
  return 0;
}



/* Linien fahren */
/* Das Programm läuft solange, bis beide Fühler nach hinten getätigt werden, 
   dann wird die Kalibrierung wieder mit dem Lauflicht gestartet. */

uint8_t run_line() 
{ 
  activate_output_group(IO_LEDS);  // LED bits als Output 
  sens_init();
  motpwm_init(); 
  motpwm_setLeft(0); 
  motpwm_setRight(0); 
  analog_init(); 
  line_readPersistent(); 
  set_output_group(IO_SENS);       // Pull-ups aktivieren 
  activate_output_bit(IO_LINE_EN); 

  int16_t speed_flt_l=0; 
  int16_t speed_flt_r=0; 

  // Countdown: LEDs blinken lassen 
  for (uint8_t i=0; i<10; ++i) { 
    led_set(LED_L_RD, 1); 
    led_set(LED_R_RD, 1); 
    _delay_ms(10); 
    led_set(LED_L_RD, 0); 
    led_set(LED_R_RD, 0); 
    _delay_ms(990); 
  } 
  led_set(LED_L_YE, 1); 
  led_set(LED_R_YE, 1); 
  _delay_ms(1000); 
  led_set(LED_L_YE, 0); 
  led_set(LED_R_YE, 0); 

  // Hauptschleife: 
  while(1) { 
	enable_interrupts(); /* Fühler aktivieren */
    sei(); 
    _delay_ms(1); 
    int16_t speed_l=0; 
    int16_t speed_r=0; 

    int16_t lval = line_get(LINE_L); 
    int16_t cval = line_get(LINE_C); 
    int16_t rval = line_get(LINE_R); 

    if (lval+cval+rval < 20) { 
      led_set(LED_L_RD, 0); 
      led_set(LED_R_RD, 0); 
      speed_r=0, speed_l=0; 
    } else if ((lval<cval) && (lval<rval)) { 
      // lval is minimum 
      led_set(LED_L_RD, 1); 
      led_set(LED_R_RD, 0); 
      speed_r=550, speed_l=450-1*(cval-lval); 
    } else if ((rval<cval) && (rval<lval)) { 
      // rval is minimum 
      led_set(LED_L_RD, 0); 
      led_set(LED_R_RD, 1); 
      speed_r=450-1*(cval-rval), speed_l=550; 
    } else { 
      // cval is minimum 
      led_set(LED_L_RD, 1); 
      led_set(LED_R_RD, 1); 
      speed_r=750 + 1*(rval-cval), speed_l=750 + 1*(lval-cval); 
    } 

    speed_flt_l*=3; speed_flt_l+=speed_l; speed_flt_l/=4; 
    speed_flt_r*=3; speed_flt_r+=speed_r; speed_flt_r/=4; 
    
    motpwm_setLeft(speed_flt_l); 
    motpwm_setRight(speed_flt_r); 

    /* Wenn beide FÜhler nach hinten gedrückt werden,
	   wird wieder das Lauflicht aktiviert und eine erneute
	   Kalibrierung kann erfolgen */
	if ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
      while ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
        delay(1);
      }
	int16_t speed_l=0; 
	int16_t speed_r=0; 
    motpwm_setLeft(speed_l); 
    motpwm_setRight(speed_r); 
	return MODE_LAUFLICHT;
    }

  } 
  return 0; 
} 


/* Hauptprogramm (main()) 
   Schlicht und einfach :-)) */

uint8_t mode = MODE_LAUFLICHT;

int main() {
	while(1==1) {
		enable_interrupts();
		delay(1);
		switch (mode) {
			case MODE_LAUFLICHT: mode = Lauflicht(); break;
			case MODE_KALIBRIEREN: mode = Kalibrieren(); break;
			case MODE_DRIVE: mode = run_line(); break;
		}

	}
return 0;
}