Feature Request: Go to a position in a coordination system.

[TechniclabErdmann]

Hey dev team,

I started to develop a functionality to move sphero to a defined waypoint. You can integrate this feature in your sdk.

There are two open tasks:

• calculate time for moving and callback for "timeout" function. Maybe there is a better solution?
• before first moving, sphero has to align to Earth's magnetic field. So the coord system is the same everytime running the code.

Function:

function goToPosition(pos, x, y, speed) {
    last_angle = 0;
    var locationInterval = setInterval(function() {
        orb.readLocator(function(err, data) {
            if (err) {
                console.log("error: ", err);
            } else {
                var xcomp = (x - data.xpos == 0) ? 1 : (x - data.xpos);
                var ycomp = (y - data.ypos == 0) ? 1 : (y - data.ypos);

                var angle = Math.atan(ycomp / xcomp);
                angle = angle * 180 / Math.PI;
          
                if(angle < 90) {
                    angle = 90 - angle;
                }
                if((x - data.xpos) < 0) {
                    angle = angle + 180;
                }

                console.log("  --" + pos + "--");
                console.log("  xpos:", data.xpos);
                console.log("  ypos:", data.ypos);
                console.log("  xerror:", x - data.xpos);
                console.log("  yerror:", y - data.ypos);
                console.log("  angle:", angle   );

                orb.roll(speed, angle);

                // positon arrived
                if(((angle < 0) || (Math.abs(angle - last_angle) > 50)) && (last_angle != 0)) {
                    clearInterval(locationInterval);
                    orb.stop();
                    console.log("Position arrived!");
                }
                last_angle = angle;
            }
    });
    }, 500);
}

An example use case:

var sphero = require("sphero"),
orb = sphero("EE:75:8E:48:D5:6D"); // change BLE address accordingly
var last_angle = 0;

/* place function goToPostion() {...} here */

orb.connect(function() {
    var opts = {
        flags: 0x01,
        x: 0x0000,
        y: 0x0000,
        yawTare: 0x0
    };

console.log("::START CALIBRATION::");
orb.startCalibration();
setTimeout(function() {
    console.log("::FINISH CALIBRATION::");
    orb.finishCalibration();

    orb.setBackLed(255);
    orb.configureLocator(opts);
    orb.color({ red: 255, green: 0, blue: 0 });

    goToPosition("toPos1", 0, 350, 50);
    setTimeout(function() {
        goToPosition("toPos2", 200, 350, 50);

    setTimeout(function() {
        goToPosition("toPos3", 200, 100, 50);

    setTimeout(function() {
        goToPosition("toPos4", 5, 5, 50);
    }, 10000);
    }, 10000);
    }, 10000);
}, 5000);
});

I'm happy about feedback the the current code and help for the open tasks. Next step will be to enable communication between to spheros for simulation swarm intelligence. Feel free to contact me if you want to support.

Bye,
Niklas