help to control servo motor and get angle reading from joint

Hi there,

I’m doing a project using a servo motor to control bending of 2 joints via thumbstick and at each joint there is pot attach to it to get the angle reading of the joint bending. My problem is when I merge the code for servo control and the code for angle reading from the pot, the servo won’t move but angle reading works. If I run just the servo code(from arduino ide servo example), the servo motor move.

I’m using arduino mega 2560 with Hitec HS-5685MH servo link to thumbstick (bending control of joint) and 2 button pots (angle reading of the joints). I’ve put the servo on external power supply and the button pots on arduino power supply. Both have common ground. The servo is attach to pin 9(PWM), thumbstick is attach to pin 0 (analog) and the two button pots is attach to pin 2(analog) and pin 3 (analog)

below is the merged code. Please point out any mistake that I’ve done and how to solve it

#include <Servo.h>

const int servo1 = 9;       // first servo

const int joyV = 0;        // U/D Parallax Thumbstick

int servoVal;           // variable to read the value from the analog pin

Servo myservo1;  // create servo object to control a servo


int value_1 = 0; 
int value_2 = 0;

// an identifier for each value 
int id_1 = 'A'; 
int id_2 = 'B';      

float maxReading[41] = {
  494,498,499,506,510,512,516,516,524,527,533,534,540,542,544,545,548,551,558,561,563,565,568,575,576,580,581,587,589,593,596,599,604,606,608,610,615,619,621,623,627};
float multValue[41];
float angleReading[41];
float trueAngle[41];
float negMaxReading[41] = {
  494,488,485,482,476,474,470,469,468,467,466,465,459,454,453,450,446,445,443,431,430,429,427,426,425,424,414,410,409,404,402,398,395,392,388,387,384,375,374,371,369};
float negMultValue[41];
float negAngleReading[41];
float negTrueAngle[41];

float maxReadingj2[41] = {
  522,525,529,531,533,541,543,545,549,552,564,568,572,575,580,582,585,587,589,594,596,598,607,610,612,613,616,617,620,621,627,631,632,636,639,640,650,654,656,657,664};
float multValuej2[41];
float angleReadingj2[41];
float trueAnglej2[41];
float negMaxReadingj2[41] = {
  522,520,514,512,511,506,504,498,497,493,489,480,479,478,477,476,468,462,461,455,449,447,445,441,440,436,427,426,419,413,411,404,400,395,393,390,388,386,381,376,364};
float negMultValuej2[41];
float negAngleReadingj2[41];
float negTrueAnglej2[41];
int add[41] = {
  0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40};


int anglePin = 2;
int anglej2Pin = 3;
//const int buttonPin = 5;     // the number of the pushbutton pin
//const int ledPin =  13;      // the number of the LED pin

// variables will change:
//int buttonState = 0;  
void setup(){
  Serial.begin(9600);
  // Servo  
  myservo1.attach(servo1);  // attaches the servo


//    pinMode(anglePin, INPUT);
//pinMode(anglej2Pin, INPUT);    
  // initialize the pushbutton pin as an input:
  //  pinMode(buttonPin, INPUT); 
}
void loop()
{
  // Display Joystick values using the serial monitor
  outputJoystick();


  // Read the horizontal joystick value  (value between 0 and 1023)
  servoVal = analogRead(joyV);           
  servoVal = map(servoVal, 0, 1023, 45, 135);     // scale it to use it with the servo (result between 70 and 180)

  myservo1.write(servoVal);                           // sets the servo position according to the scaled value

  delay(15);                                       // waits for the servo to get there


  float input = analogRead(anglePin);
  float inputj2 = analogRead(anglej2Pin);






  // and wait shortly to not overload the port/board?
  //  delay(15);
  Serial.write("A");
  Serial.print(value_1); 
  Serial.write(10);
  Serial.write("B");
  Serial.print(value_2);
  Serial.write(10);

  for(int i = 0; i<42; i++){
    multValue[i] = maxReading[i+1]-maxReading[i];
  }

  for(int j=0; j<42; j++){
    angleReading[j] = (input - maxReading[j]) / multValue[j];
  }

  for(int k=0; k<42; k++){
    trueAngle[k] = angleReading[k];
  }


  for(int l=0; l<42; l++){
    while(input >= maxReading[l] && input <= maxReading[l+1]){
      value_1 = trueAngle[l]+add[l];


      break;
    }   
  }


  for(int m = 0; m<42; m++){
    negMultValue[m] = negMaxReading[m]-negMaxReading[m+1];
  }

  for(int n=0; n<42; n++){
    negAngleReading[n] = (negMaxReading[n] - input) / negMultValue[n];
  }

  for(int o=0; o<42; o++){
    negTrueAngle[o] = negAngleReading[o];
  }


  for(int p=0; p<42; p++){
    while(input<=negMaxReading[p]&&input>=negMaxReading[p+1]){
      value_1 = negTrueAngle[p]+add[p];

      break; 
    } 
  }

  //--//

  for(int i = 0; i<42; i++){
    multValuej2[i] = maxReadingj2[i+1]-maxReadingj2[i];
  }

  for(int j=0; j<42; j++){
    angleReadingj2[j] = (inputj2 - maxReadingj2[j]) / multValuej2[j];
  }

  for(int k=0; k<42; k++){
    trueAnglej2[k] = angleReadingj2[k];
  }


  for(int l=0; l<42; l++){
    while(input >= maxReadingj2[l] && input <= maxReadingj2[l+1]){
      value_2 = trueAnglej2[l]+add[l];

      break; 
    }   
  }


  for(int m = 0; m<42; m++){
    negMultValuej2[m] = negMaxReadingj2[m]-negMaxReadingj2[m+1];
  }

  for(int n=0; n<42; n++){
    negAngleReadingj2[n] = (negMaxReadingj2[n] - inputj2) / negMultValuej2[n];
  }

  for(int o=0; o<42; o++){
    negTrueAnglej2[o] = negAngleReadingj2[o];
  }


  for(int p=0; p<42; p++){
    while(input<=negMaxReadingj2[p]&&input>=negMaxReadingj2[p+1]){

      value_2 = negTrueAnglej2[p]+add[p];

      break;
    } 
  }


}


/**
 * Display joystick values
 */
void outputJoystick(){


  Serial.print(analogRead(joyV));
  Serial.println ("----------------");
}
//Uncomment the line below when finding the maximum and minimum readings by turning your potentiometer 180 degrees.
//Serial.println(analogRead(anglePin));




//Uncomment the line below to get the angle reading.
// buttonState = digitalRead(buttonPin);
//  // check if the pushbutton is pressed.
//  // if it is, the buttonState is HIGH:
//  if (buttonState == LOW) {     
//    // turn LED on:    
//    digitalWrite(ledPin, HIGH); 
//   
//     
//  } 
//  else {
//    // turn LED off:
//    digitalWrite(ledPin, LOW); 
//  }

I haven't studied your code. Isn't the angle of the joint the same as (or directly proportional to) the angle the servo is instructed to move to? Why do you need separate pots to measure the angle?

...R

the angle of the joint is different from the servo angle. I use the servo to move 2 joints connecting 3 bar. these 3 bar and 2 joint is similar to our finger. To put is simple, the servo motor is use to move a finger like structure and the button pots is attach to each joint of the finger like structure.

I've connect the pwm pin to oscilloscope and no signal going out of it.

finally able to solve it.. change the servo control to manual instead of using arduino servo library