Robot Engage Blog Buy Now
Blog

Misty Community Forum

JavaScript "Turn to heading" function

So a while back I noticed that there was no dedicated function in Misty’s JavaScript or REST API to have Misty turn, in place, to a specific heading (0 - 360). There are a couple more recent additions that get close, like misty.driveArc and misty.driveHeading, but they always require Misty moving away from her current location in some way. What I was looking for was a turn-in-place function and it just wasn’t there. I also needed callbacks when Misty reached that heading which the current JS or REST APIs didn’t have, so I built my own.

In this post I’ll show what I came up with using JavaScript and Misty’s REST API. It isn’t necessarily meant as a plug-and-play script, but more as a reference if you need something like this for your own coding.

Firstly, I use the lightSocket.js and fetchClient.js scripts created by the Misty team for communicating with Misty’s REST API, so you’ll need those included somewhere in your setup for this to work. This code also references a global var ip which is the IP address of the robot…you can hard-code that if you wish with var ip = "123.123.123.0"; substituting it with your IP.

I start out creating a listener for Misty’s body rotation (yaw) and store that globally in currYaw:

// Subscribe to Misty IMU and listen for yaw data, change your debounce from 10ms if needed
var currYaw = 0;
var debounceMSIMU = 10;
lightSocket.Subscribe("IMUPhaser", "IMU", debounceMSIMU, null, null, null, null, function(data){
	var nYaw = data.message["yaw"];
	currYaw = nYaw; // 0 to 359.99
});

With this we can monitor Misty’s yaw which will be helpful to controlling it in the next step, and for that all-important success callback. Here’s the main function to turn Misty:

// Turn misty towards a specific heading, accepts heading of 0 to 359
var nCheckHeadingINT = "";
function nMistyTurnTo(nHeading, successCallback = null, failCallback = null){
	if(nCheckHeadingINT){ return; } // don't allow this function to be active with more than one thread
	var angularVel = 40; // default turn speed (0-100)
	var nAvgDegPerSec = 36; // how quickly Misty turns and used for timeouts, calculated manually on a hard surface
	var nCurrHeading = currYaw.toFixed(0); // from our global listener
	var nAngleDiff = 180 - Math.abs(Math.abs(nCurrHeading - nHeading) - 180);
    // slow down Misty for smaller turns, increases turn accuracy
	if(nAngleDiff<=90){ angularVel = angularVel/2; nAvgDegPerSec = nAvgDegPerSec/2; }
	if(nAngleDiff<=45){ angularVel = angularVel/2; nAvgDegPerSec = nAvgDegPerSec/2; }
	var nTurnTimeMS = (nAngleDiff/nAvgDegPerSec)*1000;
	// if we're not already at that heading, let's turn
    if(nCurrHeading != nHeading){
		if(nAngleDiff>90){ nTurnTimeMS-=500; } // split large turns into 2 different speed segments, first faster and second slower
		if(nCurrHeading>nHeading && Math.abs(nCurrHeading-nHeading)<180){ angularVel = -angularVel; }
		var nMoveData = "{ \"LinearVelocity\": "+linearVel+", \"AngularVelocity\": "+angularVel+", \"TimeMS\": "+nTurnTimeMS+" }";
		// Turn
		try {
			fetchClient = new FetchClient(ip, 10000);
			fetchClient.PostCommand("drive/time",nMoveData, function(){
				// monitor turn every debounce of the IMU for a max. of 150% of the total turn time
				var nCounter = 0, nTimeoutMS = (timeMS*1.5), nStep = debounceMSIMU;
				nCheckHeadingINT = setInterval(function(){
					nCurrHeading = currYaw.toFixed(0);
					nCounter++;
					if(Math.abs(nCurrHeading-nHeading)<=2){ // if we match heading, plus minus 2 degrees
						clearInterval(nCheckHeadingINT); nCheckHeadingINT = "";
						nStopMisty();
						if(typeof successCallback == "function"){ successCallback(); }
						return;
					}
					if(nCounter>(nTimeoutMS/nStep)){
						clearInterval(nCheckHeadingINT); nCheckHeadingINT = "";
						nStopMisty();
						nMistyTurnTo(nHeading,successCallback,failCallback); // try again
						return;
					}
				},nStep);
			});
		} catch(e) {
			clearInterval(nCheckHeadingINT); nCheckHeadingINT = "";
			if(typeof failCallback == "function"){ failCallback(); }
		}
	}
}

…and you would call it like this:

nMistyTurnTo(
	180, // or any value between 0 and 359
	function(){ console.log("turning complete!"); },
	function(){ console.log("uh oh...something went wrong."); }
);

