im making a hexopod, and for now i decided to make only one leg, and controll it with coordinates, the measure for distance is milimeters, the measure for angles is degrees, however i have a hard time believing that this

``````turnToPos(81, 51, 0);
``````

results in theese angles

beta = 61.82
-35
61
32

here is the entirety of the code:

``````#include <Wire.h>
#include<cmath>

#define SERVOMIN  60
#define SERVOMAX  500
uint8_t servo = 0;
uint8_t servo1 = 1;
uint8_t servo2 = 2;
int legAngle1;
int legAngle;
int legAngle2;

int angle;
int legLength1 = 106;
int legLength2 = 104;
int distance;
double pi = M_PI;

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){
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 beta = (180/pi)*acos((pow(legLength1, 2) + pow(h, 2) - pow(legLength2, 2))/(2 * legLength1 * h));
Serial.print("beta = ");
Serial.println(beta);
legAngle2 = (180/pi)*asin(y/l);
if(z == 0){
legAngle1 = beta;
}else if(z > 0){
legAngle1 = beta + (180/pi)*asin(z/h);
}else if(z < 0){
legAngle1 = (180/pi)*asin(z/h);
}
legAngle = 90 - (180 - ((180/pi)*acos((pow(legLength1, 2) + pow(legLength2, 2) - pow(h, 2))/(2 * legLength1 * legLength2))));
turnToAngle(servo, legAngle);
turnToAngle(servo1, legAngle1);
turnToAngle(servo2, legAngle2);
}

void loop() {
turnToPos(81, 51, 0);
Serial.println("pos1^");
delay(1000);
turnToPos(40, -30, -5);
Serial.println("pos2^");
delay(1000);
}
``````

and even tho the leg length isnt exact i doubt that a difference in 2 mm will make that huge of a difference.
Leg angle is for the leg furthest from the body, leg angle 1 is for the middle segment and leg angle 2 is for rotating the leg horisontaly

can you provide an illustration identifying what l, x, y, z, beta, legAngle1 and legAngle are?

the angles are what the leg has during pos1 and the size of the leg on the drawing is almost the same as the actual leg

or i guess here is the actual leg

but what do your l, x, y, and z represent?

l is a length, not a position. so what is above intended to do?

i help with the local robotics team. we had a workshop where students wrote code to determine the shoulder and elbow angles of a multi-segment (3) arm to position the end of the arm at the x,y position selected by a mouse on a screen. Seems similar to what you're doing

well x y and z are just overglorified lengths arent they, i mean if x = 50 that means the length to get to x is 50, l is distance to get to x and y

i don't know what you know.

lengths of what ?

i assume x is the horizontal distance between the target position and the vertical axis and similarly y iis the verrical distance between the target and horizontal axis.

but what is z?

what is `beta`?

yes,` l` is the distance between the origin and x,y.

but y and `l` are not perpendicular

z is the hight, and beta is just another angle that isnt a legAgngle

i can send a photo of my calculations on paper if thats gonna help u understand

if z is height, what is y?

it's difficult to understand your question without understanding the symbols?

here is the image of my calculations on paper, the two triangles are connected, but i couldnt represent it on paper, just know that l is a shared line between the two triangles

well if were going with 3d axes then x is length y is width and z is height
(unless x and y are swapped i dunno)

isn't this 2-dimensional.

isn't x, the horizontal distance from the end of the 2nd leg segment (where is attaches to a foot) and the pivot point on the body (i.e. shoulder)?

isn't y the distance from the body to the foot?

why would it be 2 dimentional?
i mean for the MOST part yes, but the leg is still supposed to turn up down left and right so no its 3 dimentional

well no, the x y and z are the coordinates where the ENTIRE leg is supposed to go

or am i thinking of it all wrong?

here's code used to generate the coordinates for the image above tested on a laptop, using xgraph. It assumes a 2 segment arm reaching horizontally to the right and an angle of zero being on the x-axis (not like a compass)

it assumes both segment lengths are the same. it's simpler and a fair enough simplification considering their values of 106 and 104

``````#include <stdio.h>
#include <math.h>

float ang1;
float ang2;
float beta;

int len1 = 106;
int len2 = 106;

float deg (float x)  { return x * 180 / M_PI; }
float rad (float x)  { return x * M_PI / 180; }

// -----------------------------------------------------------------------------
void pos ()
{
float x1 =  0 + len1 * cos(rad(ang1));
float y1 =  0 + len1 * sin(rad(ang1));

float x2 = x1 + len2 * cos(rad(ang1 + ang2));
float y2 = y1 + len2 * sin(rad(ang1 + ang2));

printf ("color=%s\nnext\n", "cyan");
printf (" %8.4f %8.4f\n", 0,  0);
printf (" %8.4f %8.4f\n", x1, y1);
printf (" %8.4f %8.4f\n", x2, y2);
printf ("anno %8.4f %8.4f (%.1f, %.1f)\n", x2, y2, x2, y2);
}

// -------------------------------------
void turnToPos (float x, float y, float z)
{
float d = sqrt (pow (x, 2) + pow (y, 2)); // length between x and y

beta = deg (acos ((d/2) / len1));

ang1 = beta + deg (acos (x / d));
ang2 = -2 * beta;

printf (", d %6.2f",  d);
printf (", beta %6.2f", beta);
printf (", ang1 %6.2f", ang1);
printf (", ang2 %6.2f", ang2);
printf ("\n");

pos ();
}

// -----------------------------------------------------------------------------
int main ()
{
float x, y, z;

for (x = 50; x < 120; x += 20)  {
turnToPos (x, y, z);
y += 15;
}
return 0;
}
``````

full output

``````, d  50.00, beta  76.36, ang1  76.36, ang2 -152.72
color=cyan
next
0.0000   0.0000
25.0000 103.0097
50.0000   0.0000
anno  50.0000   0.0000 (50.0, 0.0)
, d  71.59, beta  70.26, ang1  82.36, ang2 -140.53
color=cyan
next
0.0000   0.0000
14.0946 105.0588
70.0000  15.0000
anno  70.0000  15.0000 (70.0, 15.0)
, d  94.87, beta  63.42, ang1  81.85, ang2 -126.83
color=cyan
next
0.0000   0.0000
15.0233 104.9300
90.0000  30.0000
anno  90.0000  30.0000 (90.0, 30.0)
, d 118.85, beta  55.90, ang1  78.15, ang2 -111.80
color=cyan
next
0.0000   0.0000
21.7649 103.7415
110.0000  45.0000
anno 110.0000  45.0000 (110.0, 45.0)
``````

i think this fixes my problem, ty even tho it took me half an hour to desipher your text

it took me a lot longer to try to understand what you were trying to do and how you were trying to do.

there's really just 4 lines of geometry code