#include "Navigation.h"


void Navigation::start() {
  int r = m_speed, l = m_speed;
  
  
  switch(m_richtung)
  {
    case -2:
	  l = 0;
	  break;
    case -1:
	  l /= 4;
	  break;
    case 1:
	  r /= 4;
	  break;
    case 2:
	  r = 0;
  }
  
  m_board->motor(MOTOR_PORT_LINKS, l);
  m_board->motor(MOTOR_PORT_RECHTS, r);

  m_gestartet = true;
}

void Navigation::stop() {
  m_board->motor(MOTOR_PORT_LINKS, 0);
  m_board->motor(MOTOR_PORT_RECHTS, 0);

  m_gestartet = false;
}

void Navigation::setSpeed(int speed) {
  m_speed = (speed > 255) ? 255 : (speed < -255) ? -255 : speed;

  if(m_gestartet) start();
}

void Navigation::setRichtung(int richtung) {
  m_richtung = richtung;

  if(m_gestartet) start();
}