Basic Node for the Internet of Sex Toys – part 3: software

This the third part of the tutorial which has the following parts:

part 1: Basic Node for the Internet of Sex Toys

part 2: Molding the Basic Node

part 3: Software for the Basic Node

For the basic node a simple software realizes all features like Mqtt communication, Web server, basic web user interface, reading data from the accelerometer. Please use the code at github and send request over github. Now the imported parts of the code are explained.

To communicate with the IOT Mqtt is used (read more here). This is a fast protocol for data transmission. Therefore we need a Mqtt server. You can install one on your local computer or use a cloud-based Mqtt server. We use the free CloudMqtt. The following variables must be initiated with the data  of your server. Please get your own account at CloudMqtt or use my server (but don’t spam it, please). Please remember: Transmission is not encrypted, everybody can read it.

const char* mqtt_server = "m12.cloudmqtt.com";
uint16_t mqtt_port = 15376;
const char* mqtt_user = "nvcuumkf";
const char* mqtt_password = "C-X6glwisHOP";

We have now  7 different modes. In each mode the basic node behaves different.

const int offMode = 0;
const int maxMode = 1;
const int sinusMode = 2;
const int motionMode = 3;
const int constantMode = 4;
const int listenMode = 5;
const int listenAndMotionMode = 6;

In off mode the basic node is off, in max mode the vibration is maximum. In sinus mode the vibration speed is altered according to a sinus curve.

Web user interface of the basic node

In motion mode the vibration changes according to the movement of the basic node. When moved fast the speed goes up, when moved slowly or movement stops, the speed goes down. In constant mode any vibration speed can be set to any strength. This feature is only available by Mqtt messages eg. from the IOT node-RED user interface. The listen mode is still experimental. In this mode the speed will be changed by OTHER basic nodes. Finally in the listenAndMotionMode the speed is changed by movements of the basic node and by other nodes. This feature was already available with the body interaction 1 development board as standard mode!

The basic node starts a web server (see image). A web page is generated which build up the user interface. There are buttons for every mode. In addition the speed and the battery power is displayed. This is done in this function:

