esp8266 servo controlled turret with joystick

turret.ino

/*
 * mcu: ESP 8266 Boards 3.0 - LOLIN Wemos (D1 R2 & mini)
 * 
 * UHS modification: https://www.hackster.io/usini/plug-any-usb-device-on-an-esp8266-e0ca8a
 * 
 * pinout:
 * D3 - UHS.SS
 * D7 - UHS.MISO
 * D6 - UHS.MOSI
 * D5 - UHS.SCK
 * RST - UHS.RST
 * GND - UHS.GND
 * 5V - UHS.RAW (cut trace!)
 * 3.3V - UHS.VCC
 * 
 * D0 - servo.x
 * D2 - servo.y
 * D4 - servo.z
 */
#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include <ESP8266WebServer.h>

#include <Servo.h>
#include <usbhub.h>
#include <SPI.h>

#include <hiduniversal.h>
#include <usbhid.h>

int joyX = 0x200, joyY = 0x200, joyPackets = 0;
int joyZ = 0x80, joyP = 0x80, joyB = 0, joyD = 0;
long joyLast = -1;
bool joyReverseX = true;
bool joyReverseY = false;
bool continuousX = true;

int usbBad = 0;
long lastLoop = 0;
float speedX = 0.8f;
float speedY = 1.2f;
float xMin = -100.0f;
float xMax = +100.0f;
float yMin = -100.0f;
float yMax = +100.0f;
float zMin = -100.0f;
float zMax = +100.0f;
const int deadRange = 50;
float lastServoX = 90;
float lastServoY = 90;
float lastServoZ = 90;
float maxTravelX = 2.0f;
float maxTravelY = 0.6f;
float maxTravelZ = 0.6f;

int servoXMin = 0;
int servoXMax = 180;
int servoYMin = 90+35;
int servoYMax = 90-15;
int servoZMin = 90-35;
int servoZMax = 90+15;

int servoSent = 0;

// ang to acc: (ang-90)/90*100
//           90+(15-35)=80
//  90-35    90    90+15
//   |-------- ----|
//       |---- --------|

float centery = -40;
float accx = 0;
float accy = centery; 

#define RPT_GEMEPAD_LEN    5
class JoystickReportParser : public HIDReportParser {
        uint8_t oldPad[RPT_GEMEPAD_LEN];
        uint8_t oldHat;
        uint16_t oldButtons;
        int prevX{512}, prevY{512}, prevZ{128}, prevP{128};
        int prevBut{0};
public:
        JoystickReportParser();
        virtual void Parse(USBHID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf);
};

JoystickReportParser::JoystickReportParser() :
oldHat(0xDE),
oldButtons(0) {
        for (uint8_t i = 0; i < RPT_GEMEPAD_LEN; i++)
                oldPad[i] = 0xD;
}

void JoystickReportParser::Parse(USBHID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
        bool match = true;
    joyLast = millis();
    joyPackets++;

    int axisX = buf[0] | ((buf[1] & 3) << 8); // 10 bit
    int axisY = buf[1] >> 2 | ((buf[2] & 15) << 6); // 10 bit
    int axisZ = buf[3];
    int axisP = buf[5];
    int buttons = buf[4] | (buf[6]<<8);
    int dpad = buf[2] >> 4;

    if (prevX != axisX || prevY != axisY)
    {
      prevX = axisX;
      prevY = axisY;
      
      // user code:
      joyX = joyReverseX ? 0x3ff - axisX : axisX;
      joyY = joyReverseY ? 0x3ff - axisY : axisY;      
    }
    joyZ = axisZ;
    joyP = axisP;
    joyB = buttons;
    joyD = dpad;
    if (prevBut != buttons)
    {
      prevBut = buttons;
      
      // user code:
      accx = 0;
      accy = centery;
    }
// joy: len=7, buf=00 fe 87 80 00 91 00 
/*                                
 *  xxxxxxxx yyyyyyxx DDDDyyyy zzzzzzzz 87654321 pppppppp ....cba9
 *
 *  D-PAD: up=d[2]&0x80==0  (default 8) down=d[2]&0x80 ==4 left=d[2]&0x80 ==6 right=d[2]&0x80 ==2 
 *  k3=d[4]&0x04 k4=d[4]&0x08 k5=k[4]&0x10 k6=k[4]&0x20
 *  DPAD: 8 - nic, 1 = doprava hore, 2= doprava, 3=doprava dole, 4=dole, 5=dolava dole, 6=dolava
 */
 /*
  Serial.print("joy: len=");
  Serial.print(len);
  Serial.print(", buf=");
  for (int i=0; i<len; i++)
  {
    char temp[8];
    sprintf(temp, "%02x ", buf[i]);
    Serial.print(temp);
  }
  Serial.print("\n");
  */
}

