Servos: How to declare a variable correctly so that it remembers its calculated value?

Projects General Guidance
1

Hello,
I am working on a walking algorithm for a robot dog with servo motors. The code actually looks as follows:

#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(115200);
  Serial.println("Test 1");
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(Frequenz);  // Analog servos run at ~50 Hz updates

  delay(200);
}
const float a = 200.0;
const float b = 180.0;
const float h = 300.0;
const float a_quadrat = a * a;
const float b_quadrat = b * b;
const float h_quadrat = h * h;

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

  // P1: Startposition
  const float InitialX = 0.0;

  // P2: Endposition
  const float TargetX = 200.0;
  const float TargetY = 0.0;

  const float Radius = (TargetX - InitialX) / 2.0;
  
  if (float Startposition = 140) {

    for (float X = 0 ; X <= 200; X += 1.0)

    {

      float Y = sqrt(Radius * Radius - pow(X - Radius, 2));
      float c = sqrt(X * X + pow((h - Y), 2));

      float angle = degrees(atan2(X, (h - Y)));

      float c_quadrat = c * c;

      float cosB = (a_quadrat + b_quadrat - c_quadrat) / (2.0 * a * b);

      float Kneeangle = degrees(acos(cosB));

      float Thighangle = Startposition - angle;


      setServo(0, Thighangle);
      setServo(1, Kneeangle);

      delay(1000);

    }
  }
}

The one servo (thigh) positioning must start at 140° and from there it must perform a semi-circle movement together with the second servo (knee).
When running the code and trying to see the servo positioning, the ,,Thighangle" does not change even though the serial monitor prints out different values. The Thigh stays at 140°.
How should it be declared to make the servo remember the past angle position while keeping substracting the (variable) ,,angle" value from it during the ,,for-statement"?

2

The first thing that I notice is that Thighangle is declared as a float but the setServo() function takes an int as its second parameter. Frankly I am surprised that the code compiles. What range of values do you see in the Serial monitor ?

In a similar vein, in this code

    for (float X = 0 ; X <= 200; X += 1.0)

Why are you using a float variable ?

However, here is a definite mistake in you code

  if (float Startposition = 140)

I assume that you meant

  if (float Startposition == 140)
3

thanks for the quick response,

the range of the Thighangle for x € [0, 57] is [32.44, 47.8] (i stopped the loop at X = 57) and for the Knee [109.11, 72.94]

4

Using a float for Thighangle is a waste of time if you are going to use it as an input to a servo write() command

Have you fixed the error with the = sign ?

5

yes, changed it to ==

And changed the second parameter of setServo () to float

6

As I said, that is a waste of time, particularly as pulsweite is an int in the setServo() function anyway

7

when running the code with ==, the X values are not varying as when I use =

8

alright then, changed Thighangle to int.

9

Now post your code (in a new post) as it is now

10 
#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(115200);
  Serial.println("Test 1");
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(Frequenz);  // Analog servos run at ~50 Hz updates

  delay(200);
}
const float a = 200.0;
const float b = 180.0;
const float h = 300.0;
const float a_quadrat = a * a;
const float b_quadrat = b * b;
const float h_quadrat = h * h;

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

  //Startposition
  float ThighStart = 140;
  float KneeangleStart = degrees(acos((a_quadrat + b_quadrat - h_quadrat) / (2.0 * a * b)));

  int Kneeangle = KneeangleStart;

  setServo(0, ThighStart);
  setServo(1, Kneeangle);

  if (ThighStart == 140) {

    IK();
  }
}


void IK() {
  // P1: Startposition
  const float InitialX = 0.0;

  // P2: Endposition
  const float TargetX = 200.0;
  const float TargetY = 0.0;

  const float Radius = (TargetX - InitialX) / 2.0;

  int ThighStart;

  for (float X = 0 ; X <= 200; X += 1.0)

  {

    float Y = sqrt(Radius * Radius - pow(X - Radius, 2));
    float c = sqrt(X * X + pow((h - Y), 2));

    float angle = degrees(atan2(X, (h - Y)));

    float c_quadrat = c * c;

    float cosB = (a_quadrat + b_quadrat - c_quadrat) / (2.0 * a * b);

    int Kneeangle = degrees(acos(cosB));

    int Thighangle = ThighStart - angle;

    Serial.print("X=");
    Serial.print(X);
    Serial.print(", Y=");
    Serial.print(Y);

    Serial.print(", Thighangle = ");
    Serial.print(Thighangle);
    Serial.print(", Kneeangle = ");
    Serial.println(Kneeangle);
    setServo(0, Thighangle);
    setServo(1, Kneeangle);

    delay(1000);

  }
}
11

As now the X values are varying, I don't know why the Thighangle doesn't vary in accordance with X.

12 
  for (float X = 0 ; X <= 200; X += 1.0)
  {
    float Y = sqrt(Radius * Radius - pow(X - Radius, 2));
    float c = sqrt(X * X + pow((h - Y), 2));
    float angle = degrees(atan2(X, (h - Y)));
    float c_quadrat = c * c;
    float cosB = (a_quadrat + b_quadrat - c_quadrat) / (2.0 * a * b);
    int Kneeangle = degrees(acos(cosB));
    int Thighangle = ThighStart - angle;
    Serial.print("X=");
    Serial.print(X);
    Serial.print(", Y=");
    Serial.print(Y);
    Serial.print(", Thighangle = ");
    Serial.print(Thighangle);
    Serial.print(", Kneeangle = ");
    Serial.println(Kneeangle);
    setServo(0, Thighangle);
    setServo(1, Kneeangle);
    delay(1000);
  }

What do you see if you print the intermediate values in the calculation ?
Are they what you expect ?

13

Even if you change '=' to '==', I don't see how that statement makes any sense. Are you sure you didn't mean:
'const float Startposition = 140;'

14 
const float ThighStart = 140;

  if ( ThighStart == 140) {

    IK();
  }

Changed it as suggested.
Values are printed but the Thigh servo does not move according to the printed values. It just remains at 140°. While the Knee servo executes the movements. I don't see the error.

15

as expected but the Thigh servo does not move.

16

I suspect that the servo that doesn't move is either getting no power or no signal due to a wiring mistake.

17

That can be simplified to just

IK();

since ThightStart is a constant
Inside your IK() function, you never initialize ThighStart

void IK() {
...
  int ThighStart;

  for (float X = 0 ; X <= 200; X += 1.0)
  {
    ...
    int Thighangle = ThighStart - angle;

If you have your compiler warnings turned on, it would have pointed this out. Are you expecting this local variable ThighStart to somehow be related to the ThighStart variable that is local to loop()? The two are not the same. Completely different variables that just happen to share the same name, but it different scopes. Maybe you wanted it a global?

18

i changed

const float ThighStart = 140;

back to

float ThighStart = 140;

and then the servo started working.

19

You have never explained why ThighStart is a float

20

The ThighStart value states the initial Servo position (140°) from which the ,,angle" value is subtracted.
The Thighangle, in accordance to X takes on new values from which ,,angle" (also variable according to X) is taken away.
Maybe the way I declared the operation makes it confusing .