/* Jazda w przod i tyl dwa silniki*/
#include <Servo.h>
Servo centerServo;
const int motorDirectionA = 12;
const int motorDirectionB = 13;
const int motorSpeedA = 3;
const int motorSpeedB = 11;
const int motorBrakeA = 9;
const int motorBrakeB = 8;
const int fullSpeed = 255;
const int halfSpeed = 123;
char command;
void setup() {
  Serial.begin(9600);
  pinMode(16, OUTPUT);
  centerServo.attach(16);
  pinMode(motorDirectionA, OUTPUT);
  pinMode(motorBrakeA, OUTPUT);
  pinMode(motorDirectionB, OUTPUT);
  pinMode(motorBrakeB, OUTPUT);
  centerServo.write(90);
}
void forward() {
  digitalWrite(motorDirectionA, LOW); //Forward Direction for Motor A
  digitalWrite(motorDirectionB, HIGH); //Forward Direction for Motor B
  digitalWrite(motorBrakeA, LOW); //Turn off brake for Motor A
  digitalWrite(motorBrakeB, LOW); //Turn off brake for Motor B
  analogWrite(motorSpeedA, 220); //Set speed of Motor A to MAX.
  analogWrite(motorSpeedB, 220); //Set speed of Motor A to MAX.
}
void backward() {
  digitalWrite(motorDirectionA, HIGH); //Backward Direction for Motor A
  digitalWrite(motorDirectionB, LOW); //Backward Direction for Motor B
  digitalWrite(motorBrakeA, LOW); //Turn off brake for Motor A
  digitalWrite(motorBrakeB, LOW); //Turn off brake for Motor B
  analogWrite(motorSpeedA, 220); //Set speed of Motor A to half of MAX.
  analogWrite(motorSpeedB, 220); //Set speed of Motor A to half of MAX.
}
void right() {
  digitalWrite(motorDirectionA, HIGH); //Backward Direction for Motor A
  digitalWrite(motorDirectionB, LOW); //Backward Direction for Motor B
  digitalWrite(motorBrakeA, LOW); //Turn off brake for Motor A
  digitalWrite(motorBrakeB, LOW); //Turn off brake for Motor B
  analogWrite(motorSpeedA, 0); //Set speed of Motor A to half of MAX.
  analogWrite(motorSpeedB, 220); //Set speed of Motor A to half of MAX.
}
void left() {
  digitalWrite(motorDirectionA, HIGH); //Backward Direction for Motor A
  digitalWrite(motorDirectionB, LOW); //Backward Direction for Motor B
  digitalWrite(motorBrakeA, LOW); //Turn off brake for Motor A
  digitalWrite(motorBrakeB, LOW); //Turn off brake for Motor B
  analogWrite(motorSpeedA, 220); //Set speed of Motor A to half of MAX.
  analogWrite(motorSpeedB, 0); //Set speed of Motor A to half of MAX.
}
void brake() {
  digitalWrite(motorBrakeA, HIGH); //Turn on brake for Motor A
  digitalWrite(motorBrakeB, HIGH); //Turn on brake for Motor B
}
void loop() {
  if (Serial.available() > 0) {
    command = Serial.read();
    switch (command) {
      case 'F':
        forward();
        break;
      case 'B':
        backward();
        break;
      case 'L':
        left();
        break;
      case 'R':
        right();
        break;
      case 'S':
        brake();
        break;
      case 'V':
        centerServo.write(90);
        break;
      case '0':
        centerServo.write(20);
        break;
        case '1':
        centerServo.write(30);
        break;
        case '2':
        centerServo.write(40);
        break;
        case '3':
        centerServo.write(60);
        break;
        case '4':
        centerServo.write(80);
        break;
        case '5':
        centerServo.write(100);
        break;
        case '6':
        centerServo.write(120);
        break;
        case '7':
        centerServo.write(140);
        break;
        case '8':
        centerServo.write(160);
        break;
    }
  }
}