Description of Sketch
Moves Sparki according to SVG path formatted strings
Sparki Components Utilized
Pencil holder; Wheels; LCD; LED; Beep!
Sparki.h and Sparki.cpp Version Dependencies
Tested on 1.0.5.4-c.
Notes
Comes pre-loaded with a poor attempt on the ‘S’ from the Sparki logo, here’s a lot of room for improvement!
Two trim multipliers are used to get correct turn and move of Sparki, these works reasonably well on my Sparki, but your mileage may vary.
Sketch is heavily commented for learning purposes (due to complexity and length of code it’s not suitable for beginners though)
Instructions to create, store and draw new drawing
- Use SVG-edit (free web-based tool) to draw a path by using the path tool,
go to http://svg-edit.googlecode.com/svn/branches/2.6/editor/svg-editor.html
- Click save (‘s’) or Edit source (‘e’) to display the SVG code.
- Locate the “<path”-tag then find and copy the whole string following the “-d=” characters (it should start with ‘m’, if it’s a closed path it ends with ‘z’)
- Paste the whole string in to the sparki_path_1 array below, replacing the current one.
- Make sure that the strings only contains numbers and the supported characters ‘m’,‘l’,‘c’ and ‘z’ (space, minus and comma characters are also supported).
- Upload the sketch to your Sparki and position the Sparki upwards from you.
- Sparki will move through the path, if the LED turns RED it means somethings gone wrong with the parsing of the path string.
- Create multiple paths by pasting to sparki_path_2 and sparki_path3. You can even add more by defining more path, remember to add them to the sparki_paths array as well.
/*
* TITLE: Sparki: Draw This!
* BY: Ingemar Persson (Ingemar on the Sparki forum)
* VERSION: 1.0
*
* DESCRIPTION:
* Moves Sparki according to SVG path formatted string(s)
* Developed and tested using SparkiDuino 1.0.5.4-c
* INSTRUCTIONS:
* 1. Use SVG-edit (free web-based tool) to draw a path by using the path tool, go to http://svg-edit.googlecode.com/svn/branches/2.6/editor/svg-editor.html
* 2. Click save ('s') or Edit source ('e') to display the SVG code.
* 3. Locate the "<path"-tag then find and copy the whole string following the "-d=" characters (it should start with 'm', if it's a closed path it ends with 'z')
* 4. Paste the whole string in to the sparki_path_1 array below, replacing the current one.
* 5. Make sure that the strings only contains numbers and the supported characters 'm','l','c' and 'z' (space, minus and comma characters are also supported).
* 6. Upload the sketch to your Sparki and position the Sparki upwards from you.
* 7. Sparki will move through the path, if the LED turns RED it means somethings gone wrong with the parsing of the path string.
* 8. Create multiple paths by pasting to sparki_path_2 and sparki_path3. You can even add more by defining more path, remember to add them to the sparki_paths array as well.
*
* Note: Two trim multipliers are used to get correct turn and move of Sparki, these works reasonably well on my Sparki, but your mileage may vary.
*/
#include <string.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include <Sparki.h>
// First Path of Sparki, in SVG Path format
prog_char sparki_path_1[] PROGMEM = {
"m193,109l-24,-43l-96,62l-3,18l6,13l20,15l101,64l2,14l-18,11l-113,6l-9,28l136,6l42,-19l14,-37l-15,-37c0,0 -93,-63 -93,-63c0,0 4,-24 4,-24c0,0 46,-14 46,-14z"
};
// Second path
prog_char sparki_path_2[] PROGMEM = {
"m0,0l0,0l"
};
// Third path
prog_char sparki_path_3[] PROGMEM = {
"m0,0l0,0l"
};
// Defines the SVG paths that should be drawn and their order
PROGMEM const char *sparki_paths[] =
{
sparki_path_1,
// sparki_path_2, // Uncomment start of this line to use second path
// sparki_path_3, // Uncomment start of this line to use third path
};
// Multiplier to correct the rotation of Sparki
const double rotation_trim_multiplier = 1.083;
// Multiplier to correct the forward movement of Sparki (Note also does conversion from mm to cm)
const double move_trim_multiplier = 0.136;
// Macro to display coordinates on LCD (can be used for debugging)
#define DISPLAY_COORDINATE(p,i,x,y)\
sparki.print(p); sparki.print(","); sparki.print(i); sparki.print(": ("); sparki.print(x); sparki.print(","); sparki.print(y); sparki.println(")");\
sparki.updateLCD();
// Type to hold Sparkis current and next position
struct sparki_position_type
{
int x;
int y;
};
void setup()
{
// Read index to path string, keeps track of where we are in the string
int path_string_index = 0;
// Length of path string
size_t path_string_length;
// Sparkis current position
struct sparki_position_type sparki_position;
// Next position in path
struct sparki_position_type next_position;
// Sparki heading, init to 90 degrees (pointing upwards)
float sparki_heading = 90;
// Index to sparki_paths
int i;
// Init Sparki components
sparki.RGB(RGB_GREEN);
sparki.servo(0);
sparki.clearLCD();
// Wait a while before beginning
delay(5000);
// Get first coordinate from first path, it will be Sparkis initial coordinate
(void)sparki_get_next_coordinate((PGM_P)pgm_read_word(&(sparki_paths[0])), 0, &sparki_position);
// Go through all paths in sparki_paths
for(i = 0; i < sizeof(sparki_paths)/(sizeof(char *)); i++)
{
// Beep to indicate start of new path
sparki.beep();
// Get length of path string
path_string_length = strlen_P((PGM_P)pgm_read_word(&(sparki_paths[i])));
// Retrive next path starting coordinate
path_string_index = sparki_get_next_coordinate((PGM_P)pgm_read_word(&(sparki_paths[i])), 0, &next_position);
// Break loop if somethings gone wrong when reading string
if(path_string_index < 0)
break;
// Go to start of path, this is an absolute coordinate so diff between sparkis coordinate is needed
next_position.x -= sparki_position.x;
next_position.y -= sparki_position.y;
DISPLAY_COORDINATE(i, path_string_index, next_position.x, next_position.y);
sparki_heading = sparki_go_to_next_coordinate(sparki_heading, next_position);
// Update Sparkis position
sparki_position.x += next_position.x;
sparki_position.y += next_position.y;
// Go to next coordinate in path until we get to the end of path string
while((path_string_index > 0) && (path_string_index+2 < path_string_length))
{
// Get next position
path_string_index = sparki_get_next_coordinate((PGM_P)pgm_read_word(&(sparki_paths[i])), path_string_index, &next_position);
// Break loop if somethings gone wrong when reading string
if(path_string_index < 0)
break;
// Go to next coordinate
DISPLAY_COORDINATE(i, path_string_index, next_position.x, next_position.y);
sparki_heading = sparki_go_to_next_coordinate(sparki_heading, next_position);
// Update Sparkis position
sparki_position.x += next_position.x;
sparki_position.y += next_position.y;
}
// Break loop if somethings gone wrong when reading string
if(path_string_index < 0)
break;
}
// Turn LED red if somethings gone wrong
if(path_string_index < 0)
sparki.RGB(RGB_RED);
else
sparki.RGB(RGB_OFF);
}
// Do nothing here, everythings already done in setup()
void loop()
{
}
// Returns the next coordinate in the path string and index to it
int sparki_get_next_coordinate(const char* sparki_path, int read_index, struct sparki_position_type *next_position)
{
// Temporary buffer for reading from program memory
char read_buffer[32];
// Pointers to read_buffer, used for finding the coordinates in string
char *start_value_pointer, *end_value_pointer;
// Read 16 characters from path string in program memory
strncpy_P(read_buffer, &sparki_path[read_index], sizeof(read_buffer));
// Insert null terminator to mark end of string, string functions need this
read_buffer[sizeof(read_buffer)-1]='\0';
// Check first character to determine where start is
if(read_buffer[0] == 'l' || read_buffer[0] == 'm')
start_value_pointer = &read_buffer[1];
else if(read_buffer[0] == 'c')
{
start_value_pointer = &read_buffer[1];
// Ignore the first two coordinates in the curve command (curve shape), since it's not supported
for(int i = 0; i < 2; i++)
{
start_value_pointer = strchr(start_value_pointer, ' ');
if(!start_value_pointer)
return -1;
}
start_value_pointer++;
}
else
return -1;
// Get x coordinate value
// Find comma character which is delimiting the x from y value
end_value_pointer = strchr(start_value_pointer, ',');
// If no comma found, somethings wrong, return -1 to indicate fault
if(!end_value_pointer)
return -1;
// Replace comma with null terminate character so that atoi function can be used directly
*end_value_pointer = '\0';
// Convert string characters to integer value
next_position->x = atoi(start_value_pointer);
// Get y coordinate value
// Move start value pointer to end of x value
start_value_pointer = end_value_pointer + 1;
// Assert that the start_value_pointer is within the buffer
if(start_value_pointer >= (read_buffer + sizeof(read_buffer)))
return -1;
// Find 'l', 'c', or 'z' character which is start of next line
end_value_pointer = strpbrk(start_value_pointer, "lcz");
// If no 'l', 'c' or 'z' character found, something might be wrong
if(!end_value_pointer)
{
// Check if we are at end of string, if not then somethings is wrong with string, return -1 to indicate fault
if(read_index+(sizeof(read_buffer)) >= strlen_P(sparki_path))
end_value_pointer = read_buffer + strlen(read_buffer);
else
return -1;
}
// Replace delimiting character with null terminate character so that atoi function can be used directly
*end_value_pointer = '\0';
// Convert string characters to integer value (invert SVG y-axis)
next_position->y = -atoi(start_value_pointer);
// Update read_index to end of coordinates
read_index += end_value_pointer - read_buffer;
return read_index;
}
// Calculates and moves Sparki to the new coordinate, returns new Sparki heading
float sparki_go_to_next_coordinate(float sparki_heading,
struct sparki_position_type next_position)
{
uint16_t distance;
float angle;
// Return directly if no move required
if(next_position.x == 0 && next_position.y == 0)
return sparki_heading;
// Calculate distance and angle to next coordinate
distance = sqrt(sq(next_position.x) + sq(next_position.y));
angle = atan2(next_position.y,next_position.x) * (180.0/PI);
// Turn Sparki to face next coordinate
sparki_turn_to_angle(sparki_heading, angle);
// Go to next coordinate (trim compensates for incorrect move delay and converts from mm to cm)
sparki.moveForward(distance * move_trim_multiplier);
// Return new Sparki heading
return angle;
}
// Calculates how much to turn Sparki given it's current angle and new angle
void sparki_turn_to_angle(float old_angle, float new_angle)
{
float turn_angle = new_angle - old_angle;
// Calculate whether to turn left or right given the angle offset
if(turn_angle == 0)
return;
if(turn_angle > 180)
turn_angle -= 360;
if(turn_angle < -180)
turn_angle += 360;
// Compensate for incorrect turn delay
turn_angle *= rotation_trim_multiplier;
if(turn_angle > 0)
sparki.moveLeft(turn_angle);
else if(turn_angle < 0)
sparki.moveRight(abs(turn_angle));
}