void generateWebpage() {

The next lengthy procedure is this:

void mqttCallback(char* topic, byte* payload, unsigned int length) {

This is a call back function which is executed whenever a Mqtt message comes in. It parses the Mqtt message which is in the popular JSON format. The commands which are communicated within JSON are explained here. In principle there is a command for every mode, when the command “set mode to off” is send the mode is set to offMode.

In the setup() part of the code you will find a lot of lines like that:

httpServer.on("/MOTOR=MAX", []() {

They corresponds to the generateWebpage() function. When say the max button on the web page is pressed than the affiliated httpServer function is executed. So for every button on the webpage you need a corresponding httpServer function to implement the functionality. In this case (MOTOR=MAX) the mode is set to the constant speed maxMode.

Finally in the loop section of the code the following functions are implemented:

  • reading the accelerometer data
  • change the vibration motor speed according to the mode
  • generate a new JSON message which is send out via Mqtt
  • do the timing

Not mentioned is the OTA (over the air update) function, which is integrated in the code.


For controlling the toy via the internet you can use node-Red. You can find the code at github via this link.

The flow is explained here and here.


Internet of (sex) things – part 2: MQTT messages

In the first IOT tutorial we have shown how to build a sex toy based on a ESP8266 MCU.

There are two more parts of this tutorial series:

part 3: Node-RED

part 4: Building a sex toy dashboard with Node-RED

The ESP8266 is a microcontroller which can connect to the internet. You can write sketches with the Arduino IDE and connect a lot of actuator modules (like vibration motors, displays) and sensors (motion detection).

nodemcu prototype breadboard

In the first approach a web server was installed on the ESP8266. Control was possible by browsing to the web server, which could be accessed by a computer or smart phone using the same WiFi Access Point (AP).

browser vibrator control

In this second tutorial we will connect the ESP8266 to the internet and publish and receive data as well as commands by internet.


There are some protocols for publishing (sending) data. We use the MQTT protocol which is very fast and suited for short messages. In addition we need a MQTT server. We will send the ESP8266 data to the server. And the ESP8266 can receive data (or commands) from the server. Other services can connect to the MQTT server, too. In this tutorial we will use a MQTT client which can display the sex toy data and send commands to the ESP8266.


Sex toy data exchange format

A big disadvantage of commercial sex toys is their proprietary data exchange formats. Kyle Machutis developed buttplug.io a Cross Platform Framework for Sex Toy Control to overcome this issue.

For this IOT project we will use the JSON format which is a very simple way to exchange data:

This is an example for the message type “sensor”. It says that the fusioned sensor data of the “mps9250” MPU is 5657, the LED is off (0), the motor is in mode sinus curve (2) and the speed of the vibration motor is 766.


There are three message types: sensor, execute and message.

  • Sensor is for publishing sensor data to everyone who is interested in that.
  • Execute is for controlling a specific device, eg for turning the motor on.
  • Message sends a text message to specific device. If the deviceID is not given it will be sent to all receivers,

For this project we need the

  • hardware from part 1 of the IOT project
  • the Arduino library JSON for parsing and encoding exchange data
  • a library for sending and receiving MQTT data
  • the mqtt-spy software to send and receive MQTT messages
  • optional: a MQTTserver like mosquitto

Arduino JSON library

There is a great library which can parse and build JSON files. Here is the documentation: https://github.com/bblanchon/ArduinoJson

Please go into the Arduino library manager and install the JSON package.

Arduino PubSubClient library

In addition we need a package for sending and receiving MQTT messages. You will find the PubSubClient library in the library manager, too.

Documentation: https://github.com/bblanchon/ArduinoJson

You have to change the maximum length of a message. By default it is only 128. Search for the file PubSubClient.h and change MQTT_MAX_PACKET_SIZE to 1024. On my Win10 system the file is located at


Change the following line:

// MQTT_MAX_PACKET_SIZE : Maximum packet size
 #define MQTT_MAX_PACKET_SIZE 1024

MQTT-SPY software

This is an excellent piece of software which can connect to a MQTT server and send and receive messages: mqtt.spy. We use it to test the connection from the ESP8266 to the mosquitto server and for debugging our vibrator toy control software on the ESP8266.

Download from https://github.com/kamilfb/mqtt-spy/wiki/Downloads

If you haven’t your own MQTT server, you can use the Mosquito test server (only for testing, don’t spam them). Start mqtt-spy and connect to “test.mosquito.org”.


Subscription of messages: To receive the messages from the ESP8266  you have to subscribe to a topic. Use the same topic as in the source code, eg. “BIoutTopic”. You have to select the “New”-tab in the subscription, a new window pops up. Enter “BIoutTopic”. Now you receive all messages from the ESP8266.mqtt-client

Publishing messages: In the upper part of the window enter “BIinTopic”. In the “data” field you enter commands which will be sent to the ESP8266. Enter the commands and press “Publish”.

subscription-windowTry it out. Here are some examples:

JSON data exchange & command examples

Send message:

 messageType : "message",
 message : " Hello world!"

Send Command – LED on:

 messageType : "execute",
 actuator : "LED",
 actuatorValue : 1

Send command – motor1 modus is set to sinus curve vibration pattern

 messageType : "execute",
 actuator : "motor1",
 actuatorMode : "sinus"

Send Command – set motor speed to 1000.

 messageType : "execute",
 actuator : "motor1",
 actuatorValue: 1000

Send command – set motor1 off.

 messageType : "execute",
 actuator : "motor1",
 actuatorMode: "off"

Message type “sensor”: this is for publishing sensor data of a sex toy (eg. position in 3d space, movement data, vibration pattern in use). These data can be received by other sex toys for vibration adjustment and synchronization of the vibration speed .

 messageType : "sensor",
 sensor: "mps9250",
 fusionedData: 10000,

Remark: If it doesn’t work as expected, check the data field of the MQTT software. Paste & Copy doesn’t work as it should be and you may have to remove some redundant “{“-signs.


The code is based on part 1. In the new code functions for publishing and receiving JSON data over MQTT are added.

Receiving and parsing incoming JSON files is done in “callback”. “callback” will be executed whenever a MQTT message comes in.

void callback(char* topic, byte* payload, unsigned int length) {

At first the incoming message is parsed and the result is stored in “root”. Now each entry in the JSON file can be accessed by assigning a new variable to root[entry]:

StaticJsonBuffer<500> jsonBuffer;
 JsonObject& root = jsonBuffer.parseObject(s);
 if (!root.success()) {
 Serial.println("parseObject() failed");
 String messageType = root["messageType"];

Build and publishing JSON files is done in the loop. A JsonObject “root” is created and then each entry in the JSON file is added by an assignment to root eg:  root[“messageType”] = “sensor”:

StaticJsonBuffer<500> jsonBuffer;
JsonObject& root = jsonBuffer.createObject();
root["messageType"] = "sensor"; // execute, message, sensor

For producing the JSON file there is “printTo” function. The JSON array of char must be formatted with the snprintf() function and then it can be published.

 root.printTo(outMsg, sizeof(outMsg));
 snprintf (msg,1000, "%s",outMsg);
 mqttclient.publish("BIoutTopic", msg);

Download the zipped and formatted code here: nodemcu-server-mqtt-iot.

In the next part of the tutorial we use Node-RED for visual programming our IOT sex toys.

Here is the complete source code for part 2 of the tutorial.

// bodyinteraction IOT project
// IOT sex toy prototype - control via browser and MQTT
#include <ESP8266WiFi.h>
#include <Wire.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>

#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C

#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18

#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18

const char* ssid = "ALICE-WLAN";
const char* password = "36s69c3756a65";
const char* mqtt_server = "test.mosquitto.org";

int ledPin = 0; // NodeMCU pad D3 = GPIO 0
int motorPin= 13; // NodeMCU pad D7 = GPIO 13
double sinusValue=0;

// define constants for four different vibration modes
const int offMode=0;
const int maxMode=1;
const int sinusMode=2;
const int motionMode=3;

int motorMode =offMode; //current mode
int motionVector = 0; //current fusioned motion
// Acceleration in x,y and z direction at t(ime)=1 and time=0
// Geroscop data
int16_t ax,ay,az,ax1,ay1,az1,gx,gy,gz,gx1,gy1,gz1;
int valueMotor; //vibrator motor speed 0-1023

WiFiServer server(80);

WiFiClient espclient;
PubSubClient mqttclient(espclient);
char msg[512];
char outMsg[512];

bool requesting=false; // is there a request eg button pressed on webpage

//timing of mqtt messages
long now=0;
long lastMsg = 0;

int value = 0;
int valueLED = LOW;

// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
// Set register address

// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())

// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
// Set register address

// Return the response /generate webpage
void generateWebpage(WiFiClient espclient) {
espclient.println("HTTP/1.1 200 OK");
espclient.println("Content-Type: text/html");
espclient.println(""); // do not forget this one
espclient.println("<!DOCTYPE HTML>");

espclient.print("Led pin is now: ");

if(valueLED == HIGH) {
} else {

Motor pin is now: ");


espclient.println("<a href=\"/LED=ON\"\"><button>Turn On </button></a>");
espclient.println("<a href=\"/LED=OFF\"\"><button>Turn Off </button></a>

espclient.println("<a href=\"/MOTOR=MAX\"\"><button>Motor Max </button></a>");
espclient.println("<a href=\"/MOTOR=OFF\"\"><button>Motor Off </button></a>");
espclient.println("<a href=\"/MOTOR=SINUS\"\"><button>Motor sinus curve </button></a>");
espclient.println("<a href=\"/MOTOR=MOTION\"\"><button>Motor motion controlled </button></a>

// function callback is executed when a Mqtt message comes in
// - prints mqtt message
// - parse JSON file
// - execute commands
void callback(char* topic, byte* payload, unsigned int length) {
Serial.print("Message arrived [");
Serial.print("] ");
char s[length];
for (int i = 0; i < length; i++) {
StaticJsonBuffer<500> jsonBuffer;
JsonObject& root = jsonBuffer.parseObject(s);
if (!root.success()) {
Serial.println("parseObject() failed");
String messageType = root["messageType"]; //sensor, execute , message
String targetDeviceID = root["targetDeviceID"];
String actuator = root["actuator"];
int actuatorValue = root["actuatorValue"];
String actuatorMode = root["actuatorMode"];
String message = root["message"];
Serial.print("messageType: ");
Serial.print("actuator: ");
Serial.print("actuatorValue: ");

// print message
if (messageType=="message") {
Serial.print("Incoming message: ");
// LED commands
if (messageType=="execute"&&actuator=="LED"&&actuatorValue==1) {
Serial.println("LED on received");
digitalWrite(ledPin, HIGH);
valueLED = HIGH;
if (messageType=="execute"&&actuator=="LED"&&actuatorValue==0) {
Serial.println("LED off received");
digitalWrite(ledPin, LOW);
valueLED = LOW;
// set modes commands
if (messageType=="execute"&&actuator=="motor1"&&actuatorMode=="off") {
analogWrite(motorPin, 0);
valueMotor = 0;
if (messageType=="execute"&&actuator=="motor1"&&actuatorMode=="sinus") {
if (messageType=="execute"&&actuator=="motor1"&&actuatorMode=="motion") {
if (valueMotor<500) {valueMotor=500;} if (valueMotor>1023) {valueMotor=1023;}
// set motor speed command
if (messageType=="execute"&&actuator=="motor1"&&actuatorValue!=0) {
Serial.println("set motor speed to fixed value");
// incoming sensor data, adjust motor speed when in motion mode
int fusionedData = root["fusionedData"];
String sensor = root["sensor"];
if (messageType=="sensor"&&sensor=="mps9250"&&motorMode==motionMode) {
if (fusionedData > 5000) {valueMotor=valueMotor+25;} else {valueMotor=valueMotor-10;}
if (valueMotor<500) {valueMotor=500;} //values must be above 500 otherwise the motor is off if (valueMotor>1023) {valueMotor=1023;} // values higher than 1023 are not supported
analogWrite(motorPin, valueMotor); // set motor speed

// connect to mqtt server
void reconnect() {
while (!mqttclient.connected()) {
Serial.print("Attempting MQTT connection...");
// Create a random client ID
String clientId = "ESP8266Client-";
clientId += String(random(0xffff), HEX);
// Attempt to connect
if (mqttclient.connect(clientId.c_str())) {
} else {
Serial.print("failed, rc=");
Serial.println(" try again in 5 seconds");

void setup() {
// connect MPU9265 via i²c bus
// NodeMCU D1 = GPIO5 connected to MCU9265 SCL
// NodeMCU D2 = GPIO4 connected to MCU9265 SDA

// Configure gyroscope range
// Configure accelerometers range
// Set by pass mode for the magnetometers

// Request first magnetometer single measurement


// init LED pin
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, LOW);

// init motor pin
pinMode(motorPin, OUTPUT);
analogWrite(motorPin, 0);

// Connect to WiFi network
Serial.print("Connecting to ");

WiFi.begin(ssid, password);

while (WiFi.status() != WL_CONNECTED) {
Serial.println("WiFi connected");

// Start the server
Serial.println("Server started");

// Print the IP address
Serial.print("Use this URL to connect: ");

// init mqtt client
mqttclient.setServer(mqtt_server, 1883);

void loop() {
if (!mqttclient.connected()) {

// Read accelerometer and gyroscope
uint8_t Buf[14];

// Create 16 bits values from 8 bits data

// Accelerometer
ax=-(Buf[0]<<8 | Buf[1]);
ay=-(Buf[2]<<8 | Buf[3]);
az=Buf[4]<<8 | Buf[5];

// Gyroscope
gx=-(Buf[8]<<8 | Buf[9]);
gy=-(Buf[10]<<8 | Buf[11]);
gz=Buf[12]<<8 | Buf[13]; // when in "motionMode" the vibration motor is controlled by motion if (motorMode==motionMode) { motionVector=sqrt(pow(ax-ax1,2)+pow(ay-ay1,2)+pow(az-az1,2)); //calculate motion vector // adjust vibration motor speed // if motion vector > 5000 raise speed by 25
// otherwise lover speed by 10
// adjust these constants to your needs
if (motionVector > 5000) {valueMotor=valueMotor+25;} else {valueMotor=valueMotor-10;}
if (valueMotor<500) {valueMotor=500;} //values must be above 500 otherwise the motor is off if (valueMotor>1023) {valueMotor=1023;} // values higher than 1023 are not supported
analogWrite(motorPin, valueMotor); // set motor speed

Serial.print("motionVector: ");
Serial.print(", valueMotor: ");

// save values

// change vibration motor speed according to a sinus curve
if (motorMode==sinusMode) {
int sinTmp = ((sin(sinusValue)+1)*.5*(1023-500))+500;
analogWrite(motorPin, sinTmp);

StaticJsonBuffer<500> jsonBuffer;
JsonObject& root = jsonBuffer.createObject();
root["messageType"] = "sensor"; // execute, message, sensor
// execute - send command to other device
// root["targetDeviceID"] = "xxxxx"; //for execute and message message types
// root["actuator"] = "motor1";
// root["actuatorValue"]="222";
// root["actuatorMode"] = "sinus";
// root["command"] = none;
// root["commandParameter1"] ="";

// message - send message to targetDeviceID
// root["message"] = "hello world";

//sensor - for publishing sensor data
root["sensor"] = "mps9250";
// root["time"] = none;
root["fusionedData"] = motionVector;
root["messageNumber"] = 0;
// example for raw data
// JsonArray& rawdata = root.createNestedArray("rawdata"); // x,y,z,roll, pitch better??
// rawdata.add(0, 0); // ax
// rawdata.add(0, 0); // ay
// rawdata.add(0, 0); // az
// rawdata.add(0, 0); // gx
// rawdata.add(0, 0); // gx
// rawdata.add(0, 0); // gx
// rawdata.add(0, 0); // mx
// rawdata.add(0, 0); // mx
// rawdata.add(0, 0); //mx
root["LEDstatus"] = valueLED;
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;

// publish motor speed as mqtt message every 10 seconds
// only when motor in not off
if (motorMode==maxMode||motorMode==sinusMode||motorMode==motionMode) {
now = millis();
if (now - lastMsg > 1000) {
if (!mqttclient.connected()) {
lastMsg = now;
root["messageNumber"] = value;
// publish data as MQTT message in JSON format
root.printTo(outMsg, sizeof(outMsg));
snprintf (msg, 1000, "%s",outMsg);
mqttclient.publish("BIoutTopic", msg);
Serial.print("Publish message every 10 sec: ");

// Check if a client has connected to the wifi server
WiFiClient espclient = server.available();
if (!espclient) {


// read the first line of the request
String request = espclient.readStringUntil('\r');

// LED on button pressed
Serial.println("hi execute rqeuest");
if (request.indexOf("/LED=ON") != -1) {
digitalWrite(ledPin, HIGH);
valueLED = HIGH;
root["LEDstatus"] = valueLED;
// LED off button pressed
if (request.indexOf("/LED=OFF") != -1) {
digitalWrite(ledPin, LOW);
valueLED = LOW;
root["LEDstatus"] = valueLED;
// set motor to maximum speed button pressed
if (request.indexOf("/MOTOR=MAX") != -1) {
analogWrite(motorPin, 1023);
valueMotor = 1023;
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;
// set motor off button pressed
if (request.indexOf("/MOTOR=OFF") != -1) {
analogWrite(motorPin, 0);
valueMotor = 0;
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;
// motor amplitude controlled like a sinus curve
if (request.indexOf("/MOTOR=SINUS") != -1) {
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;
// motor speed is adjusted by movements (classical "body interaction" interaction pattern)
if (request.indexOf("/MOTOR=MOTION") != -1) {
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;


// send outMessage to Mqtt server
if (requesting) {
if (!mqttclient.connected()) {

root["messageNumber"] = value;
root["motor1mode"] = motorMode;
root["motor1speed"] = valueMotor;

// publish data as MQTT message in JSON format
root.printTo(outMsg, sizeof(outMsg));
snprintf (msg,1000, "%s",outMsg);
mqttclient.publish("BIoutTopic", msg);

Serial.print("Publish message: ");
%d bloggers like this: