#include <RedBot.h>
RedBotSensor left = RedBotSensor(A0);   // initialize a left sensor object on A0
RedBotSensor right = RedBotSensor(A1);  // initialize a right sensor object on A1
int ain1=13; //Right motor = a
int ain2=12;
int bin1=8; //Left motor = b
int bin2=9;
int pwma=11;
int pwmb=10;
int linethreshold = 700; 
int upperrange=900;
int basespeed=105;
int speeddelta=75;
int leftSpeed;   // variable used to store the leftMotor speed
int rightSpeed;  // variable used to store the rightMotor speed

void setup()
{
	Serial.begin(9600);
  pinMode(ain1,OUTPUT);
  pinMode(ain2,OUTPUT);
  pinMode(pwma,OUTPUT);
  pinMode(bin1,OUTPUT);
  pinMode(bin2,OUTPUT);
  pinMode(pwmb,OUTPUT);
}

void loop()
{
	Serial.print("left: ");
  Serial.println(right.read());
  Serial.print("right: ");
  Serial.print(left.read());
	// if the line is under the right sensor, adjust relative speeds to turn to the right
  if(right.read() < linethreshold && left.read()>linethreshold)
	{
		leftSpeed = basespeed + 2*speeddelta;
		rightSpeed = basespeed - speeddelta;
    turnright(rightSpeed,leftSpeed);
	}

	// if the line is under the left sensor, adjust relative speeds to turn to the left
	else if(left.read() < linethreshold && right.read()>linethreshold)
	{
		leftSpeed = basespeed - speeddelta;
		rightSpeed = basespeed + 2*speeddelta;
    turnleft(rightSpeed,leftSpeed);
	}
	
	// otherwise, run motors given the control speeds above.
  else {
    leftSpeed = basespeed;
		rightSpeed = basespeed;
    goforward(rightSpeed,leftSpeed);
  }
	delay(0);  // add a delay to decrease sensitivity.
}

void goforward(int spdr, int spdl) //Function to make both motors go forward
{
  digitalWrite(ain1,HIGH); //forward direction
  digitalWrite(ain2,LOW); //forward direction
  digitalWrite(bin1,LOW);
  digitalWrite(bin2,HIGH);
  analogWrite(pwma,spdr);
  analogWrite(pwmb,spdl);
}
void turnleft(int spdr, int spdl) //Function to make the motors turn left
{
  digitalWrite(ain1,HIGH); //forward direction
  digitalWrite(ain2,LOW); //forward direction
  digitalWrite(bin1,LOW);
  digitalWrite(bin2,HIGH);
  analogWrite(pwma,spdl);
  analogWrite(pwmb,spdr);
}
void turnright(int spdr, int spdl) //Function to make the motors turn right
{
  digitalWrite(ain1,HIGH); //forward direction
  digitalWrite(ain2,LOW); //forward direction
  digitalWrite(bin1,LOW);
  digitalWrite(bin2,HIGH);
  analogWrite(pwma,spdl);
  analogWrite(pwmb,spdr);
}