"Waldo" Motion Capture with LittleArm

// training program with gripper button working.

#include <Servo.h>  //arduino library
#include <math.h>   //standard c library

#define PI 3.141

Servo baseServo;  
Servo shoulderServo;  
Servo elbowServo; 
Servo gripperServo;

int command;

struct jointAngle{
  int base;
  int shoulder;
  int elbow;
};

int desiredGrip = 30;

int basePotPin = A0;
int shoulderPotPin = A1;
int elbowPotPin = A2; 

int desiredDelay = 3;

int potValHolder1;
int potValHolder2;
int potValHolder3;

int recordPin = 2;
int gripPin = 3;

bool gripped = 1;

bool holder = 1;

struct jointAngle desiredAngle; //desired angles of the servos

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

int servoParallelControl (int thePos, Servo theServo );
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void setup()
{ 
  Serial.begin(9600);
  baseServo.attach(9);        // attaches the servo on pin 9 to the servo object 
  shoulderServo.attach(10);
  elbowServo.attach(11);
  gripperServo.attach(6);
  
  Serial.setTimeout(50);      //ensures the the arduino does not read serial for too long
  Serial.println("started");
  baseServo.write(90);        //intial positions of servos
  shoulderServo.write(150);
  elbowServo.write(110);


  pinMode(gripPin, INPUT);
  pinMode(recordPin, INPUT);
  
} 

//primary arduino loop
void loop() 
{ 
    potValHolder1 = analogRead(basePotPin);
    if (potValHolder1 < 500){
        desiredAngle.base = map(potValHolder1, 0, 500, 175, 5);
    }
    
    potValHolder2 = analogRead(shoulderPotPin);
    if (potValHolder2 > 500){
       desiredAngle.shoulder = map(potValHolder2, 501, 1023, 175, 5);
    }
    
    potValHolder3 = analogRead(elbowPotPin);
    if (potValHolder3 < 500){
       desiredAngle.elbow = map(potValHolder3, 0, 500, 175, 5);
    }

   // holder = digitalRead(gripPin);


// read the gripper button to define position. 
// when the button is pressed the gripper changes positions
    if ((digitalRead(gripPin) ==HIGH) ){
      gripped = !gripped;    

      if (gripped == 1){
        desiredGrip = 3;
      }
      if (gripped == 0){
        desiredGrip = 75;
      }
      
    }

  int status1 = 0;
  int status2 = 0;
  int status3 = 0;
  int status4 = 0;
  int done = 0 ; 
  
  while( done == 0){  
    //move the servo to the desired position
    status1 = servoParallelControl(desiredAngle.base, baseServo, desiredDelay);
    status2 = servoParallelControl(desiredAngle.shoulder,  shoulderServo, desiredDelay);
    status3 = servoParallelControl(desiredAngle.elbow, elbowServo, desiredDelay);      
    status4 = servoParallelControl(desiredGrip, gripperServo, desiredDelay);  
    
    if (status1 == 1 & status2 == 1 & status3 == 1 & status4 == 1){
      done = 1;
      
    }
        
  }// end of while
}

//++++++++++++++++++++++++++++++FUNCTION DEFITNITIONS++++++++++++++++++++++++++++++++++++++++++

int servoParallelControl (int thePos, Servo theServo, int theSpeed ){
  
    int startPos = theServo.read();        //read the current pos
    int newPos = startPos;
    //int theSpeed = speed;
    
    //define where the pos is with respect to the command
    // if the current position is less that the actual move up
    if (startPos < (thePos-5)){
          
       newPos = newPos + 1;
       theServo.write(newPos);
       delay(theSpeed);
       return 0;
           
    }
  
   else if (newPos > (thePos + 5)){
  
      newPos = newPos - 1;
      theServo.write(newPos);
      delay(theSpeed);
      return 0;
          
    }  
    
    else {
        return 1;
    }  
    
}