OSC Control for SimpleFOC

I was asked about using OSC in conjunction with SimpleFOC

OSC (Open Sound Control) is the new MIDI…

And MIDI, while coming from the sound would, has always also been about controlling lights, animatronics and other things as well as sound devices… So it is quite suitable for controlling motors, actuators or robots too, and providing feedback from a SimpleFOC device to a UI.

In the end OSC is a simple, light-weight packet oriented protocol for exchanging key/value messages of the form “/Name/Subname/Index/etc value, value, value”.

It’s not super-fast, so you’ll want something else to implement “oscilloscope” type monitoring. But its fast enough to handle several updates per second, simple to use, and therefore pretty ideal for control/UI type connections.

It comes with a bunch of ready-made software and toolings:

Specs: http://opensoundcontrol.org/introduction-osc
TouchOSC UI: https://hexler.net/products/touchosc
Arduino Lib: https://github.com/CNMAT/OSC

There’s a lot more, MIDI is a fairly big world…

I’ll post a couple of pictures and stuff from the experiment I did below.

1 Like

So, to connect up OSC to SimpleFOC, you will need:

  • Your Arduino code, which includes the OSC and SimpleFOC libraries
  • A OSC UI, for example TouchOSC

It can look like this:

That’s a picture of my simple TouchOSC UI. The circular control shows the current sensor angle - it moves as the motor moves.
The narrow slider in the middle shows the current velocity. It moves as well.
The big slider at the bottom sets the target value, for example target velocity - it can be positive or negative…

The UI can be configured in the TouchOSC editor:

Once you’re done, you can sync your UI to the smartphone or tablet.

1 Like

A Sketch could look something like this:

#include "Arduino.h"
#include <SimpleFOC.h>

#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCBoards.h>
#include <SLIPEncodedSerial.h>
#include <Math.h>

MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8);
BLDCMotor motor = BLDCMotor(9, 10, 11, 7);
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial


void setup() {
	SLIPSerial.begin(115200);
	sensor.init();
	motor.linkSensor(&sensor);
	motor.voltage_power_supply = 8;
	motor.controller = ControlType::velocity;
	motor.PID_velocity.P = 0.2;
	motor.PID_velocity.I = 20;
	motor.PID_velocity.D = 0.001;
	motor.PID_velocity.output_ramp = 1000;
	motor.LPF_velocity.Tf = 0.01;
	motor.voltage_limit = 8;
	//motor.P_angle.P = 20;
	motor.init();
	motor.initFOC();
	_delay(1000);
}

void sendStatus(const char* statusMsg) {
	OSCMessage msgOut("/mot1/status");
	msgOut.add(statusMsg);
    SLIPSerial.beginPacket();
    msgOut.send(SLIPSerial); // send the bytes to the SLIP stream
    SLIPSerial.endPacket(); // mark the end of the OSC Packet
    msgOut.empty();
}

// velocity set point variable
float target_velocity = 2.0;
// angle set point variable
float target_angle = 1.0;


void motorControl(OSCMessage &msg){
	if (msg.isInt(0))
		target_velocity = radians(msg.getInt(0));
	else if (msg.isFloat(0))
		target_velocity = radians(msg.getFloat(0));
	else if (msg.isDouble(0))
		target_velocity = radians(msg.getDouble(0));

}

void cmdControl(OSCMessage &msg){
	char cmdStr[16];
	if (msg.isString(0)) {
		msg.getString(0,cmdStr,16);
		String it(cmdStr);
		motor.command(it);
	}
}

long lastSend = 0;
OSCMessage bundleIN;

