[Sketch] Draw This! Turns drawing into Sparki moves.

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

  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.
/* 
 * 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));
}

Thank you for sharing. I really wanted to make Sparki write and now he does. Thanks again!

I appear to have reinvented the wheel somewhat, but figure that I might as well share what I’ve got: robcook.eu/sparki/sparki-sends-his-love/