// IDE 0022
//-------------
// nicoo
// april 2013
//-------------
// servo calibration - magic numbers
//
// - send numbers through serial console to determine 
// max and min values accepted by servo
// - commmand 'A' selects servo #0
// - commmand 'B' selects servo #1
// ...
// - commmand 'F' selects servo #5
//-------------------------------------------------------------

int useless_variable_to_force_ide_to_insert_its_lines_here = 0;

#include <Servo.h>

//#define ALL_SERVOS
#ifdef ALL_SERVOS
const int NUM_SERVO = 6;
const int SERVO_PIN[NUM_SERVO] = { 11, 10, 9, 6, 5, 3 }; // PWM pins on arduino 328
int pulse[NUM_SERVO]= { 1500, 1500, 1500, 1500, 1500, 1500 };
#else
const int NUM_SERVO = 2;
const int SERVO_PIN[NUM_SERVO] = { 9, 6 }; // PWM pins on arduino 328
int pulse[NUM_SERVO]= { 1500, 1500 };
#endif

Servo servo[NUM_SERVO];
int current = 0;

const int SERVO_MAX = 3600; // absolute max
const int SERVO_MIN = 300; // absolute min

void setup()   
{
  int index;
  for(index=0; index < NUM_SERVO; index++)
    {
    servo[index].attach(SERVO_PIN[index]);
    pinMode(SERVO_PIN[index], OUTPUT);
    }
  Serial.begin(115200);
}

void loop()
{
  // read number or command ('A','B',... 'F') on serial port.
  if (Serial.available()>0) 
    {
    int ReceivedNumber=0;
    while (Serial.available()>0) 
      {
      int ReceivedByte= Serial.read(); // read one byte
      if ((ReceivedByte>='0')&&(ReceivedByte<='9'))
        ReceivedNumber = (ReceivedNumber*10)+(ReceivedByte-'0');
      if ((ReceivedByte>='A')&&(ReceivedByte<='Z'))
        current = ReceivedByte - 'A';
      delay(1);
      }
    if (ReceivedNumber == 0)
      return;
    // done reading.

    Serial.print ("Received number = ");
    Serial.println(ReceivedNumber);
  
    // position 'current' servo:
    pulse[current]=ReceivedNumber;
    if (pulse[current]>SERVO_MAX) pulse[current]=SERVO_MAX;
    if (pulse[current]<SERVO_MIN) pulse[current]=SERVO_MIN;
    servo[current].writeMicroseconds(pulse[current]);
    // done positioning

    Serial.print ("Send pulse of ");
    Serial.print(pulse[current]);
    Serial.println (" microseconds ");

    delay (1000);
    }
}