Added ps4 controller

include/config.h.orig

@@ -10,10 +10,17 @@
 #define EEPROM_SIZE 64
 
 //! OSCRemote
+//#define OSC_REMOTE // Enables OSC Remote
 //#define OSC_DEBUG // Enables CM prints if commented
 const unsigned int outPort = 9999;
 const unsigned int inPort = 8888;
 
+//! PS4 remote
+#define PS4_REMOTE
+#define HOST_MAC "DC:A2:66:DD:72:C0"
+
 //! Body
 //#define CTRL_INACTIVE
 #define SERVO_IIC_ADDR (0x40)
+
+const float pi = 3.1415926535897932385f;

include/ps4remote.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "IRemote.h"
+
+#include <list>
+
+class Ps4remote : public IRemote
+{
+public:
+
+    Ps4remote();
+
+    //! @param hostMAC The mac address to fake
+    //!                This would normally be the address of the PS4
+    //!                the controller is paired to
+    Ps4remote(String hostMAC);
+
+    ~Ps4remote() = default;
+
+    void init() override;
+
+    bool isConnected();
+
+    const Output& output() override;
+
+    void loop() override;
+
+    void registerCallback(void(*callback)(const Output&)) override;
+
+private:
+
+    IRemote::Output _output;
+
+    String _hostMac;
+
+    std::list<void (*)(const IRemote::Output&)> _callbacks;
+
+    void updateOutput();
+
+};

readme.md

@@ -33,3 +33,6 @@ Some libraries needs to be installed before building:
 ### OSC Touch
 [OSCTouch](https://hexler.net/products/touchosc) *by Hexler* is an awesome tool for OSC & MIDI to use for mobile robot projects.
 Install the desktop and mobile application for your platforms and sync the included layout to your mobile device.
+
+### PS4 Controller
+The PS4 controller can be connected using [this guide.](https://techtutorialsx.com/2020/02/15/esp32-connecting-a-ps4-controller/)

src/main.cpp

@@ -1,5 +1,6 @@
 #include "FlashConfig.h"
 #include "OSCRemote.h"
+#include "ps4remote.h"
 #include "Vbat.h"
 #include "WebOTA.h"
 #include "body.h"
@@ -77,8 +78,19 @@ void setup() {
     Serial.printf("- WiFi pass: %s\n", config.data().wifiPass.data());
     checkConfig();
     connectWiFi();
+    #ifdef OSC_REMOTE
     remote.reset(new OSCRemote(vbat));
     remote->registerCallback(testRemoteCallback);
+    #endif
+    #ifdef PS4_REMOTE
+    remote.reset(new Ps4remote(HOST_MAC));
+    #endif
+
+    if (remote)
+    {
+        remote->init();
+        remote->registerCallback(testRemoteCallback);
+    }
 
     sweepLeg();
 
@@ -119,7 +131,8 @@ void setup() {
 void loop() {
     // Serial.print(".");
     // sleep(1);
-    remote->loop();
+    if (remote)
+        remote->loop();
 
     body.healthCheck();
     webOTA.loop();

src/ps4remote.cpp

@@ -0,0 +1,110 @@
+#include "ps4remote.h"
+#include "config.h"
+
+#include <PS4Controller.h>
+
+#include <Arduino.h>
+
+#include <cmath>
+#include <limits>
+
+namespace {
+void printValues(IRemote::Output& o)
+{
+    Serial.printf("Roll: %f, Pitch: %d \n", 
+                  o.attitude.roll,
+                  PS4.data.analog.stick.ry);
+}
+}
+
+Ps4remote::Ps4remote()
+: _output{
+    .attitude = {},
+    .movement = {},
+    .isNew = true,
+}
+, _hostMac{}
+{
+}
+
+Ps4remote::Ps4remote(String hostMAC)
+: _output{
+    .attitude = {},
+    .movement = {},
+    .isNew = true,
+}
+, _hostMac{hostMAC}
+{
+}
+
+void Ps4remote::init()
+{
+    if (_hostMac == "")
+        return;
+
+    char c[_hostMac.length() + 1];
+    strcpy(c, _hostMac.c_str());
+
+    if (!PS4.begin(c))
+    {
+        Serial.println("Could not initialize PS4");
+        return;
+    }
+
+    PS4.setLed(50, 255, 125);
+}
+
+void Ps4remote::registerCallback(void (*callback)(const IRemote::Output&)) {
+    _callbacks.push_back(callback);
+}
+
+bool Ps4remote::isConnected()
+{
+    return PS4.isConnected();
+}
+
+const IRemote::Output& Ps4remote::output()
+{
+    return _output;
+}
+
+void Ps4remote::loop()
+{
+    updateOutput();
+    for (auto c : _callbacks)
+    {
+        c(_output);
+    }
+}
+
+void Ps4remote::updateOutput()
+{
+    if (!PS4.isConnected())
+    {
+        Serial.println("PS4 controller not connected");
+        return;
+    }
+    auto norm = [](int8_t in) -> float
+    {
+        float t = static_cast<float>(in);
+        return t / static_cast<float>(std::numeric_limits<int8_t>::max());
+    };
+
+    _output.attitude = {
+        .roll = norm(PS4.data.analog.stick.rx),
+        .pitch = norm(PS4.data.analog.stick.ry),
+        .yaw = norm(PS4.data.analog.stick.lx),
+        .elevator = norm(PS4.data.analog.stick.ly),
+    };
+    _output.isNew = true;
+
+    float vx = norm(PS4.data.analog.button.r2 - PS4.data.analog.button.l2);
+    vx = std::max(-1.0f, std::min(1.0f, vx));
+    _output.movement.vx = 
+        vx;
+    _output.movement.vy = 0.0f;
+
+    //printValues(_output);
+}
+
+