#define LF 0
#define RT 1
int E1 = 9; //M1 Speed Control
int E2 = 10; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 11; //M2 Direction Control
int a;
int b;
int counter = 0;

void setup() {
  int i;
  for (i = 4; i <= 7; i++)
    pinMode(i, OUTPUT);
  Serial.begin(57600); //Set Baud Rate
  Serial.println("Run keyboard control");
  digitalWrite(E1, LOW);
  digitalWrite(E2, LOW);

}

void loop() {
  Motor(2000, LF);
  Motor(1000, RT);

}
void Motor(int value, byte whichwheel)
{
  value = constrain(value, 1000, 2000);
  if (whichwheel == LF) {
    if (value > 1500) {
      a = (value - 1500) / 1.961;
      analogWrite (E1, a);
      digitalWrite(M1, HIGH);
    }
    else {
      a = (1500 - value) / 2;
      analogWrite (E1, a);
      digitalWrite(M1, LOW);
    }
  }
  else if (whichwheel == RT) {
    if (value > 1500) {
      b = (value - 1500) / 1.961;
      analogWrite (E2, b);
      digitalWrite(M2, HIGH);
    }
    else {
      b = (1500 - value) / 2;
      analogWrite (E2, b);
      digitalWrite(M2, LOW);
    }
  }
}