void loop() {
    OSCBundle bundleOUT;
    int size;

	// FOC algorithm function
	motor.move(target_velocity);
	motor.loopFOC();

	if (!SLIPSerial.endofPacket()) {
		if( (size=SLIPSerial.available()) > 0) {
			while(size--)
				bundleIN.fill(SLIPSerial.read());
		}
	}

	if (SLIPSerial.endofPacket()) {
		if(!bundleIN.hasError()){
			bundleIN.dispatch("/mot1/S", motorControl);
			bundleIN.dispatch("/mot1/C", cmdControl);
		}
		bundleIN.empty();
	}


	long now = millis();
	if (now - lastSend > 100) {
	    //BOSCBundle's add' returns the OSCMessage so the message's 'add' can be composed together
		int ang = (int)degrees(motor.shaftAngle()) % 360;
		if (ang<0) ang = 360-abs(ang);
		bundleOUT.add("/mot1/A").add((int)ang);
		bundleOUT.add("/mot1/V").add((int)degrees(motor.shaftVelocity()));
	    SLIPSerial.beginPacket();
	    bundleOUT.send(SLIPSerial); // send the bytes to the SLIP stream
	    SLIPSerial.endPacket(); // mark the end of the OSC Packet
	    bundleOUT.empty(); // empty the bundle to free room for a new one
	    lastSend = now;
	}

}
1 Like

As you can see from the code above, I’m using serial communications… but OSC is actually designed to work with UDP networking.

On an ESC32, or on an Arduino with Ethernet shield, you can use UDP directly with OSC. There are examples for this on the OSC Arduino docs, it is the default.

If your MCU does not do Networking, then you can still use OSC via serial. in this case you use the SLIP protocol (those of us who are old enough will remember this from the early days of the internet, when we still used Modems :scream:)…

SLIP is basically a way of sending Packets via a serial line. It’s super-simple, and you can use the SlipSerial library as a wrapper for the normal Serial object. The code above shows how it works.

On the receiving end, you need something which reads the SLIP packets from serial line, and sends them over the network via UDP to someone interested in receiving them.
I solve this part using pure data - pD.
See: https://puredata.info/downloads/Pd-L2Ork

I have a pd sketch which connects the SLIP Serial to UDP (and reverse) and also prints all the packets to the console. It has a bunch of “spigots” so you can selectively control the communication flow. Importantly, it also limits the amount of packets sent out via Serial, so as not to flood the MCU.

Here is a picture of the sketch:

OSC is going to replace ROS for lots of things in my projects now. Thanks for the example.

I will try to add the files, maybe @Antun_Skuric will accept them if I put them in a pull request into the examples directory?

Would that be a good way to share them?

1 Like

I use OSC a lot for exhibitions, great way to have cross communication between multiple computers and devices.

Hey @runger,

go for it, if you have a nice generic example we can add it to the example folder. Or even as a standalone repo if it makes more sense.

I’ve never used OSC, it is a very specific solution but I can clearly see the benefits.The only reason why I used serial is because it is simple and supported everywhere. :smiley:

Ok, I got round to sending a pull request for a first example.

I will try to add a README page, and a second example for MCUs with only serial port connections soon.

By the way, I am not questioning your use of serial - I think OSC is a nice addition for those who want it, but serial is obviously the way to go for the base library. People would not want the OSC library using space in their firmware if they’re not using it.

Just to further motivate my thinking, and the kind of thing one could maybe do, and why OSC and Pd are so cool:

With OSC you create a bridge to a rich world of OSC and MIDI tools and software. It could be possible to hook up existing off-the-shelf hardware arrangers, pads and control boards - and also musical instruments - to send commands which ultimately control motors.

Software like Pd (PureData) or OSCulator, and many others, can route, remix and transform the OSC messages, to create custom setups and filter the inputs to the outputs (SimpleFOC motorised systems).

This could be used to create:

  • art that moves in rhythm with music played by an artist,
  • or hardware setups that use motors to play acoustic instruments,
  • or whole new instruments involving motors and sensor control
  • or motorised lighting/lasers moving to music
  • or just animatronic art for fun or effect

The mechanical basis is always a closed loop type BLDC servo controlled by SimpleFOC - where the fine control enabled by your FOC algorithm makes it possible to control fine movement, both fast and slow.