#include <MeAuriga.h>

MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);

MeUltrasonicSensor ultraSensor(PORT_7);


void setup()
{
  Serial.begin(115200);

  //Set PWM 8KHz
  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(CS11) | _BV(WGM12);

  TCCR2A = _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(CS21);

//  Encoder_1.setPulse(9);
//  Encoder_2.setPulse(9);
//  Encoder_1.setRatio(39.267);
//  Encoder_2.setRatio(39.267);
//  Encoder_1.setPosPid(0.18,0,0);
//  Encoder_2.setPosPid(0.18,0,0);
//  Encoder_1.setSpeedPid(0.18,0,0);
//  Encoder_2.setSpeedPid(0.18,0,0);
}

void loop()
{
  Serial.println(ultraSensor.distanceCm());
  if(Serial.available())
  {
    char a = Serial.read();
    switch(a)
    {
      case 'p':
      Encoder_1.setMotorPwm(200);
      Encoder_2.setMotorPwm(-200);
      break;

      case 'z':
      Encoder_1.setMotorPwm(0);
      Encoder_2.setMotorPwm(-0);
      break;

      default:
      break;
    }
  }
//  Encoder_1.loop();
//  Encoder_2.loop();
  Serial.print("Speed 1:");
  Serial.print(Encoder_1.getCurrentSpeed());
  Serial.print(" , Speed 2:");
  Serial.println(Encoder_2.getCurrentSpeed());
}