#define MAX_SPEED 250
//Left Motor connection
int enL = 11;
int in1 = 10;
int in2 = 9;
//Right Motor connection
int enR = 6;
int in3 = 5;
int in4 = 3;

int i;

void setup()
{
  //Assign DDR
  pinMode(enL,OUTPUT);
  pinMode(in1,OUTPUT);
  pinMode(in2,OUTPUT);
  pinMode(enR,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  //Set initial state as "off"
  digitalWrite(enL,LOW);
  digitalWrite(in1,LOW);
  digitalWrite(in2,LOW);
  digitalWrite(enR,LOW);
  digitalWrite(in3,LOW);
  digitalWrite(in4,LOW);
}

void loop()
{
  testMax();
  testGradual();
}

/**
 * Sets drive to forward movement
 */
void moveForward()
{
  setL(250);
  setR(250);
}

/**
 * Sets drive to forward movement with speed control
 */
void moveForward(int a)
{
  if(a > MAX_SPEED) {
    setL(MAX_SPEED);
    setR(MAX_SPEED);
  } else if (a <= 0) {
    stop();
  } else {
    setL(a);
    setR(a);
  }
}

/**
 * Sets drive to backward movement
 */
void moveBackward()
{
  setL(-250);
  setR(-250);
}

/**
 * Sets drive to backward movement with speed control
 */
void moveBackward(int a)
{
  if(a > MAX_SPEED) {
    setL(MAX_SPEED * (-1));
    setR(MAX_SPEED * (-1));
  } else if (a <= 0) {
    stop();
  } else {
    setL(a *(-1));
    setR(a *(-1));
  }
}

/**
 * Sets drive to move counterclockwise
 */
void moveLeft()
{
  setL(50);
  setR(250);
}

/**
 * Sets drive to move counterclockwise with speed control
 */
void moveLeft(int a)
{
  if(a > MAX_SPEED) {
  	setL(MAX_SPEED/2);
    setR(MAX_SPEED);
  } else if (a < MAX_SPEED * (-1)) {
    setL(MAX_SPEED/-2);
    setR(MAX_SPEED * (-1));
  } else {
    setL(a/2);
    setR(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  }
}

/**
 * Sets drive to move clockwise
 */
void moveRight()
{
  setL(250);
  setR(50);
}

/**
 * Sets drive to move counterclockwise with speed control
 */
void moveRight(int a)
{
  if(a > MAX_SPEED) {
  	setR(MAX_SPEED/2);
    setL(MAX_SPEED);
  } else if (a < MAX_SPEED * (-1)) {
    setR(MAX_SPEED/-2);
    setL(MAX_SPEED * (-1));
  } else {
    setR(a/2);
    setL(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  }
}

/**
 * Sets drive to stop fully
 */
void stop()
{
  setL(0);
  setR(0);
}


/**
 * Left Motor control
 */
void setL(int a)
{
  digitalWrite(in1,(a>0)?HIGH:LOW);
  digitalWrite(in2,(a<0)?HIGH:LOW);
  analogWrite(enL,abs(a));
}


/**
 * Right Motor control
 */
void setR(int a)
{
  digitalWrite(in3,(a>0)?HIGH:LOW);
  digitalWrite(in4,(a<0)?HIGH:LOW);
  analogWrite(enR,abs(a));
}


/**
 * Test sequence of parameter-free actions
 */
void testMax()
{
  //Move Forwards
  moveForward();
  delay(4000);
  stop();
  delay(4000);
  //Move Backwards
  moveBackward();
  delay(4000);
  stop();
  delay(4000);
  //Turn Left
  moveLeft();
  delay(4000);
  stop();
  delay(4000);
  //Turn Right
  moveRight();
  delay(4000);
  stop();
  delay(4000);
}

/**
 * Test sequence of parametered actions
 */
void testGradual()
{
  //Accelarate Forwards
  for(i=0;i<=MAX_SPEED;i++) {
    moveForward(i);
    delay(10);
  }
  delay(4000);
  stop();
  delay(4000);
  //Accelerate Backwards
  for(i=0;i<=MAX_SPEED;i++) {
    moveBackward(i);
    delay(10);
  }
  delay(4000);
  stop();
  delay(4000);
  //Turn Backwards-Left, then Forwards-Left
  for(i=0-MAX_SPEED;i<=MAX_SPEED;i++) {
    moveLeft(i);
    delay(10);
  }
  delay(4000);
  stop();
  delay(4000);
  //Turn Backwards-Right, then Forwards-Right
  for(i=0-MAX_SPEED;i<=MAX_SPEED;i++) {
    moveRight(i);
    delay(10);
  }
  delay(4000);
  stop();
  delay(4000);
}

/**
 * Return sign of number
 */
int getSign(int a){
  return a/abs(a);
}