From e99e8e32118a1becf2b910bbe4cd2c8c7b3e04cc Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Mon, 17 Sep 2018 22:08:32 +0200
Subject: [PATCH] OSC Wifi to UART bridge

Mix of the two earlier sketches

TODO: Switch this sketch to AP mode
---
 NodeMCU/OSCWifiBridge/OSCWifiBridge.ino | 186 ++++++++++++++++++++++++
 1 file changed, 186 insertions(+)
 create mode 100644 NodeMCU/OSCWifiBridge/OSCWifiBridge.ino

diff --git a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino
new file mode 100644
index 0000000..367242f
--- /dev/null
+++ b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino
@@ -0,0 +1,186 @@
+#ifdef ESP8266
+#include <ESP8266WiFi.h>
+#else
+#include <WiFi.h>
+#endif
+#include <WiFiUdp.h>
+#include <OSCMessage.h>
+#include <OSCBundle.h>
+#include <OSCData.h>
+
+char ssid[] = "FaRgO2G4";
+char pass[] = "Johansson85";
+
+WiFiUDP Udp;
+
+// remote IP (not needed for receive)
+// Will be updated once the first package is received
+IPAddress outIp(192,168,1,10);
+
+const unsigned int outPort = 9999;
+const unsigned int localPort = 8888;
+
+OSCErrorCode error;
+
+// LED to reflect the active status of the robot
+// Start "on"
+unsigned int ledState = HIGH; 
+
+typedef struct Packet {
+  int16_t MagicWordLow;
+  int16_t MagicWordHigh;
+  
+  int16_t Throttle;
+  int16_t Steering;
+  
+  int16_t Kp;
+  int16_t Ki;
+  int16_t Kd;
+  
+  bool Enabled;
+};
+
+const int bufferSize = sizeof(Packet); // Size of Packet
+uint8_t buffer[bufferSize];
+
+Packet* pPacket = (Packet*)buffer;
+
+void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
+  Serial.begin(57600);
+  Serial.println();
+  Serial.print("Connecting to ");
+  Serial.println(ssid);
+  WiFi.begin(ssid, pass);
+  Udp.begin(localPort);
+
+  pPacket->MagicWordLow = 0x0DED;
+  pPacket->MagicWordHigh = 0x0DEC;
+}
+
+void sendPacket()
+{
+  Serial.write(buffer, bufferSize);
+}
+
+void OSCMsgReceive()
+{
+  OSCMessage msgIN;
+  int size;
+  if ((size = Udp.parsePacket()) > 0)
+  {
+    while(size--)
+    {
+      msgIN.fill(Udp.read());
+    }
+    if (!msgIN.hasError())
+    {
+      msgIN.route("/Robot/Enable",toggleEnabled);
+      msgIN.route("/Fader/Throttle",funcThrottle);
+      msgIN.route("/Fader/Steering",funcSteering);
+      msgIN.route("/Gain/Kp",funcKp);
+      msgIN.route("/Gain/Ki",funcKi);
+      msgIN.route("/Gain/Kd",funcKd);
+    }
+  }
+}
+
+void sendUdp(OSCMessage* pMessage)
+{
+  IPAddress remoteIp = Udp.remoteIP();
+  //outIp = Udp.remoteIP();
+  if (remoteIp != outIp)
+  {
+    outIp = remoteIp;
+    Serial.print("New source address: ");
+    Serial.println(outIp);
+  }
+
+  //send osc message back to controll object in TouchOSC
+  //Local feedback is turned off in the TouchOSC interface.
+  //The button is turned on in TouchOSC interface whe the conrol receives this message.
+  Udp.beginPacket(Udp.remoteIP(), outPort);
+  pMessage->send(Udp); // send the bytes
+  Udp.endPacket(); // mark the end of the OSC Packet
+  pMessage->empty(); // free space occupied by message
+  
+  sendPacket(); // Send on UART at the same time
+}
+
+void toggleEnabled(OSCMessage &msg, int addrOffset){
+  ledState = !(boolean) msg.getFloat(0);
+  OSCMessage msgOUT("/Robot/Enable");
+
+  digitalWrite(BUILTIN_LED, ledState);
+
+  msgOUT.add(!ledState);
+  sendUdp(&msgOUT);
+  pPacket->Enabled = ledState;
+
+  Serial.println( ledState ? "Robot disabled" : "Robot enabled");
+}
+
+void funcThrottle(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Fader/Throttle");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacket->Throttle = value;
+}
+
+void funcSteering(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Fader/Steering");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacket->Steering = value;
+}
+
+void funcKp(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Kp");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacket->Kp = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KpOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcKi(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Ki");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacket->Ki = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KiOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcKd(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Kd");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacket->Kd = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KdOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void loop() {
+  OSCMsgReceive();
+}
+