// Rev3 Xbee enabled

//MOTOR1 RIGHT

int M1_IN1 = 11;
int M1_IN2 = 10;

//MOTOR2 LEFT

int M2_IN1 = 9;
int M2_IN2 = 3;

int incomingByte = 0;

// the setup routine runs once when you press reset:
void setup() {
  pinMode(M1_IN1, OUTPUT);
  pinMode(M1_IN2, OUTPUT);

  pinMode(M2_IN1, OUTPUT);
  pinMode(M2_IN2, OUTPUT);

  Serial.begin(9600);
  off();
}

// the loop routine runs over and over again forever:
void loop() {
  if (Serial.available() > 0) {
    // read the incoming byte:
    incomingByte = Serial.read();
    // say what you got:
    Serial.println("I received: ");
    Serial.write(incomingByte);
    if(incomingByte=='w'){
      forward();
    }
    if (incomingByte=='s'){
      reverse();
    }
    if(incomingByte=='q')
      off();
  }
  if(incomingByte=='d'){
    right();
  }
  if(incomingByte=='a'){
    left();
  }

}


void forward(){
  digitalWrite(M1_IN2,LOW);
  analogWrite(M1_IN1,-200);

  digitalWrite(M2_IN2,LOW);
  analogWrite(M2_IN1,-200);

}

void reverse(){

  digitalWrite(M1_IN1,LOW);
  analogWrite(M1_IN2,220);

  digitalWrite(M2_IN1,LOW);
  analogWrite(M2_IN2,220);

}

void off(){
  digitalWrite(M1_IN1,HIGH);
  digitalWrite(M1_IN2,HIGH);

  digitalWrite(M2_IN1,HIGH);
  digitalWrite(M2_IN2,HIGH);
}

void right(){
  digitalWrite(M1_IN2,LOW);
  analogWrite(M1_IN1,-200);

  digitalWrite(M2_IN1,HIGH);
  digitalWrite(M2_IN2,HIGH);
}

void left(){
  digitalWrite(M2_IN2,LOW);
  analogWrite(M2_IN1,-200);

  digitalWrite(M1_IN1,HIGH);
  digitalWrite(M1_IN2,HIGH);
}