Let’s step through the code quickly of the nMistyTurnTo function.

Firstly, we check to make sure an active thread on nMistyTurnTo isn’t already…if so, we just let it continue and disregard the call. Since we’ll be monitoring our turn with a setInterval() call, we just make sure it’s clear first:

if(nCheckHeadingINT){ return; }

Secondly, we bring in some previously calculated variables that’ll help control the turn:

var angularVel = 40; // default turn speed (0-100)
var nAvgDegPerSec = 36; // how quickly Misty turns and used for timeouts, calculated manually on a hard surface
var nCurrHeading = currYaw.toFixed(0); // from our global listener

In the angularVel variable, we use 40% of the total available angular velocity on Misty as our default maximum turning speed. Faster than that and it’s likely we’ll over-turn a bit before our listener catches up with our current yaw. 40% is still a pretty fast turn nonetheless.

In the nAvgDegPerSec variable, we pass in the average degrees Misty turns per second for that angular velocity. In other words, to turn 180 degrees at 40% angular velocity, it takes about 5 seconds, or 180 / nAvgDegPerSec. We use this variable to calculate that turn time so our function can timeout safely if we exceed it and try turning again. We can also pass this in our REST request to Misty so she knows how long she’ll be turning.

For the nCurrHeading variable, we set it with the current yaw we receive from our IMU subscription directly from Misty, rounded to the nearest integer.

Next we get the amount of degrees we’ll be turning and set it into nAngleDiff. We’ll use this amount later for better control of the turn:

var nAngleDiff = 180 - Math.abs(Math.abs(nCurrHeading - nHeading) - 180);

In the next part, we give Misty much better accuracy when making smaller turns by slowing down the angular velocity (and subsequently the average degrees turned per second):

if(nAngleDiff<=90){ angularVel = angularVel/2; nAvgDegPerSec = nAvgDegPerSec/2; }
if(nAngleDiff<=45){ angularVel = angularVel/2; nAvgDegPerSec = nAvgDegPerSec/2; }

Finally, we quickly calculate the turn time, in milliseconds, that we talked about before:

var nTurnTimeMS = (nAngleDiff/nAvgDegPerSec)*1000;

After making sure that we actually need to turn by comparing our current heading to the requested one, we get ready to make our REST call with a few final items.

First, we cut short our larger turns (anything over 90 degrees) by 500 milliseconds which will let us divide that turn into two parts, one faster and the second slower, so that they can be as precise as possible.

if(nAngleDiff>90){ nTurnTimeMS-=500; }

Then we decide if we’ll be turning clockwise or counter-clockwise for this turn. If we’re going counter-clockwise, we just use the additive inverse…in other words, we add a minus sign in front of it:

if(nCurrHeading>nHeading && Math.abs(nCurrHeading-nHeading)<180){ angularVel = -angularVel; }

Let’s put it all together now and get it ready to send to Misty:

var nMoveData = "{ \"LinearVelocity\": 0, \"AngularVelocity\": "+angularVel+", \"TimeMS\": "+nTurnTimeMS+" }";

We leave the LinearVelocity at 0 since we’re not moving forward and the rest you already know about.

Now, we’re ready to turn (finally)! Within a try/catch statement, we setup our “drive/time” post command to our fetchClient and send our previously set nMoveData to begin the turn:

try {
	fetchClient = new FetchClient(ip, 10000);
	fetchClient.PostCommand("drive/time",nMoveData, function(){
		// monitor the turn
	});
} catch(e) {
	if(typeof failCallback == "function"){ failCallback(); }
}

In our catch statement, we simply call the failCallback function, if set, from the original function call.

The third parameter of the “drive/time” post command gets called back at the moment the turn begins and will allow us to monitor the turn in realtime.

For our purposes, we’ll monitor the current heading of Misty while she turns to make sure she stops when she reaches the desired heading, or a larger timeout, whichever comes first. We’ll monitor that heading every 10 milliseconds (the same amount as our debounce from the IMU…any more than that would be wasting resources) using a variable called nStep against an nCounter variable along with our timeout stored in nTimeoutMS:

var nCounter = 0, nTimeoutMS = (timeMS*1.5), nStep = debounceMSIMU;
nCheckHeadingINT = setInterval(function(){
	...
},nStep);

Within the interval, we check the current heading as reported by the IMU subscription:

nCurrHeading = currYaw.toFixed(0);

We step the counter:

nCounter++;

Then we check to see if we reached our heading, plus or minus two degrees:

