Servo not reacting accordingly

Using Arduino Programming Questions
1

Hello everyone,
I am currently working on a code for a robot dog using Arduino Mega and a Servo shield. The code is for one leg whose upper and lower legs are both controlled by one servo . When I load the code, the servos just set their position to 160° (see code below) and stay there. Why does the code and/ or the leg not run the different X values (from 0 to 60) so that the angles (A and B) can be calculated?
P.S.: The movement that needs to be performed is a semicircle.

Thanks in advance :slight_smile:


#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define Frequenz 50
#define Umax 2000
#define Umin 500
#define PI 3.1415926535897932384626433832795


String Servochannel;
int X;
void setup() {

  Serial.begin(9600);
  Serial.println("Test5");
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(Frequenz);  // Analog servos run at ~50 Hz updates

  delay(10);
  X = 0;

}
void setServo(int servo, int winkel) {
  int duty, pulsweite;
  duty = map(winkel, 0, 180, Umin, Umax);
  pulsweite = int(float(duty) / 1000000 * Frequenz * 4096);
  pwm.setPWM(servo, 0, pulsweite);
}

void loop() {

  setServo(0,  160 );
  setServo(1,  160 );
  delay(1000);

  IK();
}
void IK() {

  int a = 204.0, b = 176.0; // a= upper leg lenght, b= lower leg lenght)
  float A, B;
  float Theta;
  int h, d;
  float Y;
  h = 290;
  {
    for (X = 0; X <= 60; X += 1) {

      Y = sqrt(900 + pow((X - 30), 2));
      Theta = (50 + (atan(X / h) * 180) / PI);
      d = abs(((h - Y) / (cos(Theta) * PI / 180)));

      A = (acos((pow(a, 2) + pow(d, 2) - pow(b, 2)) / (2 * a * d)) * 180) / PI;
      B = (acos((pow(a, 2) + pow(b, 2) - pow(d, 2)) / (2 * a * b)) * 180) / PI;

      setServo(0, A);
      setServo(1, B);

      delay(3000);

    }
  }
}
[Sketch_robodog.pdf.pdf|attachment](upload://8XdqDzaswRqfmj3puI31AFfscfG.pdf) (40.3 KB)
2

Welcome to the forum

Please post your full sketch

3

That's an unusual concept of an "int"

4

thats right, i changed it from float to int. Will change it immediately to 240!
Thanks for the remark. :slight_smile:

5

done, see above

6

I can't follow your calculations but the first thing to do is surely to put some Serial prints in to see what actual values of A and B you're getting.

Steve

7

Ive tried the Serial prints, but it always printed ,,Nan" which is why i took it out.

8

What exactly did you try to print ?

9

the angles A and B

10

I mean their values

11

Added a sketch. hope it helps for a better understanding :slight_smile:

12

Break the calculation down into separate steps and print the intermediate results

13

You mean like this:

      A = (acos((pow(a, 2) + pow(d, 2) - pow(b, 2)) / (2 * a * d)) * 180) / PI;
      Serial.println(A);
      
      B = (acos((pow(a, 2) + pow(b, 2) - pow(d, 2)) / (2 * a * b)) * 180) / PI;
      Serial.println(B);

?
14

No. I meant print each step of the calculations to see whether they are reasonable and what you expect them result of that step to be

15

When I run the sketch, A and B are always 0.

Y goes from 42 down to 30.02 and back up to 42.
Theta is always 50.00
d goes from 14699 to 15437 and back down to 14699

Do those values make sense?

16

This is exactly how it should not be, because A and B should have different values depending on Theta and X, respectively.

The values are not making sense at all. Because for X = 0 -> Y = 42.42 and Theta = 58,32 (calculator)
I dont really see the error in the trigonometric formula and i really dont understand why it prints 0 when there are values.

How did you manage to get 0 printed for A and B, because all i get is nan?

17

I printed 'winkel' in setServo().

'nan' means "Not A Number" and it's the value you get when your calculation fails, like dividing by zero or taking the square root of a negative number.

I broke down your calculation step by step, as recommended earlier, and found that you are trying to take the arc-cosine of the values -6727.00 and -34436.86. Neither of those is a value between -1 and +1 so the results are both NAN.

void IK()
{
  int a = 204.0, b = 176.0; // a= upper leg lenght, b= lower leg lenght)
  float A, B;
  float Theta;
  int h, d;
  float Y;
  h = 290;
  for (X = 0; X <= 60; X += 1)
  {
    Y = sqrt(900 + pow((X - 30), 2));
    Theta = (50 + (atan(X / h) * 180) / PI);
    d = abs(((h - Y) / (cos(Theta) * PI / 180)));

    Serial.print("Y=");
    Serial.print(Y);
    Serial.print(", Theta=");
    Serial.print(Theta);
    Serial.print(", d=");
    Serial.println(d);

    float asq = pow(a, 2);
    float bsq = pow(b, 2);
    float dsq = pow(d, 2);

    Serial.print("asq=");
    Serial.print(asq);
    Serial.print(", bsq=");
    Serial.print(bsq);
    Serial.print(", dsq=");
    Serial.println(dsq);

    float fa = (asq + dsq - bsq);
    float fb = (asq + bsq - dsq);

    Serial.print("fa=");
    Serial.print(fa);
    Serial.print(", fb=");
    Serial.println(fb);

    float f2a = fa / (2 * a * d);
    float f2b = fb / (2 * a * b);

    Serial.print("f2a=");
    Serial.print(f2a);
    Serial.print(", f2b=");
    Serial.println(f2b);

    float acosA = acos(f2a);
    float acosB = acos(f2b);

    Serial.print("acosA=");
    Serial.print(acosA);
    Serial.print(", acosB=");
    Serial.println(acosB);

    A = (acosA * 180) / PI;
    B = (acosB * 180) / PI;

    setServo(0, A);
    setServo(1, B);

    delay(500);
  }
  delay(10000);
}

Results for the first iteration:

0, 160, 375
1, 160, 375
Y=42.43, Theta=50.00, d=14699
asq=41616.00, bsq=30976.00, dsq=216060608.00
fa=216071248.00, fb=-215988016.00
f2a=-6727.00, f2b=-34436.86
acosA=nan, acosB=nan
0, 0, 102
1, 0, 102

The "atan(X / h)" is a problem because 'X' and 'h' are both integers. h = 290 and X is from 0 to 60 so the result is always 0.

18

Hi @johnwasser , Thank you very much for your help.
I tried to deal the problem of atan(X/h) by defining Theta as a float.
I dont see any way to replace atan because there is no replacement for it.

19

Isn't there an 'atan2()' that takes the opposite and adjacent sides instead of the ratio?
https://www.cplusplus.com/reference/cmath/atan2/

20

hi @johnwasser, thanks again for the help.

I tried out the new the code, Theta is now printing different values.
But acosA and acosB are both printing nan. Do they need to be replaced as well?