const int U1=A0; // M1    /////up limit
const int D1=A1; // M1   ///// down limit  (Motor 1 centre)
const int p1=A2; // M1   /////pos pulse
const int d1=A3; // M1   //// neg pulse

const int U2=A4; // M2
const int D2=A5; // M2
const int p2=A6; // M2  /////Motor 2 left
const int d2=A7; // M2

const int U3=A8; // M3
const int D3=A9; // M3
const int p3=A10;// M3   ////Motor 3 right
const int d3=A11;// M3

char incomingByte = 0;
int ij,i,done;

void setup()
{   
  Serial.begin(9600);
  pinMode (p1, OUTPUT);
  pinMode (d1, OUTPUT);
  pinMode (p2, OUTPUT);
  pinMode (d2, OUTPUT);
  pinMode (p3, OUTPUT);
  pinMode (d3, OUTPUT);
  pinMode (U1, INPUT);
  pinMode (D1, INPUT);
  pinMode (U2, INPUT);
  pinMode (D2, INPUT);
  pinMode (U3, INPUT);
  pinMode (D3, INPUT);
  delay (3000);
}

// P1=Front, P2=Left, P3=Right
  
void loop()
{          
  down (25,50);
  while (1)
  {
    incomingByte = Serial.read();
    if (incomingByte=='a')
    {cmd ();done=0;}
  }
}

void cmd ()
{
  while (done==0)
  {
    incomingByte = Serial.read();
    if (incomingByte=='f')center (12,50);
    if (incomingByte=='g')down (24,50);
    if (incomingByte=='h')RL (10,125); // RL1-8s
    if (incomingByte=='i')RL (5,125);  // RL2-4s
    if (incomingByte=='j')RL (10,62);  // RL3-4s
    if (incomingByte=='k')RL (5,62);   // RL4-2s
    if (incomingByte=='m')RL (5,31);   // RL5-1s
    if (incomingByte=='n')RL (2,30);   // RL6-0.4s
    if (incomingByte=='o')back_jurk (5,30);// BJ-0.24s
    if (incomingByte=='X')done=1;      // Exit
  }
}






/////////////////////////ROTATE/////////////////////////////////////////////

void RL (int r, int sp)
{
  // Center to Right   ////////////(Rotate Left)
  digitalWrite (d2, LOW);  // up
  digitalWrite (d3, HIGH); // down
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p2, HIGH);digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(U2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(D3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
  ////////////////////////////////////////////////////// Right to Center////////////////(Rotate Right)
  digitalWrite (d2, HIGH); // down
  digitalWrite (d3, LOW);  // up
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p2, HIGH);digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(D2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(U3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
  ////////////////////////////////////////////////////// Center to Left              ////// (Return-right)
  digitalWrite (d2, HIGH); // down
  digitalWrite (d3, LOW);  // up
  
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p2, HIGH);digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(D2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(U3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
  ////////////////////////////////////////////////// Left to Center                  ///////(Return-left)
  digitalWrite (d2, LOW); // up
  digitalWrite (d3, HIGH);//down
  
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p2, HIGH);digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(U2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(D3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
}






////////////////////////////BACK JERK////////////////////////////////////////////////

void back_jurk (int r, int sp) //////// (jerk)
{
//////////////////////////////////////////// back to center              /////////(CENTRE high)
  digitalWrite (d1, HIGH); //down
  digitalWrite (d2, LOW); //up
  digitalWrite (d3, LOW); //up
  for (i=0;i<r;i++)
  { 
    for (ij=0;ij<800;ij++)
    {
      digitalWrite (p1, HIGH);digitalWrite (p2, HIGH);digitalWrite (p3, HIGH);
      delayMicroseconds (sp);
      if (digitalRead(D1)==LOW)digitalWrite (p1, LOW);
      if (digitalRead(U2)==LOW)digitalWrite (p2, LOW);
      if (digitalRead(U3)==LOW)digitalWrite (p3, LOW);
      delayMicroseconds (sp);
    }
  }
  ///////////////////////////////////////////// center to back             //////(SIDE high)
  digitalWrite (d1, LOW); //up
  digitalWrite (d2, HIGH);//down
  digitalWrite (d3, HIGH);//down
  
  for (i=0;i<r;i++)
  { 
    for (ij=0;ij<800;ij++)
    {
      digitalWrite (p1, HIGH);
      digitalWrite (p2, HIGH);
      digitalWrite (p3, HIGH);
      delayMicroseconds (sp);
      if (digitalRead(U1)==LOW)digitalWrite (p1, LOW);
      if (digitalRead(D2)==LOW)digitalWrite (p2, LOW);
      if (digitalRead(D3)==LOW)digitalWrite (p3, LOW);
      delayMicroseconds (sp);
    }
  }  
}







///////////////////////////DOWN/////////////////////////////////////////
void down (int r, int sp)/////////// (ALL 3 LOW)
{
  dir (0);
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p1, HIGH);
     digitalWrite (p2, HIGH);
     digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(D1)==LOW)digitalWrite (p1, LOW);
     if (digitalRead(D2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(D3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
}




////////////////////////////////CENTER/////////////////////////////////////////////////
void center (int r, int sp) // (all 3 high)

{
  dir (1);
  for (i=0;i<r;i++)
  { 
   for (ij=0;ij<800;ij++)
   {
     digitalWrite (p1, HIGH);
     digitalWrite (p2, HIGH);
     digitalWrite (p3, HIGH);
     delayMicroseconds (sp);
     if (digitalRead(U1)==LOW)digitalWrite (p1, LOW);
     if (digitalRead(U2)==LOW)digitalWrite (p2, LOW);
     if (digitalRead(U3)==LOW)digitalWrite (p3, LOW);
     delayMicroseconds (sp);
   } 
  }
}





//////////////////////////////////////DIR///////////////////////////////////////////
void dir (int e)
{
  if (e==1)
  {digitalWrite (d1, LOW);digitalWrite (d2, LOW);digitalWrite (d3, LOW);} // up
  
  if (e==0)
  {digitalWrite (d1, HIGH);digitalWrite (d2, HIGH);digitalWrite (d3, HIGH);} // down
}