////////////////////////////////////////////////
// This demo code uses 2 stepper motors - one on each wheel of the mini-bot
// Steers Right and Left t reach the closest light source
//
// This code may be freely used and copied.
//
// Gareth Davies - May 2013
//
////////////////////////////////////////////////

// declare pins for the light sensors
// Use analog inputs A5 for Left, A4 for Right
// Use internal 10K pullups on analog pins, to reduce component count
#define leftLDR  A5
#define rightLDR A4

//declare variables for the motor pins
//Left Motors
int lPin1 = 8;	// Blue   - 28BYJ48 pin 1
int lPin2 = 9;	// Pink   - 28BYJ48 pin 2
int lPin3 = 10;	// Yellow - 28BYJ48 pin 3
int lPin4 = 11;	// Orange - 28BYJ48 pin 4

//Right Motors
int rPin1 = 4;	// Blue   - 28BYJ48 pin 1
int rPin2 = 5;	// Pink   - 28BYJ48 pin 2
int rPin3 = 6;	// Yellow - 28BYJ48 pin 3
int rPin4 = 7;	// Orange - 28BYJ48 pin 4
                // Red    - 28BYJ48 pin 5 (VCC)

int maxSpeed = 1200;  // maximum speed of stepper motors- smaller numbers = faster
int lDelay = maxSpeed;  // variable for left stepper speed
int rDelay = maxSpeed;  // variable for right stepper speed
int lIdx = 0; // index into step lookup array for left motor
int rIdx = 7; // index into step lookup array for right motor (NB. motors rotate in opposite directions)
unsigned long lTgt, rTgt, tNow; // long values for current time and target for next step on each motor
int hysteresis = 10;  // LDR values must differ by at least this amount before sterring occurs - reduces jitter
int lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};

//////////////////////////////////////////////////////////////////////////////
void setup() {
  //declare the motor pins as outputs
  pinMode(lPin1, OUTPUT);
  pinMode(lPin2, OUTPUT);
  pinMode(lPin3, OUTPUT);
  pinMode(lPin4, OUTPUT);
  pinMode(rPin1, OUTPUT);
  pinMode(rPin2, OUTPUT);
  pinMode(rPin3, OUTPUT);
  pinMode(rPin4, OUTPUT);
  
  // declare LDR pins as inputs and enable the internal pullups
  pinMode(leftLDR, INPUT);
  pinMode(rightLDR, INPUT);
  digitalWrite(leftLDR, HIGH);  // set pullup
  digitalWrite(rightLDR, HIGH);  // set pullup
  
  // set initial target times for first step
  tNow = micros();
  lTgt = tNow + lDelay;
  rTgt = tNow + rDelay;
}

//////////////////////////////////////////////////////////////////////////////
// This is much more complex than Demo01 as the 2 stepper motors must be stepped at different speeds to adjust the direction
// So the loop contains 2 values (1 for left motor and 1 for right motor)
// when the current time reaches the selected value, the appropriate motor is stepped and the target value is reset
// Check the LDRs everytime either motor is stepped and adjust the delays so the motor closes to the light goes slower and other motor is full speed.
void loop()
{
  bool check = false;
  tNow = micros();
  if (tNow > lTgt)
  {
    lStep();
    lTgt = tNow + lDelay;
    check = true;
  }
  tNow = micros();
  if (tNow > rTgt)
  {
    rStep();
    rTgt = tNow + rDelay;
    check = true;
  }
  if(check)
  {
    int lLDR = analogRead(leftLDR);
    int rLDR = analogRead(rightLDR);
    lDelay = maxSpeed;
    rDelay = maxSpeed; // defualt to maxSpeed if no significant light difference
    if((rLDR - lLDR) > hysteresis) //lower number means brighter
    {
      lDelay = maxSpeed;
//      lDelay = maxSpeed + (rLDR - lLDR)*40; // use this line to steer harder if light difference is higher
      rDelay = maxSpeed + (1000);
    }
    else if((lLDR - rLDR) > hysteresis)
    {
      lDelay = maxSpeed + (1000);
      rDelay = maxSpeed;
//      rDelay = maxSpeed + (lLDR - rLDR)*40; // use this line to steer harder if light difference is higher
    }
  }
}

//////////////////////////////////////////////////////////////////////////////
//set pins to ULN2003 high in sequence from 1 to 4
// left motor goes opposite direction to the right notor
void rStep()
{
  digitalWrite(rPin1, bitRead(lookup[rIdx], 0));
  digitalWrite(rPin2, bitRead(lookup[rIdx], 1));
  digitalWrite(rPin3, bitRead(lookup[rIdx], 2));
  digitalWrite(rPin4, bitRead(lookup[rIdx], 3));
  rIdx++;
  if(rIdx > 7)
    rIdx = 0;
}

void lStep()
{
  digitalWrite(lPin1, bitRead(lookup[lIdx], 0));
  digitalWrite(lPin2, bitRead(lookup[lIdx], 1));
  digitalWrite(lPin3, bitRead(lookup[lIdx], 2));
  digitalWrite(lPin4, bitRead(lookup[lIdx], 3));
  lIdx--;
  if(lIdx < 0)
    lIdx = 7;
}

