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

ROT2U 6DOF WebSocket server running on ESPDuino or 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-websocket/
 */
#include <Wire.h>
#include <servo_PCA9685.h>
#include <ESP8266WiFi.h>
#include <WebSocketsServer.h>

WebSocketsServer webSocket = WebSocketsServer(81);
const char* ssid     = "yourssid";
const char* password = "yourpassword";

servo_PCA9685 servo = servo_PCA9685();

#define SERVO_SPEED 1 
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 webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t lenght) {
    Serial.printf("[%u] get Message: %s\r\n", num, payload);
    switch(type) {
        case WStype_DISCONNECTED:      
            break;
        case WStype_CONNECTED: 
            {
              IPAddress ip = webSocket.remoteIP(num);
              Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\r\n", num, ip[0], ip[1], ip[2], ip[3], payload);    
            }
            break;
        
        case WStype_TEXT:
            {
              //Serial.printf("[%u] get Text: %s\r\n", num, payload);
              String _payload = String((char *) &payload[0]);
              Serial.println(_payload);
              
              String numServo=(_payload.substring(0,6));
              String dirServo=(_payload.substring(_payload.indexOf(":"),_payload.length()));
              delay(10);
              //Serial.println(numServo);
              moveServos(numServo, dirServo); 
            }   
            break;     
             
        case WStype_BIN:
            {
              hexdump(payload, lenght);
            }
            // echo data back to browser
            webSocket.sendBIN(num, payload, lenght);
            break;
            
    }
}


void setup() {
    Serial.begin(115200);
    WiFi.begin(ssid, password);

    while(WiFi.status() != WL_CONNECTED) {
       Serial.print(".");
       delay(200);
    }
    
   Serial.println("");
   Serial.println("WiFi connected");  
   Serial.println("IP address: ");
   Serial.println(WiFi.localIP());

   delay(500);  
   
   Serial.println("Start servo and go to Home position");
   servo.begin();
   //goHome();

   Serial.println("Start Websocket Server");
   webSocket.begin();
   webSocket.onEvent(webSocketEvent);
   
}

void loop() {
    webSocket.loop();
}

void moveServos(String numServo, String dirServo){
  Serial.print("Move "); Serial.print(numServo); Serial.print(" to "); Serial.print(dirServo); Serial.print(" current angle ");
  
  if ( numServo.indexOf("0") > 0 ) {
      if ( dirServo.indexOf("left") > 0 ) {
        if ( posServo0 == 0 ) {
          posServo0 = 0;
          Serial.print("Stop");
        } else {
          posServo0 = posServo0 - SERVO_SPEED;
          servo.moveServo(pinServo0,posServo0);
        }
      } else if ( dirServo.indexOf("right") > 0 ) {    
        if ( posServo0 == 180 ) {
          posServo0 = 180;
          Serial.print("Stop");
        } else {
          posServo0 = posServo0 + SERVO_SPEED;
          servo.moveServo(pinServo0,posServo0);
        }
      }
      Serial.println(posServo0);
  } else if ( numServo.indexOf("1") > 0 ) {
      if ( dirServo.indexOf("up") > 0 ) {
        if ( posServo1 == 180 ) {
          posServo1 = 180;
          Serial.print("Stop");
        } else {
          posServo1 = posServo1 + SERVO_SPEED;
          servo.moveServo(pinServo1,posServo1);
        }
      } else if ( dirServo.indexOf("down") > 0 ) {
        if ( posServo1 == 0 ) {
          posServo1 = 0;
          Serial.print("Stop");
        } else {
          posServo1 = posServo1 - SERVO_SPEED;
          servo.moveServo(pinServo1,posServo1);
        }
      }
  } else if ( numServo.indexOf("2") > 0 ) {
      if ( dirServo.indexOf("up") > 0 ) {
        if ( posServo2 == 180 ) {
          posServo2 = 180;
          Serial.print("Stop");
        } else {
          posServo2 = posServo2 + SERVO_SPEED;
          servo.moveServo(pinServo2,posServo2);
        }
      } else if ( dirServo.indexOf("down") > 0 ) {
        if ( posServo2 == 0 ) {
          posServo2 = 0;
          Serial.print("Stop");
        } else {
          posServo2 = posServo2 - SERVO_SPEED;
          servo.moveServo(pinServo2,posServo2);
        }
      }   
  } else if ( numServo.indexOf("3") > 0 ) {
      if ( dirServo.indexOf("up") > 0 ) {
        if ( posServo3 == 180 ) {
          posServo3 = 180;
          Serial.print("Stop");
        } else {
          posServo3 = posServo3 + SERVO_SPEED;
          servo.moveServo(pinServo3,posServo3);
        }
      } else if ( dirServo.indexOf("down") > 0 ) {
        if ( posServo3 == 0 ) {
          posServo3 = 0;
          Serial.print("Stop");
        } else {
          posServo3 = posServo3 - SERVO_SPEED;
          servo.moveServo(pinServo3,posServo3);
        }
      }
   } else if ( numServo.indexOf("4") > 0 ) {
      if ( dirServo.indexOf("up") > 0 ) {
        if ( posServo4 == 180 ) {
          posServo4 = 180;
          Serial.print("Stop");
        } else {
          posServo4 = posServo4 + SERVO_SPEED;
          servo.moveServo(pinServo4,posServo4);
        }
      } else if ( dirServo.indexOf("down") > 0 ) {
        if ( posServo4 == 0 ) {
          posServo4 = 0;
          Serial.print("Stop");
        } else {
          posServo4 = posServo4 - SERVO_SPEED;
          servo.moveServo(pinServo4,posServo4);
        }
      }
  } else if ( numServo.indexOf("5") > 0 ) {
      if ( dirServo.indexOf("close") > 0 ) {
        if ( posServo5 == 180 ) {
          posServo5 = 180;
          Serial.print("Stop");
        } else {
          posServo5 = posServo5 + SERVO_SPEED;
          servo.moveServo(pinServo5,posServo5);
        }
      } else if ( dirServo.indexOf("open") > 0 ) {
        if ( posServo5 == 0 ) {
          posServo5 = 0;
          Serial.print("Stop");
        } else {
          posServo5 = posServo5 - SERVO_SPEED;
          servo.moveServo(pinServo5,posServo5);
        }
      }
  } else {
    Serial.print("Servo inconnu !"); Serial.println(numServo);
  }
}