# Servo not reacting accordingly

**URL:** <https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337>\
**Category:** Programming\
**Created:** [December 28, 2021, 3:40pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337 "2021-12-28T15:40:38Z")\
**Posts on this page:** 20\
**Page:** 1

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 3:40pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/1 "2021-12-28T15:40:38Z")

</div>

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 🙂

```cpp

#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)
```

---

<div class="post-metadata">

**Author:** ![UKHeliBob](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/ukhelibob/32/215663_2.png) [@UKHeliBob](https://forum.arduino.cc/u/UKHeliBob)\
**Post date:** [December 28, 2021, 3:42pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/2 "2021-12-28T15:42:11Z")

</div>

Welcome to the forum

Please post your full sketch

---

<div class="post-metadata">

**Author:** ![anon73444976](https://avatars.discourse-cdn.com/v4/letter/a/439d5e/32.png) [@anon73444976](https://forum.arduino.cc/u/anon73444976)\
**Post date:** [December 28, 2021, 3:43pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/3 "2021-12-28T15:43:56Z")

</div>

> [@nabyla](#):
>
> ```arduino
> int a = 204.0,
> 
> ```

That's an unusual concept of an "int"

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 3:46pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/4 "2021-12-28T15:46:54Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 3:47pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/5 "2021-12-28T15:47:56Z")

</div>

done, see above

---

<div class="post-metadata">

**Author:** ![slipstick](https://avatars.discourse-cdn.com/v4/letter/s/50afbb/32.png) [@slipstick](https://forum.arduino.cc/u/slipstick)\
**Post date:** [December 28, 2021, 3:54pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/6 "2021-12-28T15:54:07Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 3:58pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/7 "2021-12-28T15:58:10Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![UKHeliBob](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/ukhelibob/32/215663_2.png) [@UKHeliBob](https://forum.arduino.cc/u/UKHeliBob)\
**Post date:** [December 28, 2021, 4:02pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/8 "2021-12-28T16:02:47Z")

</div>

What exactly did you try to print ?

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 4:03pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/9 "2021-12-28T16:03:52Z")

</div>

the angles A and B

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 4:04pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/10 "2021-12-28T16:04:10Z")

</div>

I mean their values

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 4:05pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/11 "2021-12-28T16:05:43Z")

</div>

Added a sketch. hope it helps for a better understanding 🙂

---

<div class="post-metadata">

**Author:** ![UKHeliBob](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/ukhelibob/32/215663_2.png) [@UKHeliBob](https://forum.arduino.cc/u/UKHeliBob)\
**Post date:** [December 28, 2021, 4:06pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/12 "2021-12-28T16:06:09Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 4:14pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/13 "2021-12-28T16:14:16Z")

</div>

You mean like this:

```cpp
      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);

?
```

---

<div class="post-metadata">

**Author:** ![UKHeliBob](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/ukhelibob/32/215663_2.png) [@UKHeliBob](https://forum.arduino.cc/u/UKHeliBob)\
**Post date:** [December 28, 2021, 4:37pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/14 "2021-12-28T16:37:21Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![johnwasser](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/johnwasser/32/214270_2.png) [@johnwasser](https://forum.arduino.cc/u/johnwasser)\
**Post date:** [December 28, 2021, 6:07pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/15 "2021-12-28T18:07:45Z")

</div>

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?

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 28, 2021, 6:58pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/16 "2021-12-28T18:58:36Z")

</div>

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?

---

<div class="post-metadata">

**Author:** ![johnwasser](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/johnwasser/32/214270_2.png) [@johnwasser](https://forum.arduino.cc/u/johnwasser)\
**Post date:** [December 28, 2021, 9:42pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/17 "2021-12-28T21:42:05Z")

</div>

> [@nabyla](#):
>
> How did you manage to get 0 printed for A and B, because all i get is nan?

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.

```arduino
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:

```arduino
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.

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [December 30, 2021, 1:25am UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/18 "2021-12-30T01:25:45Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![johnwasser](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/johnwasser/32/214270_2.png) [@johnwasser](https://forum.arduino.cc/u/johnwasser)\
**Post date:** [December 30, 2021, 1:45am UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/19 "2021-12-30T01:45:09Z")

</div>

> [@nabyla](#):
>
> I dont see any way to replace atan because there is no replacement for it.

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

---

<div class="post-metadata">

**Author:** ![nabyla](https://avatars.discourse-cdn.com/v4/letter/n/73ab20/32.png) [@nabyla](https://forum.arduino.cc/u/nabyla)\
**Post date:** [January 5, 2022, 4:45pm UTC](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337/20 "2022-01-05T16:45:07Z")

</div>

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?

[Next page](https://forum.arduino.cc/t/servo-not-reacting-accordingly/940337.md?page=2)