#include "hardware.h"
#include "avr.h"
#include "Motor.h"
#include "Navigation.h"


Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);

Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);


static void delay() {
  for(unsigned long i = 0; i < 25000; i++)
  	asm("nop");
}


int main() {
  initHardware();
  
  navigation->setSpeed(1.0);
  
  for(int i = 0; ; i = (i+1)%360) {
    navigation->setDirection(i);
    
    delay();
  }
  
  return 0;
}