Wemos rc car alfa

#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <Adafruit_MotorShield.h>
#include <Servo.h>

unsigned long StartMillis;
unsigned long AktualisMillis;
const unsigned long szunet = 500;
const byte bal_index = 0;
const byte jobb_index = 15;
int UdpCsomag;
int index_seged;  // ha 0 akkor off, 1-es balra, 2-es jobbra

//--wifi config
  #ifndef APSSID
  #define APSSID "D1R2"
  #define APPSK  "nincsnincs"
  #endif
  const char *ssid = APSSID;
  const char *password = APPSK;
  IPAddress ip(192, 168, 1, 10); // Wemos ip címe
  IPAddress netmask(255, 255, 255, 0); //netmask

//--udp config
  WiFiUDP UdpServer;
  unsigned int UDPPort = 4000; //udp szerver portja
  const int packetSize = 20; // csomag mérete
  char packetBuffer[packetSize];

//---SERVO motor megadása
   Servo myservo;

//--MOTOR shield object
   Adafruit_MotorShield AFMS = Adafruit_MotorShield();
   Adafruit_DCMotor *myMotor = AFMS.getMotor(1);

void setup()
{
  Serial.begin(115200);  //start Serial
  pinMode(bal_index, OUTPUT);
  pinMode(jobb_index, OUTPUT);
  StartMillis = millis();  //initial start time

  //--WIFI
  WiFi.softAPConfig(ip, ip, netmask);
  WiFi.softAP(ssid, password, 8);
  Serial.println("Indul...");
  Serial.println("Wemos D1R2 UDP teszt 2019.05.22");
  Serial.println((String)"SSID: " + ssid + "  PASS: " + password);
  Serial.println((String)"RoboRemo app udp adatok " + ip.toString() + ":" + UDPPort);

  //--UDP start
  UdpServer.begin(UDPPort);

  //--MOTOR
  AFMS.begin();  // create with the default frequency 1.6KHz  AFMS.begin(1000); OR with a different frequency, say 1KHz
  myMotor->setSpeed(150); // Set the speed to start, from 0 (off) to 255 (max speed)
  //myMotor->run(FORWARD);
  myMotor->run(RELEASE); // turn on motor

  //--Servo motor GPIO
    myservo.attach(13,600,2300);  //GPIO 13, meg min max, ezt nem értem, de ennyi volt a példában
}

void loop()

{
  AktualisMillis = millis();
  int cb = UdpServer.parsePacket();
  if (cb) {

    int len = UdpServer.read(packetBuffer, packetSize); //len változó a csomag hossza
    if (len > 0) {

      packetBuffer[len - 1] = '\0'; //betesz az utolsó karakternek egy lezárást

      UdpCsomag = atoi(packetBuffer + 3);  //integert csinálok a packetbuffer tartalmából
      Serial.println("Van udp kapcsolat!");
      Serial.println(UdpCsomag);

      if (UdpCsomag == 501)
      {
        index_seged = 1;
      }
      else if (UdpCsomag == 502)
      {
        index_seged = 2;
      }
      else if (UdpCsomag == 503)
      {
        index_seged = 3;
      }
      else if (UdpCsomag == 500)
      {
        index_seged = 0;
      }

      if (index_seged == 0)
      {
        digitalWrite(bal_index, LOW);
        digitalWrite(jobb_index, LOW);
      }
      else if (index_seged == 1)
      {
        digitalWrite(jobb_index, LOW);
        if (AktualisMillis - StartMillis >= szunet)
        {
          digitalWrite(bal_index, !digitalRead(bal_index));
          StartMillis = AktualisMillis;
        }
      }
      else if (index_seged == 2)
      {
        digitalWrite(bal_index, LOW);
        if (AktualisMillis - StartMillis >= szunet)
        {
          digitalWrite(jobb_index, !digitalRead(jobb_index));
          StartMillis = AktualisMillis;
        }
      }
      else if (index_seged == 3)
      {
        if (AktualisMillis - StartMillis >= szunet)
        {
          digitalWrite(jobb_index, !digitalRead(jobb_index));
          digitalWrite(bal_index, !digitalRead(bal_index));
          StartMillis = AktualisMillis;
        }
      }

  if (UdpCsomag <= 2300 && UdpCsomag >= 600)
            {

              myservo.write(UdpCsomag);


            } else
              {

                if (UdpCsomag >= 0 && UdpCsomag < 200)
                  {
                    myMotor->run(FORWARD);
                    myMotor->setSpeed(UdpCsomag);

                  }
                else if (UdpCsomag < 0 && UdpCsomag > -200)
                  {
                    myMotor->run(BACKWARD);
                    myMotor->setSpeed(UdpCsomag*-1);

                  }

              }

    }

  }
}