#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);
	
	uart1_puts("Hallo hier ist das RNMega128Funk-Board");


	int speed = 255;
	board.motor(0, speed);
	board.motor(1, speed);
	board.motor(2, speed);

	sleep(2);
	navigation.Aktualisieren(180,0,speed);
	while(true) { sleep(1); }
	/*board.motor(0,speed);
	board.motor(2,speed);
	sleep(2);
	board.motor(0,speed);
	board.motor(2,0);
	board.motor(1,speed);
	sleep(2);



	for(int i=-255;i<=255;i+=10) {
		board.motor(0,i);
		board.motor(1,i);
		board.motor(2,i);
		board.motor(3,i);
		msleep(500);
	}

	
	board.motor(0, speed);
	board.motor(1, speed);
	board.motor(2, speed);
	board.motor(3, speed);
	while(true) { sleep(1); }*/
	/*
	//PORTB |= (1 << PB5) | (1 << PB6); // Setze PWM-Ports auf high
	//PORTE |= (1 << PE3); // | (1 << PE4);
	//PORTA |= (1 << PA5); // | (1 << PA5); 
	//sleep(10);
	*/
	/*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;
}