void converge(float& last, float cur, float travel, void (*update)(int))
{
  float diff = cur - last;
  if (diff == 0)
   return;
  int prevLast = (int)last;
  diff = max(-travel, min(diff, travel));
  last += diff;
  if ((int)last != prevLast)
    update((int)last);
}

ESP8266WebServer server(80);
char bufMeno[64];
char bufHeslo[64];
Servo servo1, servo2, servo3;
USB Usb;
USBHub Hub(&Usb);
HIDUniversal Hid(&Usb);
JoystickReportParser Joy;//(&JoyEvents);
int usbInit = 0;
int usbDevices = -1;
bool demo = false;

void setup() {
  Serial.begin(115200);
  /*
  WiFi.begin("***", "***");

  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  
  Serial.print("\n");
  Serial.print("Connected!\n");
  Serial.print("IP address: ");
  Serial.print(WiFi.localIP());
  Serial.print("\n");
  */
  
  WiFi.softAP("valky.eu-turret", "12345678");
  // 192.168.4.1
  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(IP);

  server.on("/", []() {
    Serial.print("Return homepage\n");
    server.send(200, "text/html", R"(
    <html>
  <head>
      <title>Turret config</title>
  </head>
<body>
<h1>turret control by valky.eu, v3.0</h1>
<!--
<form action="/config">
Meno: <input type=text name="meno"><br>
Heslo: <input type=password name="heslo"><br>
<input type="submit" value="Prihlasit">
</form>
-->
<div class="slidecontainer">
  Joystick:<br>
  <input type="range" min="0" max="1023" value="512" id="joyx" style="width:800px"><br>
  <input type="range" min="0" max="1023" value="512" id="joyy" style="width:800px"><br>
  <br>
  Accumulator:<br>
  <input type="range" min="-100" max="100" value="0" id="posx" style="width:800px" disabled><br>
  <input type="range" min="-100" max="100" value="0" id="posy" style="width:800px" disabled><br>
  <br>
  Servo:<br>
  <input type="range" min="1" max="180" value="90" id="servo1" style="width:800px"><br>
  <input type="range" min="1" max="180" value="90" id="servo2" style="width:800px"><br>
  <input type="range" min="1" max="180" value="90" id="servo3" style="width:800px"><br>
  <a href="/home">Go home</a> <a href="/reset">Reset UHS</a> <a href="/init">Init UHS</a>
  <a href="/probe">Probe UHS</a>
</div>

<div id="status">status</div>
<!--
  <center style="font-size:50px;">
      <a href="/3">3</a>
      <a href="/4">4</a>
      <a href="/5">5</a>
      <a href="/6">6</a>
      <a href="/7">7</a>
  </center> 
  -->
  <script>
  var lastServoX = -1, lastServoY = -1, lastServoZ = -1;
  var lastJoyX = -1, lastJoyY = -1;
setInterval(()=>{
  fetch("/status").then(x=>x.json()).then(x=>{
    if (lastJoyX != x.joyx)
    {
      lastJoyX = x.joyx;
      document.querySelector("#joyx").value = x.joyx;
    }
    if (lastJoyY != x.joyy)
    {
      lastJoyY = x.joyy;
      document.querySelector("#joyy").value = x.joyy;
    }
    document.querySelector("#posx").value = x.x;
    document.querySelector("#posy").value = x.y;
    if (lastServoX != x.servox)
    {
      lastServoX = x.servox;
      document.querySelector("#servo1").value = x.servox;
    }
    if (lastServoY != x.servoy)
    {
      lastServoY = x.servoy;
      document.querySelector("#servo2").value = x.servoy;
    }
    if (lastServoZ != x.servoz)
    {
      lastServoZ = x.servoz;
      document.querySelector("#servo3").value = x.servoz;
    }
    document.querySelector("#status").innerHTML = JSON.stringify(x);
  })
    
}, 100);
var timeout;

  document.querySelector("#servo1").addEventListener("change", (x, y)=>{
    fetch("/set?a="+x.target.value)
  });
  document.querySelector("#servo2").addEventListener("change", (x, y)=>{
    fetch("/set?b="+x.target.value)
  });
  document.querySelector("#servo3").addEventListener("change", (x, y)=>{
    fetch("/set?c="+x.target.value)
  });
  document.querySelector("#joyx").addEventListener("change", (x, y)=>{
    fetch("/set?x="+x.target.value)
  });
  document.querySelector("#joyy").addEventListener("change", (x, y)=>{
    fetch("/set?y="+x.target.value)
  });

</script>

</body>
</html>
    )");
  });

  server.on("/status", []() {
    char temp[256];
    sprintf(temp, "{\"millis\":%d,\"usb\":%d,"
    "\"ustate\":%d,\"vbus\":%d,\"demo\":%d"
    ",\"joyx\":%d,\"joyy\":%d,\"joyz\":%d,\"joyp\":%d,\"joybtn\":%d,\"joydpad\":%d,"
    "\"joyc\":%d,\"x\":%d,\"y\":%d,\"servox\":%d,\"servoy\":%d,\"servoz\":%d"
    ",\"servoSent\":%d}", millis(), 
      usbInit, Usb.getUsbTaskState(), Usb.getVbusState(), demo,
      joyX, joyY, joyZ, joyP, joyB, joyD,
      joyPackets, (int)accx, (int)accy, (int)lastServoX, (int)lastServoY, (int)lastServoZ,
      servoSent);
    server.send(302, "application/json", temp);
  });
  server.on("/config", []() {
    String meno = server.arg("meno");
    String heslo = server.arg("heslo");

    strcpy(bufMeno, meno.c_str());
    strcpy(bufHeslo, heslo.c_str());
    server.sendHeader("Location", String("/"), true);
    server.send(302, "text/plain", "");
  });
  server.on("/demo1", []() {
    demo = true;
    server.send(200, "application/json", "{}");
  });
  server.on("/demo0", []() {
    demo = false;
    server.send(200, "application/json", "{}");
  });
  server.on("/home", []() {
     accx = 0;
     accy = centery;
    server.sendHeader("Location", String("/"), true);
    server.send(302, "text/plain", "");
     
  });
  server.on("/reset", []() {
    digitalWrite(D1, LOW);
    delay(20);
    digitalWrite(D1, HIGH);
    delay(100);

    server.sendHeader("Location", String("/"), true);
    server.send(302, "text/plain", "");  
  });
  server.on("/init", []() {
    usbInit = Usb.Init();  
    server.sendHeader("Location", String("/"), true);
    server.send(302, "text/plain", "");  
  });
  server.on("/probe", []() {
    Usb.busprobe();
    server.sendHeader("Location", String("/"), true);
    server.send(302, "text/plain", "");  
  });
  
  server.on("/set", []() {
    String a = server.arg("a");
    String b = server.arg("b");
    String c = server.arg("c");
    String x = server.arg("x");
    String y = server.arg("y");
    String ax = server.arg("accx");
    String ay = server.arg("accy");
    if (a != "")
      servo1.write(a.toInt());
    if (b != "")
      servo2.write(b.toInt());
    if (c != "")
      servo3.write(c.toInt());
    if (x != "")
      joyX = x.toInt();
    if (y != "")
      joyY = y.toInt();
    if (ax != "")
      accx = ax.toInt();
    if (ay != "")
      accy = ay.toInt();

    server.send(200, "application/json", "{}");
  });
  server.begin();

  servo1.attach(D0, 500, 2500);
  servo2.attach(D2, 500, 2500);
  servo3.attach(D4, 500, 2500);

  pinMode(D1, OUTPUT);
  digitalWrite(D1, LOW);
  delay(20);
  digitalWrite(D1, HIGH);
  delay( 100 );

  usbInit = Usb.Init();
  if (!Hid.SetReportParser(0, &Joy))
          ErrorMessage<uint8_t > (PSTR("SetReportParser"), 1);
  
  delay( 200 );
}

