int sen1, sen2,sen3,sen4;


#define ENA_PIN  11
#define ENB_PIN   6
#define MOTOR_L_1 10
#define MOTOR_L_2 7
#define MOTOR_R_1 9
#define MOTOR_R_2 8

#define FORWARD 0
#define BACK    1
#define LEFT    2
#define RIGHT   3

int carSpeed = 150;
int runTime = 2000;
char blueToothVal;           //value sent over via bluetooth
char lastValue;              //stores last state of device (on/off)
uint8_t val =0;
uint8_t Lval =0;
long long t = 0;
void setup() {
  // put your setup code here, to run once:
    pinMode(ENA_PIN, OUTPUT);
    pinMode(ENB_PIN, OUTPUT);
    pinMode(MOTOR_L_1, OUTPUT);
    pinMode(MOTOR_L_2, OUTPUT);
    pinMode(MOTOR_R_1, OUTPUT);
    pinMode(MOTOR_R_2, OUTPUT);
    Serial.begin(9600);

        Serial.println("Copyright © (C) Frizman && WAndreyG");




}

void loop() {

  // put your main code here, to run repeatedly:
  val = 0;
  val |= digitalRead(2)<<0;
  val |= digitalRead(3)<<1;
  val |= digitalRead(4)<<2;
  val |= digitalRead(5)<<3;
  Serial.println(val);
switch(val)
{
  case 15 :
                CAR_move(FORWARD,250,250);t=millis();break;
  case 0 :  if((millis()-t >2000)&&(Lval !=6)){
                digitalWrite(MOTOR_L_1,HIGH);
                digitalWrite(MOTOR_L_2,HIGH);
                digitalWrite(MOTOR_R_1,HIGH);
                digitalWrite(MOTOR_R_2,HIGH);}break;

  case 9 :
                  CAR_move(FORWARD,carSpeed,carSpeed);t=millis();break;
  case 6 :
                  CAR_move(FORWARD,250,250);t=millis();break;
  case 3 :
                  CAR_move(RIGHT,150,150);t=millis();break;
  case 7 :
                  CAR_move(RIGHT,150,150);t=millis();break;
  case 12 :
                  CAR_move(LEFT,150,150);t=millis();break;
  case 14 :
                  CAR_move(LEFT,150,150);t=millis();break;
  case 8 :
                  CAR_move(LEFT,150,150);t=millis();break;
  case 1 :
                  CAR_move(RIGHT,150,150);t=millis();break;
}
Lval =val;
}




void CAR_move(unsigned char direction, unsigned char speed_left, unsigned char speed_right){
    switch(direction){
        case 0: digitalWrite(MOTOR_L_1,HIGH);
                digitalWrite(MOTOR_L_2,LOW);
                digitalWrite(MOTOR_R_1,HIGH);
                digitalWrite(MOTOR_R_2,LOW);
                break;
        case 2: digitalWrite(MOTOR_L_1,HIGH);
                digitalWrite(MOTOR_L_2,LOW);
                digitalWrite(MOTOR_R_1,HIGH);
                digitalWrite(MOTOR_R_2,LOW);;
                speed_right -= 125;
                speed_left += 105;
                break;
        case 3: digitalWrite(MOTOR_L_1,HIGH);
                digitalWrite(MOTOR_L_2,LOW);
                digitalWrite(MOTOR_R_1,HIGH);
                digitalWrite(MOTOR_R_2,LOW);
                speed_right += 105;
                speed_left -= 125;
                break;

        default: break;
    }
    analogWrite(ENA_PIN,speed_left);
    analogWrite(ENB_PIN,speed_right);
}