#include "global.h"
#include "hal/keylcd.h"
#include "navigation.h"
#include "sensor/sensor.h"

Board board;
KeyLCD keylcd;
Navigation navigation;
Sensor sensor;

int main() {
	//board.ledOn();
	sleep(2);
	int speed = 255;
	/*for(int i=-255;i<=255;i+=30) {
		board.motor(0,i);
		board.motor(1,i);
		board.motor(2,i);
		board.motor(3,i);
		sleep(1);
	}*/
	board.motor(0, speed);
	board.motor(1, speed);
	board.motor(2, speed);
	board.motor(3, speed);
	PORTB |= (1 << PB5) | (1 << PB6); // Setze PWM-Ports auf high
	PORTE |= (1 << PE3); // | (1 << PE4);
	PORTA |= (1 << PA4); // | (1 << PA5); 
	//sleep(10);
	while(true) { sleep(1); }
	/*sleep(10);
	board.motor(0, 0);
	board.motor(1, 0);
	board.motor(2, 0);
	board.motor(3, 0);*/
	//while(true) { sleep(1); }
	//board.beep(450);
	//board.ledOn();
	/*keylcd.print("test");
	sleep(1);
	//board.ledOff();
	//sleep(1);
	for(int i=0;i<keylcd.error();i++) {
		board.ledOn();
		sleep(1);
		board.ledOff();
		sleep(1);
	}
	
	board.ledOn();
	uart1_puts("18");
	uart1_puts("Hallo Du! Hier ist dein AtMega128\n");
	sleep(1);
	board.ledOff();
	sleep(10);
	//board.ledOff();*/	
	
	while(true) {
	navigation.Aktualisieren(0,0,speed);
	sleep(2);
	navigation.Aktualisieren(90,0,speed);
	sleep(2);
	navigation.Aktualisieren(180,0,speed);
	sleep(2);
	navigation.Aktualisieren(270,0,speed);
	sleep(2);

	}
	


	while(true)
	{
		//board.ledOn();
		//sleep(1);
		//board.ledOff();
		sleep(1);
	}
	//for(int i=0;i<=7;i++) board.GetADC(i);	

	return 0;
}