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() 

  baseServo.attach(9);  // attaches the servo on pin 9 to the servo object 

void loop() 

  if (Serial.available()){
    command = Serial.parseInt();  
    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 =;
  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;

       while (newPos > (thePos + 5)){
         newPos = newPos - 5;

No comments:

Post a Comment