OSC Remote

.vscode/settings.json

@@ -6,6 +6,17 @@
         "unordered_map": "cpp",
         "unordered_set": "cpp",
         "vector": "cpp",
-        "initializer_list": "cpp"
+        "initializer_list": "cpp",
+        "*.tcc": "cpp",
+        "algorithm": "cpp",
+        "memory": "cpp",
+        "random": "cpp",
+        "fstream": "cpp",
+        "iosfwd": "cpp",
+        "istream": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "streambuf": "cpp",
+        "utility": "cpp"
     }
 }

include/IRemote.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <Arduino.h>
+
+class IRemote
+{
+public:
+
+  //! @brief Attitude is the "pose" of the Hexapod
+  //!        without movement
+  //!
+  //! @arg roll/pitch   Tilt it around.    Deg [-45, 45]
+  //! @arg yaw          Twist it.          Deg [-45, 45]
+  //! @arg elevator     Lower or raise it. m [0, 0.1]
+  typedef struct {
+    float roll;
+    float pitch;
+    float yaw;
+    float elevator;
+  } Attitude;
+
+  //! @brief Movement consists of the target velocity for
+  //!
+  //! @arg vx Vel back/forward. m [-?, ?]
+  //! @arg vy Vel left/right.   m [-?, ?]
+  //! @arg w  Rotational speed. deg/s [-20, 20]
+  typedef struct {
+    float vx;
+    float vy;
+    float w;
+  } Movement;
+
+  typedef struct {
+    Attitude attitude;
+    Movement movement;
+    bool isNew;
+  } Output;
+
+  virtual const Output& output() = 0;
+
+  virtual void loop() = 0;
+
+  virtual void registerCallback(void (*callback)(const Output&)) = 0;
+};

include/OSCRemote.h

@@ -0,0 +1,34 @@
+#pragma once
+#include "IRemote.h"
+
+#include "Vbat.h"
+
+#include <WiFiUdp.h>
+#include <OSCMessage.h>
+#include <OSCBundle.h>
+#include <OSCData.h>
+
+#include <list>
+
+class OSCRemote : public IRemote
+{
+public:
+  //! Needs to be called in "Setup"
+  OSCRemote(const Vbat&);
+
+  void loop();
+
+  const IRemote::Output& output() override;
+
+  void registerCallback(void (*callback)(const IRemote::Output&)) override;
+
+private:
+  //! Place this after WiFi.begin() in main
+  void init();
+  
+  IRemote::Output _output;
+
+  std::list<void(*)(const IRemote::Output&)> _callbacks;
+
+  const Vbat& _vbat;
+};

include/Vbat.h

@@ -0,0 +1,18 @@
+#pragma once
+
+
+//! @todo:
+//! * Actual reading of battery
+//! * Some simple calibration method e.g via terminal
+//! * Callback registry for cutoff action
+
+//! @brief Keep track of the battery status
+class Vbat {
+public:
+  Vbat();
+
+  float getCurrentVoltage() const;
+
+  //! @return Charge in %
+  float getCurrentCharge() const;
+};

include/body.h

@@ -25,6 +25,7 @@ private:
   struct JointInfo {
     uint8_t index;
     uint8_t trim;
+    double center; 
     double pos;
   };
 

src/OSCRemote.cpp

