Drive robotic arm with a Gamepad from a Raspberry Pi with HTTP request and ESP8266 - Arduino Code + Server

ROT2U 6DOF HTTP server running on ESPDuino esp8266 gamepad 6 servos PCA9685 shield

/*
 * WebSocket server to drive Arduino Robotic kit (ROT2U 6DOF) with a Gamepad and a Raspberry Pi 3
 * Serveur webSocket permettant de piloter un bras robotique (kit ROT2U 6DOF) avec un Gamepad depuis un Raspberry Pi 3
 * https://projetsdiy.fr - https://diyprojects.io - dev. 2017
 * Toutes les étapes https://projetsdiy.fr/piloter-bras-robotique-wifi-gamepad-raspberry-pi-python-evdev-requete-http/
 */

#include <Wire.h>
#include <servo_PCA9685.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <ESP8266HTTPClient.h>

  
const char* ssid     = "yourssid";           
const char* password = "yourpassword";      

#define SERVO_SPEED 5 
#define MAX_SERVOS  16


//ESP8266 I2C Pins: SDA: 4 (D2), SCL: 5 (D1)
servo_PCA9685 servo = servo_PCA9685();

uint8_t servonum = 0;
WiFiClient client;
ESP8266WebServer server ( 80 );
HTTPClient http;

int posServo0 = 90; 
int posServo1 = 30; 
int posServo2 = 120;
int posServo3 = 90; 
int posServo4 = 90; 
int posServo5 = 90; 
int pinServo0 = 0;
int pinServo1 = 1;
int pinServo2 = 2;
int pinServo3 = 3;
int pinServo4 = 4;
int pinServo5 = 5;

void goHome(){
  servo.moveServo(pinServo0,posServo0);
  servo.moveServo(pinServo1,posServo1);
  servo.moveServo(pinServo2,posServo2);
  servo.moveServo(pinServo3,posServo3);
  servo.moveServo(pinServo4,posServo4);
  servo.moveServo(pinServo5,posServo5);
}

void setup() {
  Serial.begin(115200);
 
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
 
  Serial.println("WiFi connected");  
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  delay(2000);
    
  Serial.println("Start servo");
  servo.begin();
  goHome();
    
  server.on("/cmd", moveServos);
  server.begin();
}

void loop() {
  server.handleClient();
  /*while(client.available()){
    String line = client.readStringUntil('\r');
    Serial.print(line);
  }*/
}

void moveServos(){
  server.send(200, "text/plain", "Execute movement");
  for ( int i = 0 ; i < server.args(); i++ ) {
    Serial.print("Move servo "); Serial.print(server.argName(i)); Serial.print(" to "); Serial.println(server.arg(i));
  }
  String numServo = server.argName(0);
  String dirServo = server.arg(0);
  
  if ( numServo == "servo0" ) {
      if ( dirServo == "left" ) {
        if ( posServo0 == 0 ) {
          posServo0 = 0;
          Serial.print("Stop");
        } else {
          posServo0 = posServo0 - SERVO_SPEED;
          servo.moveServo(pinServo0,posServo0);
        }
      } else if ( dirServo = "right" ) {    
        if ( posServo0 == 180 ) {
          posServo0 = 180;
          Serial.print("Stop");
        } else {
          posServo0 = posServo0 + SERVO_SPEED;
          servo.moveServo(pinServo0,posServo0);
        }
      }
  } else if ( numServo == "servo1" ) {
      if ( dirServo == "up" ) {
        if ( posServo1 == 180 ) {
          posServo1 = 180;
          Serial.print("Stop");
        } else {
          posServo1 = posServo1 + SERVO_SPEED;
          servo.moveServo(pinServo1,posServo1);
        }
      } else if ( dirServo == "down" ) {
        if ( posServo1 == 0 ) {
          posServo1 = 0;
          Serial.print("Stop");
        } else {
          posServo1 = posServo1 - SERVO_SPEED;
          servo.moveServo(pinServo1,posServo1);
        }
      }
  } else if ( numServo == "servo2" ) {
      if ( dirServo == "up" ) {
        if ( posServo2 == 180 ) {
          posServo2 = 180;
          Serial.print("Stop");
        } else {
          posServo2 = posServo2 + SERVO_SPEED;
          servo.moveServo(pinServo2,posServo2);
        }
      } else if ( dirServo == "down" ) {
        if ( posServo2 == 0 ) {
          posServo2 = 0;
          Serial.print("Stop");
        } else {
          posServo2 = posServo2 - SERVO_SPEED;
          servo.moveServo(pinServo2,posServo2);
        }
      }   
  } else if ( numServo == "servo3" ) {
      if ( dirServo == "up" ) {
        if ( posServo3 == 180 ) {
          posServo3 = 180;
          Serial.print("Stop");
        } else {
          posServo3 = posServo3 + SERVO_SPEED;
          servo.moveServo(pinServo3,posServo3);
        }
      } else if ( dirServo == "down" ) {
        if ( posServo3 == 0 ) {
          posServo3 = 0;
          Serial.print("Stop");
        } else {
          posServo3 = posServo3 - SERVO_SPEED;
          servo.moveServo(pinServo3,posServo3);
        }
      }
   } else if ( numServo == "servo4" ) {
      if ( dirServo == "up" ) {
        if ( posServo4 == 180 ) {
          posServo4 = 180;
          Serial.print("Stop");
        } else {
          posServo4 = posServo4 + SERVO_SPEED;
          servo.moveServo(pinServo4,posServo4);
        }
      } else if ( dirServo == "down" ) {
        if ( posServo4 == 0 ) {
          posServo4 = 0;
          Serial.print("Stop");
        } else {
          posServo4 = posServo4 - SERVO_SPEED;
          servo.moveServo(pinServo4,posServo4);
        }
      }
  } else if ( numServo == "servo5" ) {
      if ( dirServo == "close" ) {
        if ( posServo5 == 180 ) {
          posServo5 = 180;
          Serial.print("Stop");
        } else {
          posServo5 = posServo5 + SERVO_SPEED;
          servo.moveServo(pinServo5,posServo5);
        }
      } else if ( dirServo == "open" ) {
        if ( posServo5 == 0 ) {
          posServo5 = 0;
          Serial.print("Stop");
        } else {
          posServo5 = posServo5 - SERVO_SPEED;
          servo.moveServo(pinServo5,posServo5);
        }
      }
  } else {
    Serial.print("Unknown servo!"); Serial.println(numServo);
  }
}