udp_roboremo_dc_servo_0516

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

// --------------------------------[ config ]-------------------------------------
  //--LED
    //const int ledPin = 14; //teszt jelleggel, sck led

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

  //--wifi config
    #ifndef APSSID
    #define APSSID "WEMOS"
    #define APPSK  "nincs"
    #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 UDPTestServer;
    unsigned int UDPPort = 4000; //udp szerver portja
    const int packetSize = 20; // csomag mérete
    char packetBuffer[packetSize];

  //--MOTOR shield object
    Adafruit_MotorShield AFMS = Adafruit_MotorShield();
    Adafruit_DCMotor *myMotor = AFMS.getMotor(1); // Select which 'port' M1, M2, M3 or M4. In this case, M1 Sima DC motor
    //Adafruit_DCMotor *myMotor = AFMS.getMotor(2); // Select which 'port' M1, M2, M3 or M4. In this case, M2


//----------------------------------[ SETUP ]-----------------------------------
void setup() {

    Serial.begin(115200);
//--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 5, meg min max, ezt nem értem, de ennyi volt a példában

  //--WIFI
    WiFi.softAPConfig(ip, ip, netmask);
    WiFi.softAP(ssid, password, 8); // WifiAp SSID és jelszó beállítás, csatorna, qurvára nem csinál semmit, a 20-al ezelőttit használja
    Serial.println("Indul...");  //ezt sose írja ki, csak időhúzás
    Serial.println("Wemos D1R2 UDP teszt 2019.05.15");
    Serial.println((String)"SSID: " + ssid + "  PASS: " + password);  //ennek sincs sok értelme, úgy se ez a valős
    Serial.println((String)"RoboRemo app udp adatok " + ip.toString() + ":" + UDPPort);
  //--UDP start
    UDPTestServer.begin(UDPPort);
}
//---------------------------------[ A lényeg :) ]-------------------------------
void loop() {
   handleUDPServer();
   delay(1);
}
//---------------------------------[ A program... ]------------------------------
void handleUDPServer() {
  int cb = UDPTestServer.parsePacket(); //A cb az üzenet mérete. Ha van üzenet, akkor megy tovább....
    if (cb) {
       int len = UDPTestServer.read(packetBuffer, packetSize); //len változó a csomag hossza
          if (len > 0) {
            packetBuffer[len-1] = '\0'; //betesz az utolsó karakternek egy lezárást
              // Serial.println(len); len változó hossza
              // Serial.println(packetBuffer);  pocketBuffer a csomag tartalma
            int iValue = atoi(packetBuffer + 3);
            Serial.println(iValue);


             if (iValue <= 2000 && iValue >= 1000)
            {
              myservo.write(iValue);

            } else
              {

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

              }
          }

        }

 }