Antenna Turret Toy with Computer Tracking

/*  AZ/EL Rotator toy controller for Arduino with servos
 *  ========================================================
 *  Uses EasyComm2 protocol for computer - Tracking Software
 *  servomotors to move the antenna in Az (360 deg) and El (90 deg)
 *  
 *  Viorel Racoviteannu
 *  https://www.youtube.com/channel/UCiRLZX0bV9rS04BGAyUf-fA
 *  https://racov.ro
 *  YO3RAK@gmail.com
 *  
 * I cannot take any responsibility for missuse of this code
 * or any kind of damage it may occur from using this code.
 * 
 */
#include <Wire.h>
/*
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSans18pt7b.h>
#include <Fonts/FreeSans12pt7b.h>
Adafruit_SSD1306 display(4);
*/
#include <Servo.h>
Servo AzServo;  // create servo object to control the azimuth servo
Servo ElServo;  // create servo object to control the elevation servo

  String Azimuth = "";
  String Elevation = "";
  String ComputerRead = "";
  String ComputerWrite = "";
  int ComAzim = 0;
  int ComElev = 0;

void setup()   {
  Serial.begin(9600);
  Serial.setTimeout(50);           // miliseconds to wait for USB sata. Default 1000
/*
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3C (for the 128x32)
  display.display();    
  display.setTextColor(WHITE);
  display.setFont(&FreeSans12pt7b);
  delay(2000);
  display.clearDisplay();                     // Clear the buffer
  
  display.setCursor(-3,24);
  display.println("EasyComm2");
  display.display();
  delay (1000);
  display.setFont(&FreeSans18pt7b);
*/  
  AzServo.attach(5);  // connect Az servo to pin 5
  ElServo.attach(6);  // connect Az servo to pin 6


   /********************** make a short movement *************************/
  ComAzim = 0;
  ComElev = 40;
  Rotate();                // center the servos
  delay(100);
  for (ComAzim = 0; ComAzim <= 45; ComAzim += 1) { // goes from 0 degrees to 45 degrees
    // in steps of 1 degree
    Rotate();              // tell servo to go to position
    delay(20);             // waits 20ms for the servo to reach the position
  }
  for (ComElev = 20; ComElev <= 75; ComElev += 1) {
    Rotate();
    delay(20);
  }
  ComAzim = 0;
  ComElev = 35;
  Rotate();
  DisplayPosition();

}

void loop() {

  if (Serial.available()){
    SerComm();
    Rotate();
    DisplayPosition();  
    delay (100);
  }
  
}

void SerComm() {
  // initialize readings
  ComputerRead = "";
  Azimuth = "";
  Elevation = "";

  while(Serial.available()) {
    ComputerRead= Serial.readString();  // read the incoming data as string
//    Serial.println(ComputerRead);     // echo the reception for testing purposes
  }
  
// looking for command <AZxxx.x>
    for (int i = 0; i <= ComputerRead.length(); i++) {
     if ((ComputerRead.charAt(i) == 'A')&&(ComputerRead.charAt(i+1) == 'Z')){ // if read AZ
      for (int j = i+2; j <= ComputerRead.length(); j++) {
        if (isDigit(ComputerRead.charAt(j))) {                                // if the character is number
          Azimuth = Azimuth + ComputerRead.charAt(j);
        }
        else {break;}
      }
     }
    }
    
// looking for command <ELxxx.x>
    for (int i = 0; i <= (ComputerRead.length()-2); i++) {
      if ((ComputerRead.charAt(i) == 'E')&&(ComputerRead.charAt(i+1) == 'L')){ // if read EL
        if ((ComputerRead.charAt(i+2)) == '-') {
          ComElev = 0;                  // if elevation negative
          break;
        }
        for (int j = i+2; j <= ComputerRead.length(); j++) {
          if (isDigit(ComputerRead.charAt(j))) {                               // if the character is number
            Elevation = Elevation + ComputerRead.charAt(j);
          }
          else {break;}
        }
      }
    }
    
// if <AZxx> received
    if (Azimuth != ""){
      ComAzim = Azimuth.toInt();
      ComAzim = ComAzim%360;          // keeping values between limits(for trackers with more than 360 deg. rotation)
      }

// if <ELxx> received
    if (Elevation != ""){
      ComElev = Elevation.toInt();
      if (ComElev>180) { ComElev = 0;}
      if (ComElev>90) {               //if received more than 90deg. (for trackers with 180deg. elevation)
        ComElev = 180-ComElev;        //keep below 90deg.
        ComAzim = (ComAzim+180)%360;  //and rotate the antenna on the back
      }
    }

// looking for <AZ EL> interogation for antenna position
  for (int i = 0; i <= (ComputerRead.length()-4); i++) {
    if ((ComputerRead.charAt(i) == 'A')&&(ComputerRead.charAt(i+1) == 'Z')&&(ComputerRead.charAt(i+3) == 'E')&&(ComputerRead.charAt(i+4) == 'L')){
    // send back the antenna position <+xxx.x xx.x>
      ComputerWrite = "+"+String(ComAzim)+".0 "+String(ComElev)+".0";
      Serial.println(ComputerWrite);
    }
  }
}

void Rotate() {
  // rotate the servos to the coordinates received thru serial
  // because the servo rotates in the oposite direction, we have to reverse the movement
  if (ComElev >90){                // if received elevation more than 90 deg
    ComElev = 180-ComElev;         // keep it below 90
    ComAzim = (180+ComAzim)%360;   // and flip the antenna
  }
  if (ComAzim > 180) {              // if azim more than 180 deg
      AzServo.write(360-ComAzim);   // flip the antenna
      ElServo.write(180-ComElev);
    }
    else {
      AzServo.write(180-ComAzim);
      ElServo.write(ComElev);
    }
}

void DisplayPosition() {
  // display values
/*  
  display.clearDisplay();
  display.setCursor(0,28);
  display.print(ComAzim);
  display.print(" / ");
  display.print(ComElev);
  display.display();
*/
}