#include <Servo.h>

//Pin assignment
int pivot1 = 8;
int pivot2 = 9;
int turn1 = 10;
int turn2 = 11;

//Variable initialisation
int pivot = 0;
int turn = 0;
int trackP = 0;
int trackT = 0;

Servo motor1;
int pos1 = 0; // Variable where the first motor's position will be stored (in degrees)
int step1 = 0; // Variable used for the first motor's position step
Servo motor2; 
int pos2 = 0; // Variable where the second motor's position will be stored (in degrees)
int step2 = 0; // Variable used for the second motor's position step

void setup() {
  Serial.begin(9600);
  motor1.attach(5); // Attach the first motor to pin 5
  motor1.write(pos1); // Initialise the first motor's position to 0
  motor2.attach(6); //Attach the second motor to pin 6
  motor2.write(pos2); //Initialise the second motor's position to 0
  
  //Initialise pins
  pinMode(pivot1, INPUT_PULLUP);
  pinMode(pivot2, INPUT_PULLUP);
  pinMode(turn1, INPUT_PULLUP);
  pinMode(turn2, INPUT_PULLUP);
}

void loop() {
  Pivot();
  Turn();
  Move();

}
void Pivot(){
  if (digitalRead(pivot1)==digitalRead(pivot2)){ //Test to see if both button are unpressed or both pressed
    pivot=0; //Do not pivot
  } else if ((digitalRead(pivot1) == LOW)){ //Test to see if the forward button is pressed
    pivot=1; //Pivot forward
  } else { //Backward button must be pressed
    pivot=2; //Pivot backward
  }
}
void Turn(){
  if (digitalRead(turn1)==digitalRead(turn2)){ //Test to see if both buttons are unpressed or both pressed
    turn=0; //Do not turn
  } else if ((digitalRead(turn1) == LOW)){ //Test to see if forward button is pressed
    turn=1; //Turn forward
  } else { //Backward button must be pressed
    turn=2; //Turn backward
  }
}
void Move(){
  static unsigned int motorMillis=millis(); //Initialise time variable
  if (millis()-motorMillis>5){ //Test to see if 5ms have passed. This controls speed!
    if (pivot==1){ //If pivoting forward...
      if (turn==1){ //...And turning forward
        step1=1; //Motor 1 spin forward
        step2=0; //Motor 2 static
      } 
      else if (turn==0){ //..And not turning
        step1=1; //Motor 1 spin forward
        step2=1; //Motor 2 spin forward
      } 
      else if (turn==2){ //...And turning backward
        step1=0; //Motor 1 static
        step2=1; //Motor 2 spin forward
      }
    } else if (pivot==0){ //If not pivoting...
      if (turn==1){ //...And turning forward
        step1=1; //Motor 1 spin forward
        step2=-1; //Motor 2 spin backward
      } 
      else if (turn==0){ //...And not turning
        step1=0; //Motor 1 static
        step2=0; //Motor 2 static
      } 
      else if (turn==2){ //...And turning backward
        step1=-1; //Motor 1 spin backward
        step2=1; //Motor 2 spin forward
      }
    } else if (pivot==2){ //If pivoting backward...
      if (turn==1){ //...And turning forward
        step1=0; //Motor 1 static
        step2=-1; //Motor 2 spin backward
      } 
      else if (turn==0){ //...And not turning
        step1=-1; //Motor 1 spin backward
        step2=-1; //Motor 2 spin backward
      } 
      else if (turn==2){ //...And turning backward
        step1=-1; //Motor 1 spin backward
        step2=0; //Motor 2 static
      }
    }
    //Increment position by step
    pos1+=step1;
    pos2+=step2;

    Track(); //Run position tracker
    motorMillis=millis(); //reset timer
  }
  //Actuate motors
  motor1.write(pos1);
  motor2.write(pos2);
}
void Track(){
  if (step1==0){ //If motor 1 is static...
    if (step2==0){ //...And motor 2 is static
      //Not moving
    } else if (step2==1){ //...And motor 2 is spinning forward
      //Pivot forward slow and turn backward slow
      trackP+=0.5;
      trackT-=0.5;
    } else if (step2==-1){ //...And motor 2 is spinning backward
      //Pivot backward slow and turn forward slow
      trackP-=0.5;
      trackT+=0.5;
    }
  } else if (step1==1){ //If motor 1 is spinning forward...
      if (step2==0){ //...And motor 2 is static
        //Pivot forward slow and turn forward slow
        trackP+=0.5;
        trackT+=0.5;
      } else if (step2==1){ //...And motor 2 is spinning forward
        //Pivot forward fast
        trackP+=1;
      } else if (step2==-1){ //...And motor 2 is spinning backward
        //Turn forward fast
        trackT+=1;
      }
  } else if (step1==-1){ //If motor 1 is spinning backward...
      if (step2==0){ //...And motor 2 is static
        //Pivot backward slow and turn backward slow
        trackP-=0.5;
        trackT-=0.5;
      } else if (step2==1){ //...And motor 2 is spinning forward
        //Turn backward fast
        trackT-=1;
      } else if (step2==-1){ //...And motor 2 is spinning backward
        //Pivot backward fast
        trackP-=1;
      }
  }
  Serial.print("Pivot angle = ");
  Serial.print(trackP);
  Serial.print("   Turn angle = ");
  Serial.println(trackT);
}