ROV Controlling Sketch, Receiving Commands via UART

arduino_ROV.ino

#include <Servo.h>

Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

int x = 220;

void setup() {

  Serial.begin(115200);
  Serial.setTimeout(50);

  pinMode(2, OUTPUT);//m1
  pinMode(3, OUTPUT);//m2
  pinMode(4, OUTPUT); //m3
  pinMode(5, OUTPUT); //m4

  pinMode(6, OUTPUT); //arm1
  pinMode(7, OUTPUT); //arm1
  pinMode(8, OUTPUT); //arm1

  pinMode(10, OUTPUT); //arm2
  pinMode(11, OUTPUT); //arm2
  pinMode(12, OUTPUT); //arm2

  esc1.attach(2, minPulseRate, maxPulseRate);
  esc2.attach(3, minPulseRate, maxPulseRate);
  esc3.attach(4, minPulseRate, maxPulseRate);
  esc4.attach(5, minPulseRate, maxPulseRate);

}

void loop() {

  if (Serial.available() > 0) {

    String Str = Serial.readString();

    if (Str == "u") {

      esc1.write(50);
      esc2.write(50);
      Serial.print(Str);

    } else if (Str == "d" ) {

      esc1.write(130);
      esc2.write(130);
      Serial.print(Str);

    } else if (Str == "r" ) {

      esc3.write(130);
      esc4.write(50);
      Serial.print(Str);

    } else if (Str == "l" ) {
      esc3.write(50);
      esc4.write(130);
      Serial.print(Str);


    } else if (Str == "f") {

      Serial.print(Str);
      esc3.write(50);
      esc4.write(50);

    } else if (Str == "b") {

      esc3.write(130);
      esc4.write(130);

      Serial.print(Str);

    } else if (Str == "rotate") {

      digitalWrite(6, LOW);
      digitalWrite(7, HIGH);

      analogWrite(8, x);
      
      delay(3000);

      digitalWrite(6, LOW);
      digitalWrite(7, LOW);

      analogWrite(8, 0);
      Serial.print(Str);

    } else if (Str == "rotaate") {

      digitalWrite(6, HIGH);
      digitalWrite(7, LOW);

      analogWrite(8, x);

      delay(3000);

      digitalWrite(6, LOW);
      digitalWrite(7, LOW);

      analogWrite(8, 0);
      Serial.print(Str);

    } else if (Str == "o" ) {

      digitalWrite(9, HIGH);
      digitalWrite(10, LOW);

      analogWrite(11, x);
      delay(3000);

      digitalWrite(10, LOW);
      digitalWrite(9, LOW);
      analogWrite(11, 0);
      
      Serial.print(Str);

    } else if (Str == "c") {

      digitalWrite(10, HIGH);
      digitalWrite(9, LOW);
      analogWrite(11, x);

      delay(3000);

      digitalWrite(10, LOW);
      digitalWrite(9, LOW);
      analogWrite(11, 0);

      Serial.print(Str);

    } else if (Str == "s") {

      digitalWrite(10, LOW);
      digitalWrite(9, LOW);
      analogWrite(11, 0);

      digitalWrite(6, LOW);
      digitalWrite(7, LOW);
      analogWrite(8, 0);

      Serial.print(Str);

    } else if (Str == "t") {

      float d2 = -1000.00001 ;
      int test = 3 ;


      for ( int a = 0 ; a < 500 ; a++ ) {

        int raw = analogRead(A0);
        float depth =  (((raw - 100 ) * 2.6875) + .8) ;


        float voltage = ((((int) raw * 5.0 / 1024.0) + .02));    // voltage at the pin of the Arduino

        if ( test < raw  &&  d2 < depth ) {
          test = raw ;
          d2 = depth ;

        }

        delay(3);

      } float pressure = (d2 * 98.1) ;
      Serial.print(pressure); Serial.print("-->"); Serial.print(d2); Serial.print('\n'); delay(500) ;


    } else if (Str == "p") {

      tempC = analogRead(tempPin);           //read the value from the sensor
      tempC = (5.0 * tempC * 100.0) / 1024.0; //convert the analog data to temperature
      Serial.print((byte)tempC);             //send the data to the computer
      delay(1000);

    } else {

      x = Str.toInt();

      Serial.print(x);

    }

  }
}