wg2_optimized.c

#include <SoftwareSerial.h>
#include <Servo.h>
#include <PWM.h>
byte countByte = 0;
int byteTemp[4];
bool change = false;
int tt = 0;
int serAng = 90;
int serAngy = 90;
int delayTime = 7;
int a, b, c;
int ppx = 0;
int ang = 1;
Servo myservo;
Servo myservo2;


SoftwareSerial mySerial(10, 11); // TX, RX
unsigned long previousMillis = 0;
long interval = 1000;
int motor = 9;
unsigned long previousMillis2 = 0;
long interval2 = 1000;
void setup()
{
  pinMode(6, OUTPUT);
  digitalWrite(6, 0);

  Serial.begin(115200);
  myservo.attach(5);
  myservo2.attach(3);

  // myservo3.attach(9);
  myservo.write(serAng);
  myservo2.write(serAngy);
  while (!Serial)
    ;
  mySerial.begin(115200);
  byteTemp[0] = 100;
  byteTemp[1] = 100;
  byteTemp[2] = 0;
}
void loop()
{
  unsigned long currentMillis = millis();
  unsigned long currentMillis2 = millis();

  while (mySerial.available())
  {
    tt = mySerial.read();
    //Serial.print(tt);
    if (tt == 255)
    {
      Serial.println();
      countByte = 0;
    }
    else {
      byteTemp[countByte] = tt;
      countByte++;
      //change = true;
    }
  }

  //Serial.println(tt);
  /*Serial.print(byteTemp[0]);
    Serial.print(" ");
    Serial.print(byteTemp[1]);
    Serial.print(" ");
    Serial.println(byteTemp[2]);*/

  //////////////////// x

  if (byteTemp[0] > 100) { // plus x
    if (serAng > 40) {
      interval  = abs(200 - byteTemp[0]) + delayTime;
      //Serial.println(interval);
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        //Serial.println("AB");
        serAng -= ang;
      }
      //myservo.write(serAng);
      // Serial.println(byteTemp[0]);
    }
  }
  else if (byteTemp[0] < 100 && byteTemp[0] >= 0) {
    if (serAng < 145) {
      interval  = (abs(byteTemp[0]) + delayTime)   ;
      //Serial.println(interval);
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        //Serial.println("AB");
        serAng += ang;
        ;
      }
      //myservo.write(serAng);
      // Serial.println(byteTemp[0]);
    }
  }
  /////////////////////// y
  if (byteTemp[1] > 100) { // plus x
    if (serAngy < 140) {
      interval2  = abs(200 - byteTemp[1]) + delayTime ;
      //Serial.println(interval2);
      if (currentMillis2 - previousMillis2 >= interval2) {
        previousMillis2 = currentMillis2;
        //Serial.println("AB");
        serAngy += ang;
      }
      //myservo.write(serAng);
      // Serial.println(byteTemp[0]);
    }
  }
  else if (byteTemp[1] < 100 && byteTemp[1] >= 0) {
    if (serAngy > 50) {
      interval2  = (abs(byteTemp[1]) + 0) + delayTime   ;
      //Serial.println(interval2);
      if (currentMillis2 - previousMillis2 >= interval2) {
        previousMillis2 = currentMillis2;
        //Serial.println("AB");
        serAngy -= ang;
      }
      //myservo.write(serAng);
      // Serial.println(byteTemp[0]);
    }
  }
  //if (change) {
  myservo.write(serAng);
  myservo2.write(serAngy);
  analogWrite(6, (byteTemp[2] * 255) / 100);
  delay(6);

  /* Serial.print(" x: ");
    Serial.print(serAng);
    Serial.print(" y: ");
    Serial.println(serAngy);*/
  /* while (byteTemp[0] != 100) {
     Serial.println(byteTemp[0]);
    }*/

  /* Serial.print(" x: ");
    Serial.print(byteTemp[0]);
    Serial.print(" y: ");
    Serial.print(byteTemp[1]);
    Serial.print(" speed: ");
    Serial.println(byteTemp[2]);*/

  //change = false;

}