@@ -0,0 +1,181 @@
+#include "OSCRemote.h"
+
+//! --- Start of onfiguration ---
+//#define DEBUG // Enables CM prints if commented
+
+const unsigned int outPort = 9999;   
+const unsigned int inPort = 8888;
+//! --- End of configuration  ---
+
+IRemote::Output* pOutput;
+
+namespace {
+
+  WiFiUDP* pUdp;
+  IPAddress remoteIP;
+
+  // Scaling
+  //! @todo This is just temporary placement
+  const float rollFactor = 45;
+  const float pitchFactor = 45;
+  const float yawFactor = 45;
+  const float elevatorFactor = 0.1;
+  const float vxFactor = 0.1;
+  const float vyFactor = 0.1;
+  const float wFactor = 10;
+
+  void remoteMsgParser(OSCMessage &msg, int offset)
+  {
+    char topic[20];
+    msg.getAddress(topic);
+    Serial.println(topic);
+
+    if (!strcmp(topic, "/att/xy"))
+    {
+      pOutput->attitude.pitch = msg.getFloat(0)*pitchFactor; 
+      pOutput->attitude.roll = msg.getFloat(1)*rollFactor;
+    }
+    else if (!strcmp(topic, "/att/z"))
+    {
+      pOutput->attitude.yaw = msg.getFloat(0)*yawFactor;
+    } 
+    else if (!strcmp(topic, "/att/e"))
+    {
+      pOutput->attitude.elevator = msg.getFloat(0)*elevatorFactor; 
+    }
+    else if (!strcmp(topic, "/mov/xy"))
+    {
+      pOutput->movement.vy = msg.getFloat(0)*vxFactor; 
+      pOutput->movement.vx = msg.getFloat(1)*vyFactor;
+    }
+    else if (!strcmp(topic, "/mov/z"))
+    {
+      pOutput->movement.w = msg.getFloat(0)*wFactor;
+    }
+    else
+    {
+      Serial.println("OSC Unkown topic");
+    }
+  }
+
+  void remoteMsgParser(OSCMessage &msg)
+  {
+    remoteMsgParser(msg, 0);
+  }
+
+  String getOSCErrorName(OSCErrorCode error) 
+  {
+    switch(error) 
+    {
+      case OSC_OK: 
+        return String("OSC_OK");
+      case BUFFER_FULL: 
+        return String("BUFFER_FULL");
+      case INVALID_OSC: 
+        return String("INVALID_OSC");
+      case ALLOCFAILED: 
+        return String("ALLOCFAILED");
+      case INDEX_OUT_OF_BOUNDS: 
+        return String("INDEX_OUT_OF_BOUNDS");
+      default:
+        return String("UNKOWN ERROR");
+    }
+  }
+
+  template<class T>
+  void msgOut(String topic, T data)
+  {
+    OSCMessage msg(topic.c_str());
+    msg.add(data);
+    pUdp->beginPacket(remoteIP, outPort);
+    msg.send(*pUdp); // send the bytes
+    pUdp->endPacket(); // mark the end of the OSC Packet
+    msg.empty(); // free space occupied by message
+  }
+
+  bool msgReceive()
+  {
+    OSCBundle msg;
+
+    int size = pUdp->parsePacket();
+    remoteIP = pUdp->remoteIP();
+    if (size)
+    {
+      while (size--) { msg.fill(pUdp->read()); }
+    
+      #ifdef DEBUG
+      Serial.printf("OSC packet from: %s\n", 
+                    remoteIP.toString().c_str());
+      #endif
+      
+      if (!msg.hasError())
+      {
+        msg.dispatch("/att/xy", remoteMsgParser);
+        msg.dispatch("/att/e", remoteMsgParser);
+        msg.dispatch("/att/z", remoteMsgParser);
+
+        msg.dispatch("/mov/xy", remoteMsgParser);
+        msg.dispatch("/mov/z", remoteMsgParser);
+        return true;
+      }
+      else
+      {
+        #ifdef DEBUG
+        Serial.printf("error: %s\n", 
+                      getOSCErrorName(msg.getError()).c_str());
+        #endif
+      }
+    }
+    return false;
+  }
+}
+
+OSCRemote::OSCRemote(const Vbat& vbat)
+: _vbat(vbat)
+{
+  init();
+}
+
+void OSCRemote::init()
+{
+  pOutput = &_output;
+  pUdp = new WiFiUDP();
+  pUdp->begin(inPort);
+}
+
+void OSCRemote::loop()
+{
+  if (msgReceive())
+  {
+    for (auto c : _callbacks)
+    {
+      c(_output);
+    }
+  }
+
+  unsigned long t = millis();
+  static unsigned long tTrigger = t;
+  const unsigned long updateInterval = 1000;
+  if (t >= tTrigger)
+  {
+    // TODO: convert to string and append %
+    char voltage[5]; // "100%\n"
+    dtostrf(_vbat.getCurrentCharge(),
+            3, // lenght [0, 100]
+            0, // precision 
+            voltage);
+    strcat(voltage, "%");
+    msgOut("/label_bat", voltage); 
+    tTrigger += updateInterval;
+  }
+}
+
+const IRemote::Output& OSCRemote::output()
+{
+  return _output;
+}
+
+void OSCRemote::registerCallback(void (*callback)(const IRemote::Output&))
+{
+  _callbacks.push_back(callback);
+}

src/Vbat.cpp

@@ -0,0 +1,15 @@
+#include "Vbat.h"
+
+Vbat::Vbat()
+{
+}
+
+float Vbat::getCurrentVoltage() const
+{
+  return 4.12;
+}
+
+float Vbat::getCurrentCharge() const
+{
+  return 87.0;
+}

src/body.cpp

@@ -5,6 +5,8 @@
 #include <Adafruit_PWMServoDriver.h>
 #include <SPI.h>
 
+#define CTRL_INACTIVE
+
 
 #define SERVO_IIC_ADDR  (0x40)
 
@@ -23,13 +25,16 @@ Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver(0x40);
 unsigned long freqWatchDog = 0;
 unsigned long SuppressScamperUntil = 0;  // if we had to wake up the servos, suppress the power hunger scamper mode for a while
 
