/*
   Bumper robot code written by: Kristian Blåsol 6/9/2015
   BYJ48 Stepper motor code written By :Mohannad Rawashdeh 28/9/2013
*/

//Stepper1
#define IN11  8
#define IN12  9
#define IN13  10
#define IN14  11

//Stepper2
#define IN21  4
#define IN22  5
#define IN23  6
#define IN24  7

#define leftBumper 2
#define rightBumper 3

int Steps1 = 0;
int Steps2 = 0;
//defaults to forward
boolean Direction1 = false;// gre
boolean Direction2 = true;// gre
//dir1=1 dir2=0 goes backwards
//dir1=1 dir2=1 turns right
//dir1=0 dir2=0 turns left
//dir1=0 dir2=1 goes forward

unsigned long last_time;
unsigned long currentMillis ;
int steps_left1=4095; //4095 is a whole rotation
int steps_left2=4095; //4095 is a whole rotation
long time;


void setup()
{
//Serial.begin(115200);
pinMode(IN11, OUTPUT); 
pinMode(IN12, OUTPUT); 
pinMode(IN13, OUTPUT); 
pinMode(IN14, OUTPUT); 
// delay(1000);
pinMode(IN21, OUTPUT); 
pinMode(IN22, OUTPUT); 
pinMode(IN23, OUTPUT); 
pinMode(IN24, OUTPUT); 


}
void loop()
{
  while(1>0){
    //check bumpers
    int readLeft = digitalRead(leftBumper);
    int readRight = digitalRead(rightBumper);    
   if (readLeft == HIGH) {
     //back up a bit
     steps_left1=1024;
     steps_left2=1024;
    while(steps_left1>0){
       //dir1=1 dir2=0 goes backwards
       Direction1 = true;
       Direction2 = false;
       currentMillis = micros();
      if(currentMillis-last_time>=1000){
        stepper1(1); 
        stepper2(1); 
        time=time+micros()-last_time;
        last_time=micros();
        steps_left1--;
        steps_left2--;
      }
    }     
      //turn right
      steps_left1=1024;
      steps_left2=1024;
      //dir1=1 dir2=1 turns right
    while(steps_left1>0){
       Direction1 = true;
       Direction2 = true;
       currentMillis = micros();
      if(currentMillis-last_time>=1000){
        stepper1(1); 
        stepper2(1); 
        time=time+micros()-last_time;
        last_time=micros();
        steps_left1--;
        steps_left2--;
      }     
    }
    }
    if (readRight == HIGH) {
      //back up a bit
     steps_left1=1024;
     steps_left2=1024;
     //dir1=1 dir2=0 goes backwards
    while(steps_left1>0){
       Direction1 = true;
       Direction2 = false;
       currentMillis = micros();
      if(currentMillis-last_time>=1000){
        stepper1(1); 
        stepper2(1); 
        time=time+micros()-last_time;
        last_time=micros();
        steps_left1--;
        steps_left2--;
      }
    }
      //turn left
      steps_left1=1024;
      steps_left2=1024;
      //dir1=0 dir2=0 turns left
    while(steps_left1>0){
       Direction1 = false;
       Direction2 = false;
       currentMillis = micros();
      if(currentMillis-last_time>=1000){
        stepper1(1); 
        stepper2(1); 
        time=time+micros()-last_time;
        last_time=micros();
        steps_left1--;
        steps_left2--;
      }     
    }

    }
    currentMillis = micros();
    if(currentMillis-last_time>=1000){
      //moving forward
      //defaults to moving forward
      Direction1 = false;
      Direction2 = true;
      stepper1(1); 
      stepper2(1); 
      time=time+micros()-last_time;
      last_time=micros();
    }
  }
}

void stepper1(int xw){
  for (int x=0;x<xw;x++){
switch(Steps1){
   case 0:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, HIGH);
   break; 
   case 1:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, HIGH);
     digitalWrite(IN14, HIGH);
   break; 
   case 2:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, HIGH);
     digitalWrite(IN14, LOW);
   break; 
   case 3:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, HIGH);
     digitalWrite(IN13, HIGH);
     digitalWrite(IN14, LOW);
   break; 
   case 4:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, HIGH);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, LOW);
   break; 
   case 5:
     digitalWrite(IN11, HIGH); 
     digitalWrite(IN12, HIGH);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, LOW);
   break; 
     case 6:
     digitalWrite(IN11, HIGH); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, LOW);
   break; 
   case 7:
     digitalWrite(IN11, HIGH); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, HIGH);
   break; 
   default:
     digitalWrite(IN11, LOW); 
     digitalWrite(IN12, LOW);
     digitalWrite(IN13, LOW);
     digitalWrite(IN14, LOW);
   break; 
}
SetDirection1();
}
} 

void stepper2(int xw){
  for (int x=0;x<xw;x++){
switch(Steps2){
   case 0:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, HIGH);
   break; 
   case 1:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, HIGH);
     digitalWrite(IN24, HIGH);
   break; 
   case 2:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, HIGH);
     digitalWrite(IN24, LOW);
   break; 
   case 3:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, HIGH);
     digitalWrite(IN23, HIGH);
     digitalWrite(IN24, LOW);
   break; 
   case 4:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, HIGH);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, LOW);
   break; 
   case 5:
     digitalWrite(IN21, HIGH); 
     digitalWrite(IN22, HIGH);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, LOW);
   break; 
     case 6:
     digitalWrite(IN21, HIGH); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, LOW);
   break; 
   case 7:
     digitalWrite(IN21, HIGH); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, HIGH);
   break; 
   default:
     digitalWrite(IN21, LOW); 
     digitalWrite(IN22, LOW);
     digitalWrite(IN23, LOW);
     digitalWrite(IN24, LOW);
   break; 
}
SetDirection2();
}
}


void SetDirection1(){
if(Direction1==1){ Steps1++;}
if(Direction1==0){ Steps1--; }
if(Steps1>7){Steps1=0;}
if(Steps1<0){Steps1=7; }
}
void SetDirection2(){
if(Direction2==1){ Steps2++;}
if(Direction2==0){ Steps2--; }
if(Steps2>7){Steps2=0;}
if(Steps2<0){Steps2=7; }
}