[Sketch] Dead Reckoning

Description of Sketch
Dead Reckoning
A set of routines to give Sparki some positional awareness. By using these routines instead of the standard Sparki move commands the sketch maintains x & y coordinates, in cm. The direction Sparki is facing, which I call heading, is also maintained. Any interference with movement such as picking up Sparki or the wheels slipping will throw off the coordinates and heading.

As a demo, Sparki will make a series of random moves based on turn angles and distances and then return to home and rotate to the original heading(more or less). Then a second set of random moves are made by sending Sparki to random coordinates. Sparki will return to home again, this time moving backward.

Variables
xPos, yPos, heading

Functions:
trackTurnLeft(degrees)
trackTurnRight(degress)
trackTurnToHeading(degrees)
trackMoveForward(cm)
trackMoveBackward(cm)
trackMoveTo(x,y)
trackMoveBackwardTo(x,y)

Sparki Components Utilized
Wheels; RGB; Beep

Sparki.h and Sparki.cpp Version Dependencies
None

Notes
While returning home gets pretty close, there are minor variations. If the loop keeps repeating these will build up. At the start of each loop Sparki resets it’s coordinates and heading to 0. The RGB will flash red and then go yellow for 2 seconds at the start of each loop. During this time you can pick up Sparki and reposition it. Reset or turning Sparki off and on works also.

If distance accuracy is needed there should be a distance adjustment factor in the trackMoveForward() and trackMoveBackward() routines.

I have a fairly small surface, about 3 ft x 2 ft, for Sparki to run on so the random distances and angles are pretty tame. Feel free to adjust for your available space.

Use a piece of tape or a sticky note to mark the home position without disturbing movement.

I developed this independently but not too surprisingly it has a lot of similarities to Ingemar’s Draw This! sketch found here: [Sketch] Draw This! Turns drawing into Sparki moves.

[code]/*******************************************
Dead_Reckoning by Zax 2014-03-01
Demo of routines to track Sparki’s location
on an x,y coordinate system
Movement and turning must be made through
the tracking routines
Any interference with Sparki’s movements
invalidates the location.

uses variable xPos, yPos and heading to
keep track of Sparki’s location and direction

Functions:
trackTurnLeft(degrees)
trackTurnRight(degress)
trackturnToHeading(degrees)
trackMoveForward(cm)
trackMoveBackward(cm)
trackMoveTo(x,y)
trackMoveBackwardTo(x,y)

Sparki will make a series of random moves
and then return to it’s original position
********************************************/
#include <Sparki.h> // include the sparki library

/*******************************************
Angle adjustment factors to correct for
Sparki variations
********************************************/
const float angleAdjLeft = 1.055;
const float angleAdjRight = 1.055;

float xPos = 0.0;
float yPos = 0.0;
float heading = 0;

void setup() // code inside these brackets runs first, and only once
{
sparki.servo(SERVO_CENTER); // Position servo forward
randomSeed(sparki.magX()); // initialize random seed
sparki.RGB(0,0,0); // Turn off RGB
}

void loop() // code inside these brackets runs over and over forever
{
// Reset home position and heading at the start of each loop
heading = 0;
xPos = 0;
yPos = 0;

// “Home” - delay to allow Sparki
// to be moved, resetting the home position
// Blink the RGB in red
for(int i = 0; i < 7; i++)
{
sparki.RGB(RGB_RED);
delay(700);
sparki.RGB(RGB_OFF);
delay(100);
}
// RGB Yellow for a 2 seconds as final warning
sparki.RGB(50,80,0);
delay(2000);

// “Exploring” Phase 1 Make a series of random moves
// using turn diections and distance
sparki.RGB(RGB_GREEN);
sparki.beep(); // Beep once for phase 1
int numMoves = random(3,5);
for (int i = 0; i < numMoves; i++)
{ float dir = random(-60,60);
int dist = random(6,14);
if (dir < 0)
{
trackTurnLeft(dir * -1);
}
else
{
trackTurnRight(dir);
}
trackMoveForward(dist);
}

//Return to home
sparki.RGB(RGB_BLUE);
trackMoveTo(0,0);
// Turn Sparki to face “forward” 0 degrees
trackTurnToHeading(0);
sparki.RGB(RGB_OFF);
delay(1000);

// “Exploring” Phase 2 Make a series of random moves
// using random coordinates
sparki.RGB(0,50,50); // Aqua?
sparki.beep(); // Beep twice for phase 2
delay(500);
sparki.beep();

numMoves = random(3,5);
for (int i = 0; i < numMoves; i++)
{
trackMoveTo(random(-20,20),random(-30,30));
}

//Return to home - backwards
sparki.RGB(RGB_BLUE);
trackMoveBackwardTo(0,0);
// Turn Sparki to face “forward” 0 degrees
trackTurnToHeading(0);

}

void trackTurnLeft(float angle)
{
// 0 makes sparki turn until stopped,
// bad for tracking
if (angle != 0)
{
float turnAngle = angle;
heading = heading - turnAngle;
sparki.moveLeft(turnAngle * angleAdjLeft);
}
}

void trackTurnRight(float angle)
{
// 0 makes sparki turn until stopped,
// bad for tracking
if (angle != 0)
{
float turnAngle = angle;
heading = heading + turnAngle;
sparki.moveRight(turnAngle * angleAdjRight);
}
}

void trackMoveForward(int cm)
{
xPos = xPos + cm * sin(heading * PI / 180);
yPos = yPos + cm * cos(heading * PI / 180);
sparki.moveForward(cm);
}

void trackMoveBackward(int cm)
{
xPos = xPos - cm * sin(heading * PI / 180);
yPos = yPos - cm * cos(heading * PI / 180);
sparki.moveBackward(cm);
}

void trackTurnToHeading(float angle)
{
// Calc degrees needed for new heading
float turnAngle = angle - heading;
// Convert negative angles and make sure
// angles are between 0 & 360
while(turnAngle < 0)
{
turnAngle += 360;
}
while(turnAngle > 360)
{
turnAngle -= 360;
}

if (turnAngle != 0)
{
if(turnAngle < 180)
{
trackTurnRight(turnAngle);
}
else
{
trackTurnLeft(360 - turnAngle);
}
}
heading = angle;
}

void trackMoveTo(float x, float y)
{
float newHeading = atan2(x - xPos, y - yPos) * 180 / PI;
trackTurnToHeading(newHeading);
float distance = sqrt((x - xPos)* (x - xPos) +(y - yPos)* (y - yPos));
trackMoveForward(distance);
}

void trackMoveBackwardTo(float x, float y)
{
float newHeading = atan2(x - xPos, y - yPos) * 180 / PI;
newHeading += 180;
trackTurnToHeading(newHeading);
float distance = sqrt((x - xPos)* (x - xPos) +(y - yPos)* (y - yPos));
trackMoveBackward(distance);
}[/code]