//Here is my Arduino Uno Sketch for RoboRealm using 4 Ping Ultrasonic Sensors
//And 4 Servos. (they're my robot's bumpers)

//I wanted to send multiple commands to the Arduino in one go,
//with the minimum being at least two commands at the same time.
//This is so that I can move a servo by calling the servo and giving it a value.
//I also wanted to be able to turn on and off individual Ping Sensors.
//The way I found - was to make a string in Roborealm's VB Script module and send it
//with the Serial module whenever it's changed.  The problem was that I needed
//Integer values out of that string.  So I converted the String into
//smaller character arrays then the character arrays into integers.


//The Arduino is looking for a String of commands that look like this:
//111 180,222 90,333 0,501 0,
//The first number is a command for which servo to use
//The second number is a value to move that servo to in degrees 0 - 180
//then a comma to make the second part of the string a different command.
//the 501 turns on Ping Sensor One.. you'll see it all in the switch case statement

//If you need to, you can reach me at spacehermit@gmail.com
//created: 02_12_11
//btw this is my first Arduino Sketch but it does the minimum of what I need.
 

//Two Quick Notes:
//First- Ping Sensors are on pins 4,5,6,7  Servos are on pins 3,9,10,11

//Second-
  //In Roborealm's Serial module I am expecting the ping results to look like this:
  
  //[InchesOne]in,<sp>[CentimetersOne]cm[InchesTwo]in,<sp>[CentimetersTwo]cm[InchesThree]in,
  //<sp>[CentimetersThree]cm[InchesFour]in,<sp>[CentimetersFour]cm<cr><lf>
  
  // Eight Variables, All One Line




#include <WString.h>              //provides easy string handling
#include <stdio.h>
#include <stdlib.h> 
#include <Servo.h> 
 
String readString = String(100);  //stores the entire string up to a comma
String parce1 = String(10);       //first part of the string up to a space
String parce2 = String(10);       //second part of the string
int pos = 0;                      //stores the length of the string
int ind1 = 0;                     //position of space in string

                                  //my commands and values are 3 digit numbers
char elstringoUno[4];             //character array of the first part of the string
char elstringoDos[4];             //character array of the second part of the string

                  // create servo object to control a servo 
Servo myservoOne;
Servo myservoTwo;
Servo myservoThree;
Servo myservoFour;

const int ping1Pin = 4;  //puts PingOne on pin 4
const int ping2Pin = 5;  //puts PingTwo on pin 5
const int ping3Pin = 6;  //puts PingThree on pin 6
const int ping4Pin = 7;  //puts PingFour on pin 7