if(Math.abs(nCurrHeading-nHeading)<=2){ // if we match heading, plus minus 2 degrees
	clearInterval(nCheckHeadingINT); nCheckHeadingINT = "";
	nStopMisty();
	if(typeof successCallback == "function"){ successCallback(); }
	return;
}

…and if so, we clear out our interval from our previous setInterval() call to stop monitoring, we stop Misty dead in her tracks with a call to nStopMisty() and before breaking out of our function, we call call the successCallback function, if set, from the original function call.

If we haven’t reached our heading yet, we check to see if we’ve timed out yet:

if(nCounter>(nTimeoutMS/nStep)){
	clearInterval(nCheckHeadingINT); nCheckHeadingINT = "";
	nStopMisty();
	nMistyTurnTo(nHeading,successCallback,failCallback); // try again
	return;
}

…and if so, we again clear out our interval from our previous setInterval() call to stop monitoring, we physically stop Misty, and we try turning again.

The only thing we’re missing now is the nStopMisty() function we called previously. Not to worry, it’s incredibly simple:

function nStopMisty(){
	try {
		fetchClient = new FetchClient(ip, 10000);
		fetchClient.PostCommand("drive/stop", null);
	} catch(e){}
}

Summary
Hopefully you found this writeup and code useful…I’m still hopeful that the Misty team will add a dedicated “turn to heading” function in their JS and REST APIs, but even if they do, you may still need to reference what I described here if you need a success callback.

If you find this useful, or if you have any suggestions to optimize the code in any way, please leave a comment. I’d love to hear your feedback.

3 Likes

thanks for your code and explanation!

do you have experimental data to evaluate accuracy of your proposed “turn to heading” algorithm?

I’ve been testing the function voraciously over the last 2 days, tweaking the turn velocities and timeouts to get them as close to perfect as possible. Accuracy is within +/- 2 degrees (on purpose) every time and Misty usually is pretty quick with the turns.

I’ll be continuing to test and tweak the code a bunch soon since this function is an integral part of a larger Misty skill I’m building alongside my Moving Map. I’ll also post an updated version of that relatively soon in this community and you can test it out online yourself with your own robot, no coding needed.

4 Likes

Great to see this @michaelrod. The team has been discussing ways to improve the APIs for Misty’s “turn-and-drive-in-x-direction” functionality, and I would expect to see some improvements there in the months to come. Good to see your implementation, both to inform decisions in design and to understand the details of the workarounds required for the software in its current state.

Thanks @johnathan…glad the team still has more to come in that regard. The “Turn to heading” feature seemed basic enough that I was surprised it didn’t already exist, but as you can see from my code, it can be done with relative ease.

The only real issue I had in putting that together was the tendency for Misty to overshoot the turn, so that’s why I opted for keeping any turn under 90 degrees at 20% velocity or less and splitting larger turns in two phases. If you or the team have any better suggestions based on your experience, I’d love to hear them. Thanks again!

For now I can just offer a similar implementation of your “turn-to-heading” REST application that I’ve sometimes used in JavaScript skills. In this snippet, the turn function uses the yaw reading from Misty’s IMU to convert a relative heading into an absolute heading that is passed into the misty.DriveArc() method. When it runs, Misty should always rotate the same amount, no matter which direction she is facing when the skill starts or which direction she was facing when she booted up & her IMU initialized to yaw = 0.

misty.Set("currentYaw", "0");

// Turns 180 degrees to the left, in 45 degree increments
turn(45);
misty.Pause(3000);
turn(45);
misty.Pause(3000);
turn(45);
misty.Pause(3000);
turn(45);
misty.Pause(3000);

// Turns 180 degrees to the right, in 90 degree increments
turn(-90);
misty.Pause(3000);
turn(-90);


// Gets current yaw
function getCurrentYaw() {
    misty.AddReturnProperty("getCurrentYaw", "yaw");
    misty.RegisterEvent("getCurrentYaw", "IMU", 10, false);
}

// IMU event callback, use to update currentYaw
function _getCurrentYaw(data) {
    var currentYaw = data.AdditionalResults[0];
    // Save current yaw
    misty.Set("currentYaw", currentYaw.toString())
    misty.Debug("currentYaw = " + misty.Get("currentYaw"))
}

// Turns Misty to relative heading, instead of using absolute heading.
function turn(relativeHeading) {
    
    getCurrentYaw();
    misty.Pause(1000); // Pause to let IMU event trigger the callback

    // Converts relativeHeading to absolute heading for DriveArc command
    var heading = parseInt(misty.Get("currentYaw")) + relativeHeading;
    if (heading > 360) {
        heading = heading - 360
    }

    // Turnabout Misty!
    misty.DriveArc(heading, 0, 10, false);
}
2 Likes