123

#include <ShiftLCD.h>

#include <AFMotor.h>
 #include <AFMotor.h>
#define trigPin 12
#define echoPin 11
AF_DCMotor motorRight(4);
AF_DCMotor motorLeft(3);
#include <ShiftLCD.h>
ShiftLCD lcd(2, 4, 3);

char dataIn = 'S';
char determinant;
char det;
int vel = 0;
int frontLight = 13;
int horn = 7;
void setup() {
  lcd.begin(16, 2);
Serial.begin(9600);
pinMode(frontLight, OUTPUT);
pinMode(horn, OUTPUT);

lcd.print("Robot Arduino");
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  motorRight.setSpeed(255);
motorLeft.setSpeed (255);

motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(RELEASE);
motorLeft.run(RELEASE);
digitalWrite(frontLight, LOW);
noTone(horn);
}
void loop() {

det = check();


switch (det){

case 'F':
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("GO Forward");
delay(100);
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(BACKWARD);
motorLeft.run(BACKWARD);
det = check();
break;


case 'B':
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0,0);

lcd.print("Go Backward");
delay(00);
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(FORWARD);
motorLeft.run(FORWARD);
det = check();
break;
case 'L':
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Turn Left");
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(FORWARD);
motorLeft.run(BACKWARD);
det = check();
break;
case 'R':
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Turn Right");
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(BACKWARD);
motorLeft.run(FORWARD);
det = check();
break;


break;
case 'S': // S, stop
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Czekam na przycisk");
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
motorRight.run(RELEASE);
motorLeft.run(RELEASE);
det = check();
break;
case 'V':
tone(horn, 800, 100);
break;
case 'v':
noTone(horn);
break;
case 'W':
digitalWrite(frontLight, HIGH);
break;
case 'w':
digitalWrite(frontLight, LOW);
break;

case 'H':
while(true){
if(dataIn == '1') break;
   long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance < 15) {
motorRight.setSpeed(255);
motorLeft.setSpeed(255);
    motorRight.run(BACKWARD);
    motorLeft.run (FORWARD);

}
  else {

   delay (15);
   motorRight.setSpeed(255);
motorLeft.setSpeed(255);
   motorRight.run(BACKWARD);
    motorLeft.run(BACKWARD);
    break;
  }
}





}
}

int check(){
if (Serial.available() > 0){
dataIn = Serial.read();
//check the code
if (dataIn == 'F'){
determinant = 'F';
}
else if (dataIn == 'B'){
determinant = 'B';
}
else if (dataIn == 'L'){
determinant = 'L';
}
else if (dataIn == 'R'){
determinant = 'R';
}
else if (dataIn == 'I'){
determinant = 'I';
}
else if (dataIn == 'J'){
determinant = 'J';
}
else if (dataIn == 'G'){
determinant = 'G';
}
else if (dataIn == 'H'){
determinant = 'H';
}
else if (dataIn == 'S'){
determinant = 'S';
}
else if (dataIn == '0'){
vel = 0;
}
else if (dataIn == '1'){
vel = 25;
}
else if (dataIn == '2'){
vel = 50;
}
else if (dataIn == '3'){
vel = 75;
}
else if (dataIn == '4'){
vel = 100;
}
else if (dataIn == '5'){
vel = 125;
}
else if (dataIn == '6'){
vel = 150;
}
else if (dataIn == '7'){
vel = 175;
}
else if (dataIn == '8'){
vel = 200;
}
else if (dataIn == '9'){
vel = 225;
}
else if (dataIn == 'q'){
vel = 255;
}
else if (dataIn == 'U'){
determinant = 'U';
}
else if (dataIn == 'u'){
determinant = 'u';
}
else if (dataIn == 'W'){
determinant = 'W';
}
else if (dataIn == 'w'){
determinant = 'w';
}
else if (dataIn == 'V'){
determinant = 'V';
}
else if (dataIn == 'v'){
determinant = 'v';
}
else if (dataIn == 'X'){
determinant = 'X';
}
else if (dataIn == 'x'){
determinant = 'x';
}
}
return determinant;
}