int PingOne = 0;   //turns ping1 on or off
int PingTwo = 0;   //turns ping2 on or off
int PingThree = 0; //turns ping3 on or off
int PingFour = 0;  //turns ping4 on or off

 
void setup() 
{ 

    Serial.begin(115200);

    myservoOne.attach(3);     //Puts ServoOne on pin 3
    myservoTwo.attach(9);     //Puts ServoTwo on pin 9
    myservoThree.attach(10);  //Puts ServoThree on pin 10
    myservoFour.attach(11);   //Puts ServoFour on pin 11

} 
 
 
void loop() 
{ 

      
  //expect a string like: 111 180,222 90,501 180,
  //where first number is a command then a space then second number is a value then a comma
  //this worked best for me cause I'm simply moving certain servos 
  //and turning certain ping sensors On or Off

  while (Serial.available()) {
        delay(10);  
    	  if (Serial.available() >0) {
            char c = Serial.read();  //gets one byte from serial buffer


            //if it's a comma then don't add to string and skip ahead
  
            if (c == ',') { goto parce;}  

              //only happens till there is a comma
              readString = readString + c;  //puts the char at end of the string
            } //end of if statement

          } //end of if statement


     parce:  
      if (readString.length() >0) {
      
        pos = readString.length(); //capture string length
        ind1 = readString.indexOf(' '); //position of the space
        parce1 = readString.substring(0, ind1); //first part of string
        parce2 = readString.substring(ind1+1, pos); //second part of string

        readString="";  //empties the readString for the next command

        parce1.toCharArray(elstringoUno, 4);  //turns 1st part of string into char array
        parce2.toCharArray(elstringoDos, 4);  //turns 2nd part of string into char array

        int kommand, tvalue;  //will be the number of the command and value sent


        kommand = atoi(elstringoUno);  //turns the 1st char array into an integer

        tvalue = atoi(elstringoDos);   //turns the 2nd char array into an integer



    switch (kommand)
    {
                
      case  111:      //Move myServoOne to a value 

            myservoOne.write(tvalue);
      break;
      case  222:      //Move myServoTwo to a value

            myservoTwo.write(tvalue);
      break;
      case  333:      //Move myServoThree to a value

            myservoThree.write(tvalue);
      break;
      case  444:      //Move myServoFour to a value

            myservoFour.write(tvalue);
      break;
      case  500:      //Turn Off PingOne

            PingOne = 0;
      break;
      case  501:      //Turn On PingOne

            PingOne = 1;
      break;
      case  600:      //Turn Off PingTwo

            PingTwo = 0;
      break;
      case  601:      //Turn On PingTwo

            PingTwo = 1;
      break;
      case  700:      //Turn Off PingThree

            PingThree = 0;
      break;
      case  701:      //Turn On PingThree

            PingThree = 1;
      break;
      case  800:      //Turn Off PingFour

            PingFour = 0;
      break;
      case  801:      //Turn On PingFour

            PingFour = 1;
      break;

    } //end of switch statement

  } //end of if statement

  // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
 

//long durationOne, inchesOne, cmOne, durationTwo, inchesTwo, cmTwo, durationThree, inchesThree, //cmThree, durationFour, inchesFour, cmFour;


//Sets default values from the Ping Sensors to 999 
//so I can continue driving while they're turned off

long durationOne;
long inchesOne = 999;
long cmOne = 999;


long durationTwo;
long inchesTwo = 999;
long cmTwo = 999;


long durationThree;
long inchesThree = 999;
long cmThree = 999;


long durationFour;
long inchesFour = 999;
long cmFour = 999;


  if(PingOne == 1) //Turns On PingOne and returns inches and centimeters
   {

    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(ping1Pin, OUTPUT);
    digitalWrite(ping1Pin, LOW);
    delayMicroseconds(2);
    digitalWrite(ping1Pin, HIGH);
    delayMicroseconds(5);
    digitalWrite(ping1Pin, LOW);

    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(ping1Pin, INPUT);
    durationOne = pulseIn(ping1Pin, HIGH);

    // convert the time into a distance
    inchesOne = microsecondsToInches(durationOne);
    cmOne = microsecondsToCentimeters(durationOne);

   }
  if(PingTwo == 1) //Turns On PingTwo and returns inches and centimeters
   {

    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(ping2Pin, OUTPUT);
    digitalWrite(ping2Pin, LOW);
    delayMicroseconds(2);
    digitalWrite(ping2Pin, HIGH);
    delayMicroseconds(5);
    digitalWrite(ping2Pin, LOW);

    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(ping2Pin, INPUT);
    durationTwo = pulseIn(ping2Pin, HIGH);

    // convert the time into a distance
    inchesTwo = microsecondsToInches(durationTwo);
    cmTwo = microsecondsToCentimeters(durationTwo);

   }
  if(PingThree == 1) //Turns On PingThree and returns inches and centimeters
   {

    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(ping3Pin, OUTPUT);
    digitalWrite(ping3Pin, LOW);
    delayMicroseconds(2);
    digitalWrite(ping3Pin, HIGH);
    delayMicroseconds(5);
    digitalWrite(ping3Pin, LOW);

    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(ping3Pin, INPUT);
    durationThree = pulseIn(ping3Pin, HIGH);

    // convert the time into a distance
    inchesThree = microsecondsToInches(durationThree);
    cmThree = microsecondsToCentimeters(durationThree);

   }
  if(PingFour == 1) //Turns On PingFour and returns inches and centimeters
   {
    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(ping4Pin, OUTPUT);
    digitalWrite(ping4Pin, LOW);
    delayMicroseconds(2);
    digitalWrite(ping4Pin, HIGH);
    delayMicroseconds(5);
    digitalWrite(ping4Pin, LOW);

    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(ping4Pin, INPUT);
    durationFour = pulseIn(ping4Pin, HIGH);

    // convert the time into a distance
    inchesFour = microsecondsToInches(durationFour);
    cmFour = microsecondsToCentimeters(durationFour);

  }

  //In Roborealm's Serial module I am expecting the ping results to look like this:
  //[InchesOne]in,<sp>[CentimetersOne]cm[InchesTwo]in,<sp>[CentimetersTwo]cm[InchesThree]in,
  //<sp>[CentimetersThree]cm[InchesFour]in,<sp>[CentimetersFour]cm<cr><lf>
  //Eight Variables, All One Line 
 
  Serial.print(inchesOne);
  Serial.print("in, ");
  Serial.print(cmOne);
  Serial.print("cm");

  Serial.print(inchesTwo);
  Serial.print("in, ");
  Serial.print(cmTwo);
  Serial.print("cm");

  Serial.print(inchesThree);
  Serial.print("in, ");
  Serial.print(cmThree);
  Serial.print("cm");

  Serial.print(inchesFour);
  Serial.print("in, ");
  Serial.print(cmFour);
  Serial.print("cm");


  Serial.println();
  
  delay(100);



} //end of void loop










long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}
