SerialServo

// Sweep
// by BARRAGAN <http://barraganstudio.com>
// This example code is in the public domain.


#include <Servo.h>
#include <stdlib.h>
#include <stdio.h>
#define NUM_SERVOS 1
#define MAX_SERVOS 6
#define OUTBUF_SIZE 100
char outbuf[OUTBUF_SIZE];
const int pins[] = { 3, 5, 6, 9, 10, 11 }; //all the PWM pins on the arduino platform minus the serial console
static Servo * servos[NUM_SERVOS]; // create servo object to control a servo
static Servo * ServoPointer;     // a maximum of eight servo objects can be created


void setup()
{
  for (int i = 0; i < NUM_SERVOS && i < MAX_SERVOS; i++)  //Initializes the servo objects
  {
    ServoPointer = new Servo;
    servos[i] = ServoPointer;
    servos[i]->attach(pins[i]); //actually attach them (in order of the arduino numbering of the PWM pins)
  }
  Serial.begin(115200);

}

void loop()
{
#define INPUT_SIZE 30


// Get next command from Serial (add 1 for final 0)
char input[INPUT_SIZE + 1];
byte size = Serial.readBytes(input, INPUT_SIZE);
// Add the final 0 to end the C string
input[size] = 0;

// Read each command pair
char* command = strtok(input, "&");
while (command != 0)
{
    // Split the command in two values
    char* separator = strchr(command, ':');
    if (separator != 0)
    {
        // Actually split the string in 2: replace ':' with 0
        *separator = 0;
        int servoId = atoi(command); //Parsing code credit to https://arduino.stackexchange.com/questions/1013
        ++separator;
        int position = atoi(separator);

        if ((servoId <= NUM_SERVOS) && !(servoId <1))
        {
          servos[servoId]->write(position);
          sprintf(outbuf, "Moving Servo ID %d to position %d", servoId, position);
          Serial.println(outbuf);
        }else {
          sprintf(outbuf, "Servo ID %d is out of bounds, please make it between 1 and %d", servoId, NUM_SERVOS);
          Serial.println(outbuf);
        }
    }
    // Find the next command in input string
    command = strtok(0, "&");
}
}