-inline void resetServoDriver() {
+void resetServoDriver() {
+  #ifndef CTRL_INACTIVE
   servoDriver.begin(); 
   servoDriver.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~60 Hz updates
+  #endif
 }
 
-inline void checkForServoSleep() {
+void checkForServoSleep() {
 
+  #ifndef CTRL_INACTIVE
   if (millis() > freqWatchDog) 
   {
 
@@ -49,6 +54,7 @@ inline void checkForServoSleep() {
     }
     freqWatchDog = millis() + 100;
   }
+  #endif
 }
 
 double modifiedMap(double x, double in_min, double in_max, double out_min, double out_max)
@@ -78,8 +84,8 @@ void Body::healthCheck()
 }
 
 Leg::Leg(uint8_t hipp, uint8_t knee)
-: _joints({{Joint::Hipp, {.index=hipp, .trim=0, .pos=0}},
-           {Joint::Knee, {.index=knee, .trim=0, .pos=0}}})
+: _joints({{Joint::Hipp, {.index=hipp, .trim=0, .center=0.0, .pos=0}},
+           {Joint::Knee, {.index=knee, .trim=0, .center=45.0, .pos=0}}})
 {
 }
 
@@ -92,14 +98,15 @@ void Leg::setPos(Joint j, double angle)
 {
   //! @todo Trying to evaluate if one of these has better performance on the servo
 
+  #ifndef CTRL_INACTIVE
   auto angleToPulse = [](double a)
-  { return modifiedMap(a, 0.0, 180.0, SERVOMIN, SERVOMAX); };
+  { return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX); };
+  //servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle));
 
   auto angleToMicroPulse = [](double a)
-  { return modifiedMap(a, 0.0, 180.0, USMIN, USMAX); };
-
-  //servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle));
-  servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle));
+  { return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); };
+  servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle) + _joints[j].center);
+  #endif
 
   _joints[j].pos = angle;
 }

src/main.cpp

@@ -3,6 +3,8 @@
 
 #include "body.h"
 #include "config.h"
+#include "OSCRemote.h"
+#include "Vbat.h"
 
 //! --- Start of onfiguration ---
 const char* hostname = "Hexapod";
@@ -10,14 +12,15 @@ const char* hostname = "Hexapod";
 
 
 Body body = Body::instance();
-
 Config config;
+IRemote* remote;
+Vbat vbat;
 
 void servoTest() {
   for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++)
   {
-    body.legs[legnum].setPos(Leg::Hipp, 90);
-    body.legs[legnum].setPos(Leg::Knee, 45);
+    body.legs[legnum].setPos(Leg::Hipp, 0);
+    body.legs[legnum].setPos(Leg::Knee, 0);
   }
 }
 
@@ -36,6 +39,34 @@ void connectWiFi()
   Serial.println(WiFi.localIP());
 }
 
+void testRemoteCallback(const IRemote::Output& o)
+{
+  for (size_t i = 0; i < 6; ++i)
+  {
+    body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll)*4);
+    body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch)*4);
+    //Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
+  }
+}
+
+
+void sweepLeg()
+{
+  static uint8_t d = 0;
+  if (d < 2)
+  {
+    for (long angle = 0; angle < 180*5; angle++)
+    {
+      body.legs[1].setPos(Leg::Knee, double(angle/5));
+    }
+    for (long angle = 180*5; angle > 0; angle--)
+    {
+      body.legs[1].setPos(Leg::Knee, double(angle/5));
+    }
+  }
+  d++;
+}
+
 void setup() {
   Serial.begin(9600);
 
@@ -43,12 +74,16 @@ void setup() {
   config.init();
   delay(250);
 
+  
   servoTest();
   config.load();
+
   Serial.printf("SSID: %s\n", config.data().wifiSSID);
   Serial.printf("pass: %s\n", config.data().wifiPass);
-
   connectWiFi();
+  remote = new OSCRemote(vbat);
+  remote->registerCallback(testRemoteCallback);
+  //remote->init();
 
 
 
@@ -58,29 +93,16 @@ void setup() {
   config.setWifiPass("testPass");
   config.save();
   */
+  sweepLeg();
 }
 
 
-
 void loop() {
-  Serial.print(".");
-  sleep(1);
+  //Serial.print(".");
+  //sleep(1);
+  remote->loop();
   
-  static uint8_t d = 0;
-  if (d < 2)
-  {
-    for (long angle = 0; angle < 180*5; angle++)
-    {
-      body.legs[1].setPos(Leg::Knee, double(angle/5));
-      //delay(1);
-    }
-    for (long angle = 180*5; angle > 0; angle--)
-    {
-      body.legs[1].setPos(Leg::Knee, double(angle/5));
-      //delay(1);
-    }
-  }
-  d++;
+
 
   body.healthCheck();
 }