void loop()
{
  long now = millis();
  
  static long lastTest = -1;
  if (now - lastTest > 3000)
  {
    lastTest = now;
    if (joyLast == -1)
    {
      Serial.print("forced init\n");
      usbInit = Usb.Init();
    } else
    //if (Usb.getVbusState()==0)
    if (now - joyLast > 3000)
    {
      Usb.busprobe();
      Serial.print("forced probe, vbus=");
      Serial.print(Usb.getVbusState());
      Serial.print("\n");
    }
  }
  
  static long l0 = 0;
  if (lastLoop-now > 100)
  {
    Serial.print("lastloop reset\n");
    lastLoop = now;
  }
  int loops = 0;
  while (now - lastLoop > 5)
  {
    lastLoop += 5;
    if (continuousX)
    {
      
      if (joyX < 0x200 - deadRange)
        accx = (joyX - (0x200 - deadRange))*80.0f/512.0f;
      else if (joyX > 0x200 + deadRange)
        accx = (joyX - (0x200 + deadRange))*80.0f/512.0f;
      else
        accx = 0;
    } else {
      if (joyX < 0x200 - deadRange)
        accx += (joyX - (0x200 - deadRange))/512.0f*speedX;
      if (joyX > 0x200 + deadRange)
        accx += (joyX - (0x200 + deadRange))/512.0f*speedX;              
    }
      
    if (joyY < 0x200 - deadRange)
      accy += (joyY - (0x200 - deadRange))/512.0f*speedY;
    if (joyY > 0x200 + deadRange)
      accy += (joyY - (0x200 + deadRange))/512.0f*speedY;
    //if (joyY < 0x200 - deadRange)
//      accz += (joyY - (0x200 - deadRange))/512.0f*speedY;
    //if (joyY > 0x200 + deadRange)
//      accz += (joyY - (0x200 + deadRange))/512.0f*speedY;
    loops++;
  }
  accx = max(xMin, min(accx, xMax));
  accy = max(yMin, min(accy, yMax));
  //accz = max(zMin, min(accy, zMax));
  float servoX = servoXMin + (accx - xMin) * (float)(servoXMax-servoXMin) / (xMax - xMin);
  float servoY = servoYMin + (accy - yMin) * (float)(servoYMax-servoYMin) / (yMax - yMin);
  float servoZ = servoZMin + (accy - zMin) * (float)(servoZMax-servoZMin) / (zMax - zMin);
  converge(lastServoX, servoX, maxTravelX*loops, [](int v){ servoSent++; servo1.write(v); });
  converge(lastServoY, servoY, maxTravelY*loops, [](int v){ servoSent++; servo2.write(v); });
  converge(lastServoZ, servoZ, maxTravelZ*loops, [](int v){ servoSent++; servo3.write(v); });
  
  Usb.Task();
  server.handleClient();
  if (demo)
  {
    long phase = ((now>>2) & 1023);
    if (phase > 512)
      phase = 1023-phase;
    servo1.write(phase*180/512);
  
    phase = (((now>>2)+300) & 1023);
    if (phase > 512)
      phase = 1023-phase;
    servo2.write(phase*180/512);
  
    phase = (((now>>2)+600) & 1023);
    if (phase > 512)
      phase = 1023-phase;
    servo3.write(phase*180/512);
  }
}