Trying to figure out why my func

Using Arduino Programming Questions
1

i am making a script for my hexopod leg, currently only for one to test out if my leg will work at all. Here is the script

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN  60 
#define SERVOMAX  500
uint8_t servo = 0;
uint8_t servo1 = 1;
uint8_t servo2 = 2;

int angle;
int legLength1 = 106;
int legLength2 = 104;
int distance;

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(50);
}

void turnToAngle(int pin, int angle) {
  float pulse;
  pulse = map(angle, -90, 90, SERVOMIN, SERVOMAX);
  pwm.setPin(pin, pulse);
  Serial.println(angle);
}

void turnToPos(float x, float y, float z){
  // the legs are counted from body to end
  float l = sqrt(pow(x, 2) + pow(y, 2)); // length between x and y
  float h = sqrt(pow(l, 2) + pow(z, 2)); // length between l and z
  float omega = acos((pow(legLength1, 2) + pow(h, 2) - pow(legLength2, 2))/(2 * legLength1 * h));
  int legAngle1 = omega - atan(x / y);
  int legAngle2 = acos((pow(legLength1, 2) + pow(legLength2, 2) - pow(h, 2))/(2 * legLength1 * legLength2)) - 90;
  turnToAngle(servo2, legAngle2);
  Serial.println(legAngle2);
  turnToAngle(servo1, legAngle1);  
  Serial.println(legAngle1);
}

void loop() {
  turnToPos(50, 50, 10);
  Serial.println(1);
  delay(1000);
  turnToPos(40, 5, -5);
  Serial.println(2);
  delay(1000);
}

for reasons i do not yet fully understand, the leg only turns to the first set of angle's it is given, aka (50, 50, 10) and then stops.

2

also here is what the Serial.println types in:

0
1
-89
-89
0
0
2
-89
-89
0
0
1
-89
-89
0
0
2
-89
-89
0
0
1
-89
-89
0
0
2
-89
-89
0
0
1
3

Have you accounted for the fact that trig functions use radians, not degrees?

Print the value you actually write to the servo at the very point that you do.

a7

1 Like
4

About 95% of the servo problems reported on this forum are due to inadequate servo power supplies. Make sure to use a separate power supply, capable of 1 Ampere per servo for small servos (e.g. SG90) and 2.5 Amperes per servo for large (e.g. MG996R).

Don't forget to connect the grounds!

1 Like
5

that i already made sure, the servo's are working well.

6

Judging by the presence of the number 90 in this statement I'd say no they didn't account for that.

1 Like
7

Since acos() returns angles in radians, the servo won't move much in this sequence (regardless of the "90").

int legAngle2 = acos((pow(legLength1, 2) + pow(legLength2, 2) - pow(h, 2))/(2 * legLength1 * legLength2)) - 90;
turnToAngle(servo2, legAngle2);
8

Yes.

acos() returns a value between 0 and 3.14159265…, so that expression will have values between -90 and -86.858... no so good for the servo, which may constrain the value and end up using zero.

which @water_28 might see if the values were printed. It might have been a clue.

a7

9

thatnks for that, shouldve studied math better at school

10

Better to the point, you should have read the documentation for the maths functions, where you would have seen the units used in the arguments and return values.

Radians are used almost exclusively in maths libraries; there will usually be functions to convert between degrees and radians, as well as a good enough constant for the ratio of a circle's circumference to its diameter. Like 22/7. :wink:

And please take away the sink,est method for investigating things that aren't behaving: use copious printing to see the values of variables including intermediate expressions, that they are plausible and are properly informing the flow through your code.

Here in particular, if a servo isn't doing what you think it should, confirm that you are telling it what you think.

Good luck with everything else!

a7