#include <SoftwareSerial.h>

SoftwareSerial mySerial(12, 11); // RX, TX
#define E1 9 // Enable Pin for motor 1
#define E2 10

#define I1 4  // Control pin 1 for motor 1
#define I2 5  // Control pin 2 for motor 1

#define I3 6  // Control pin 1 for motor 2
#define I4 7  // Control pin 2 for motor 2

#define led 13

byte incomingByte = 0;// for incoming serial data
int rychlost = 255;
byte incoming2Byte = 0;
int mod = 0;
int sensorValue;
void setup() {

    mySerial.begin(9600);
    pinMode(E1, OUTPUT);
    pinMode(I1, OUTPUT);
    pinMode(I2, OUTPUT);
    pinMode(I3, OUTPUT);
    pinMode(I4, OUTPUT);
    pinMode(led, OUTPUT);
}

void test(){
 if(mod == 1){
        sensorValue = analogRead(0);
        if(sensorValue > 600){
          digitalWrite(I1, LOW);
          digitalWrite(I2, LOW);
          digitalWrite(I3, LOW);
          digitalWrite(I4, LOW);
          delay(200);
          digitalWrite(I4, HIGH);
          digitalWrite(I1, HIGH);
          delay(1000);
          digitalWrite(I4, LOW);
          digitalWrite(I1, LOW);
          delay(200);

        }
      }
}
void loop() {
    /*
    if(analogRead(0) == 0){
      if(mod == 0){
        mod = 1;
        digitalWrite(led, HIGH);
        delay(500);
      }else if(mod == 1){
        mod = 0;
        digitalWrite(led, LOW);
        delay(500);
      }
    }

    if(mod == 1){
       if(mySerial.available() > 2){
            incomingByte = mySerial.read();
            incoming2Byte = mySerial.read();
       }
      if(incomingByte == 'S' && incoming2Byte == 'S'){
          digitalWrite(I1, LOW);
          digitalWrite(I2, LOW);
          digitalWrite(I3, LOW);
          digitalWrite(I4, LOW);
          analogWrite(E1, 0);
          analogWrite(E2, 0);
      }
      if(incomingByte == 'F' && incoming2Byte == 'F'){
          digitalWrite(I1, HIGH);
          digitalWrite(I3, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
      }
      if(incomingByte == 'B' && incoming2Byte == 'B'){
          digitalWrite(I2, HIGH);
          digitalWrite(I4, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
      }
      if(incomingByte == 'F' && incoming2Byte == 'B'){
          digitalWrite(I4, HIGH);
          digitalWrite(I1, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
      }
      if(incomingByte == 'B' && incoming2Byte == 'F'){
          digitalWrite(I3, HIGH);
          digitalWrite(I2, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
      }
    } else{
      */

      if(mySerial.available() > 0)
          incomingByte = mySerial.read();
      switch (incomingByte) {
        case 'X':
          mod = 1;
          digitalWrite(led, HIGH);
          break;
        case 'x':
          mod = 0;
          digitalWrite(led, LOW);
          break;
        case 'H':
          digitalWrite(I2, HIGH);
          digitalWrite(I4, HIGH);
          analogWrite(E2, rychlost);
          analogWrite(E1, ((rychlost/100)*70));
          break;
        case 'J':
          digitalWrite(I2, HIGH);
          digitalWrite(I4, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, ((rychlost/100)*70));
          break;
        case 'I':
          digitalWrite(I1, HIGH);
          digitalWrite(I3, HIGH);
          analogWrite(E2, rychlost);
          analogWrite(E1, ((rychlost/100)*70));
          break;
        case 'G':
          digitalWrite(I1, HIGH);
          digitalWrite(I3, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, ((rychlost/100)*70));
          break;
        case 'F':
          digitalWrite(I1, HIGH);
          digitalWrite(I3, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
          test();
          break;
        case 'B':
          digitalWrite(I4, HIGH);
          digitalWrite(I2, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
          break;
        case 'L':
          digitalWrite(I1, HIGH);
          digitalWrite(I3, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
          break;
        case 'R':
          digitalWrite(I2, HIGH);
          digitalWrite(I4, HIGH);
          analogWrite(E1, rychlost);
          analogWrite(E2, rychlost);
          break;
        case 'S':
          digitalWrite(I1, LOW);
          digitalWrite(I2, LOW);
          digitalWrite(I3, LOW);
          digitalWrite(I4, LOW);
          analogWrite(E1, 0);
          analogWrite(E2, 0);
          break;
        case '0':
          rychlost = 0;
          break;
        case '1':
          rychlost = 25;
          break;
        case '2':
          rychlost = 50;
          break;
        case '3':
          rychlost = 75;
          break;
        case '4':
          rychlost = 100;
          break;
        case '5':
          rychlost = 125;
          break;
        case '6':
          rychlost = 150;
          break;
        case '7':
          rychlost = 175;
          break;
        case '8':
          rychlost = 200;
          break;
        case '9':
          rychlost = 225;
          break;
        case 'q':
          rychlost = 255;
          break;
      }
   // }
}