#include <Servo.h>
Servo myservo;
void setup() {
  pinMode(22,OUTPUT);
  pinMode(24,OUTPUT);
  pinMode(26,OUTPUT);
  pinMode(28,OUTPUT);
  pinMode(30,OUTPUT);
  pinMode(32,OUTPUT);
  pinMode(34,OUTPUT);
  pinMode(36,OUTPUT);
  myservo.attach(7);
}
int p1=22;
int p2=24;
int p3=26;
int p4=28;
int p5=30;
int p6=32;
int p7=34;
int p8=36;
int cr=0,cl=0;//ca,cb:contador para matriz
unsigned long delta=0;
const float kd=((32.0*25792.0/405.0)/(49.5*PI))*1.588,ka=kd*55.0,kb=0.02*ka;
int dx=0, dy=0;
float da=0;
int L[]={p1,p4,p2,p1,p3,p2,p4,p3};//matrizes que guardan el orden de las bobinas
int R[]={p5,p8,p6,p5,p7,p6,p8,p7};
void backlash(int ba[]){
  int cb=0;
  float m=0.0;
  while (m<kb){
    if (millis()>=delta+2){
      digitalWrite(ba[cb],HIGH);
      cb--;
      digitalWrite(ba[cb],LOW);
      cb--;
      delta=millis()+2;
      m+=1;
    }
    if (cb<0)cb=7;
    else if (cb>7) cb=0;
  }
}
void polar(int x, int y, int p){
  int dist=sqrt(((x-dx)^2)+((y-dy)^2));
  float ang;
  if (x-dx!=0){
    ang=atan((y-dy)/(x-dx));
    if (x-dx<0) ang=ang+PI;
    else if ((x-dx>0) and (y-dy<0)) ang=ang+(2.0*PI);
  }
  else {
    if (y-dy>0) ang=PI/2.0;
    if (y-dy==0) ang=0.0;
    if (y-dy<0) ang=3.0*PI/2.0;
  }
  ang=ang-da;
  if (ang<-PI)ang=ang+(2*PI);
  if (ang>PI)ang=ang-(2*PI);
  if (p)myservo.write(90);
  else myservo.write(100);
  float i=0;
  while (i<abs(ang*ka)){
    if (millis()>=delta+2){
      if (ang<0.0){//girar contra el reloj
        digitalWrite(R[cr],HIGH);
        digitalWrite(L[cl],HIGH);
        cr--;
        cl++;
        digitalWrite(R[cr],LOW);
        digitalWrite(L[cl],LOW);
        cr--;
        cl++;
        delta=millis()+2;
        i++;
      }else if (ang>0.0){//girar con el reloj
        digitalWrite(R[cr],HIGH);
        digitalWrite(L[cl],HIGH);
        cr++;
        cl--;
        digitalWrite(R[cr],LOW);
        digitalWrite(L[cl],LOW);
        cr++;
        cl--;
        delta=millis()+2;
        i++;
      }
      if (cr<0)cr=7;
      else if (cr>7) cr=0;
      if (cl<0)cl=7;
      else if (cl>7)  cl=0;
    }
  }
  if (ang>0) backlash(L);
  if (ang<0) backlash(R);
  da=ang+da;
  if (da>(2*PI)) da=da-(2*PI);
  if (da<0) da=da+(2*PI);
  int j=0;
  while (j<(dist*kd)){//avanzar hacia enfrente
    if (millis()>=delta+2){
      digitalWrite(R[cr],HIGH);
      digitalWrite(L[cl],HIGH);
      cr--;
      cl--;
      digitalWrite(R[cr],LOW);
      digitalWrite(L[cl],LOW);
      cr--;
      cl--;
      delta=millis()+2;
      j++;
    }
    if (cr<0)cr=7;
    else if (cr>7) cr=0;
    if (cl<0)cl=7;
    else if (cl>7)  cl=0;
  }
}
int k=0;
void loop() {
  // put your main code here, to run repeatedly:
  if (k<1){
    //polar(k,sin(k)*100,1);
    polar(200,0,0);
    polar(200,0,1);
    polar(200,0,0);
    polar(200,0,1);
    polar(200,0,0);
    polar(200,0,1);
    polar(200,0,0);
    polar(200,0,1);
    polar(200,0,0);
    polar(200,0,1);
    polar(5000,0,0);
    //polar(0,0,0);
    k+=1;
  }
  for (int l=0; l<8; l++){
    digitalWrite(L[l],LOW);
    digitalWrite(R[l],LOW);
  }
}