#include "SAMI_2BRobots.h"
#include <Adafruit_DotStar.h>

#define circunferencia 256 //256mm = 360° circunferencia del robot, se calcula con la distancia entre centro de llantas siendo este el radio
//La velocidad maxima de este motor son 430RPM. Relacion de motores 75:1 con ruedas de 42mm, velocidad maxima 56 metros/minuto

SAMI_2BRobots Mizq = SAMI_2BRobots();
SAMI_2BRobots Mder = SAMI_2BRobots();
Adafruit_DotStar strip = Adafruit_DotStar(1, INTERNAL_DS_DATA, INTERNAL_DS_CLK, DOTSTAR_BGR);

void setup()
{
  strip.begin();
  strip.setPixelColor(0, 64, 0, 0); strip.show(); //led red
  delay(100);

  Mizq.begin(0x25);
  Mder.begin(0x26);
  Mizq.RESET();
  Mder.RESET();
  delay(50);

  Mizq.setMode(3);
  Mizq.setGear(75);
  Mizq.setDiameter(42);
  Mizq.setRPM_PID_KP(1.81);
  Mizq.setRPM_PID_KD(11.73);
  Mizq.setRPM_PID_KI(0.18);
  Mizq.setATS_PID_KP(25.65);
  Mizq.setATS_PID_KD(0.87);
  Mizq.setATS_PID_KI(0.00);
  Mizq.inverted(false);
  Mder.setMode(3);
  Mder.setGear(75);
  Mder.setDiameter(42);
  Mder.setRPM_PID_KP(1.81);
  Mder.setRPM_PID_KD(11.73);
  Mder.setRPM_PID_KI(0.18);
  Mder.setATS_PID_KP(5.65);
  Mder.setATS_PID_KD(0.87);
  Mder.setATS_PID_KI(0.00);
  Mder.inverted(true);

  strip.setPixelColor(0, 0, 64, 0); strip.show(); //led green
  delay(500);
}

void loop()
{
  strip.setPixelColor(0, 0, 0, 64); strip.show(); //led blue
  avanzar(300, 120); //el primer dato es la distancia en mm (puede ser negativo para reversa) y el segundo las RPM
  vuelta(90, 80);  //el primer datos son los grados que pueden ser positivos o negativos y el segundo dato son las RPM
  vuelta(-180, 80);
  vuelta(90, 80);
  avanzar(-300, 120);
  strip.setPixelColor(0, 64, 0, 0); strip.show(); //led red
  while (1); //aqui detenemos el programa indefinidamente
}

void avanzar(int distancia, int rpm) //pasamos la distancia que queremos avanzar en mm y las rpm
{
  rpm = abs(rpm);
  if (distancia < 0)
  {
    Mizq.setSpeed(rpm * -1);
    Mder.setSpeed(rpm * -1);
  }
  else
  {
    Mizq.setSpeed(rpm);
    Mder.setSpeed(rpm);
  }

  Mizq.setDistance(distancia);
  Mder.setDistance(distancia);

  Mizq.Run(1);
  Mder.Run(1);

  while (Mizq.isRunning() | Mder.isRunning());
  delay(200);
}

void vuelta(int angulo, int rpm) //pasamos los grados que queremos girar y las rpm
{
  rpm = abs(rpm);
  if (angulo > 0) //giro a la derecha
  {
    int d = (int)(((float)circunferencia / (float)360) * (float)angulo); //calculamos los grados proporcionalmente con la circunferencia
    Mizq.setSpeed(rpm);
    Mder.setSpeed(rpm * -1);

    Mizq.setDistance(d);
    Mder.setDistance(d * -1);

    Mizq.Run(1);
    Mder.Run(1);
  }
  else //giro a la izquierda
  {
    angulo = abs(angulo);
    int d = (int)(((float)circunferencia / (float)360) * (float)angulo);
    Mizq.setSpeed(rpm * -1);
    Mder.setSpeed(rpm);

    Mizq.setDistance(d * -1);
    Mder.setDistance(d);

    Mizq.Run(1);
    Mder.Run(1);
  }
  while (Mizq.isRunning() | Mder.isRunning());
  delay(200);
}