Arduino Code: Basic Servo Control Function

Here is a first iteration of a function that can be used to control the position of a servo. The user may user the arduino serial interface or some other serial program to send a position to the arduino and have the servo move to that position.

The code is below

#include <Servo.h> 

Servo baseServo;  
Servo shoulderServo;  
Servo wristServo; 

int command;
int pos = 0;    // variable to store the servo position 

//+++++++++++++++FUNCTION DECLARATIONS++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo);

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void setup() 
{ 
  Serial.begin(9600);
  baseServo.attach(9);  // attaches the servo on pin 9 to the servo object 
  shoulderServo.attach(10);
  wristServo.attach(11);
  Serial.setTimeout(50);
} 

void loop() 
{ 
  if (Serial.available()){
    command = Serial.parseInt();  
    Serial.println(command); 
    servoControl(command, 15, baseServo);
    servoControl(command, 15, shoulderServo);
    servoControl(command, 15, wristServo);    

  }
}

//++++++++++++FUNCTION DEFINITIONS+++++++++++++++++++++++
void servoControl (int thePos, int theSpeed, Servo theServo){
  Serial.println("in function");
  //read the current pos
  int startPos = theServo.read();
  int newPos = startPos;
  
  //define where the pos is with respect to the command
  // if the current position is less that the actual move up
  if (startPos < thePos){
        while (newPos < (thePos - 5)){
          newPos = newPos + 5;
          theServo.write(newPos);
          delay(theSpeed);
        }    
  }

  else{
       while (newPos > (thePos + 5)){
         newPos = newPos - 5;
         theServo.write(newPos);
         delay(theSpeed);
       }     
  }
}