I am having difficulty with my inverse kinematics equation specifically the shoulder which is the base servo for elevating the first linkage.
as it stands the elbow can correctly articulate to the proper angle for the hypotenuse however when shifting x values left or right neither correctly keeps the end effector's motion horizontal
if needed I could create a drawing of the issue.
I have no experience in trigonometry and have learned from reading cosine rules and using triangle calculators to check im getting the right relationships
#include <Servo.h>
#include <math.h>
Servo shoulder;
Servo elbow;
double dx = 0;
double dy = 5;
double dz = 0;
double pitch;
double heading;
int centerpointx = 0; // x center of coordinate plane from which to calc hypotenuse
int centerpointy = 0; // y center of coordinate plane from which to calc hypotenuse
int centerpointz = 0; // z center of coordinate plane from which to calc hypotenuse
double distance; //hypotenuse
double distance2; // hypotenuse between center and Z axis
int inputfromserial;
float shoulderlength = 10; // first arm segent length
float elbowlength = 10; // second arm segment length
double shoulderangle; //angle opposite of hypotenuse for elbow
double elbowangle;
int shoulderoffset = 0;
int elbowoffset = 0;
int analoginputy = A0;
int analogoutputy;
int analoginputx = A1;
int analogoutputx;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
shoulder.attach(9);
elbow.attach(2);
}
//serial dx value input
void serialEvent(){
inputfromserial = Serial.parseInt();
dx = inputfromserial;
Serial.println(inputfromserial);
}
void loop() {
// code for moving dx and dy with joystick
analogoutputy = analogRead(analoginputy);
analogoutputx = analogRead(analoginputx);
if(analogoutputy < 200){
dy = dy - 0.25;
}
if(analogoutputy > 700){
dy = dy + 0.25;
}
if(analogoutputx < 200){
dx = dx - 0.25;
}
if(analogoutputx > 700){
dx = dx + 0.25;
}
//pitch calculation
pitch = atan2(dy,dx);
pitch = pitch*57296/1000;
// heading calculation for eventual z rotation. not used
heading = atan2(dz, dy);
distance2 = sqrt( sq(dz-centerpointz) + sq(dy-centerpointy) );
//distance calc
distance = sqrt( sq(dx-centerpointx) + sq(dy-centerpointy) );
distance = distance + distance2;
//Law of cosines stuff
// middle joint also known as C
shoulderangle = acos( (sq(elbowlength) - sq(shoulderlength) + sq(distance) ) / (2 * elbowlength * distance) );
shoulderangle = (shoulderangle * 57296)/1000;
//elbow angle calc //deg = rad * 57296/1000 acos((sq(shoulderlength)-sq(elbowlength)-sq(distance))/(2*elbowlength*shoulderlength))
elbowangle = acos( (sq(shoulderlength) + sq(elbowlength) - sq(distance) ) / (2 * elbowlength * shoulderlength) );
elbowangle = elbowangle*57296/1000;
//Serial.println("////////");
Serial.print(" angle to point from center ");
Serial.print(pitch);
Serial.print(" hypotenuse ");
Serial.print(distance);
Serial.print(" shoulder angle ");
Serial.print(shoulderangle);
Serial.print(" elbowangle ");
Serial.print(elbowangle);
Serial.println(" ");
shoulder.write(pitch-shoulderangle);
elbow.write(elbowangle);
delay(50);
}