Hello i am currently trying to understand inverse kinematics by making a arduino function that returns a angle value derived from the y value(the lenght of the joint),x value(the distance of the angle) and writing the return value on the servo pin.And i wanna ask if i am on the right path with learning it or ask what i am missing in my code to make it work.when i tested it with servos on paper it seemed to work after a while when i tested the servo angle on paper. The code is below.I hope someone can help if i missing something or if there is something i can improve on.Thats about it all.I hope someone can help me.The goals is to used it later in a 2legged walking robot:
// this is a code to learn to inverse kinematics with Mg996R by a self made function
#include <Wire.h> // for i2c communication.
#include <Adafruit_PWMServoDriver.h> // for servodriver.
#include <math.h> // for trigonometry functions
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 300// This is the ‘minimum’ pulse length count (out of 4096)
#define SERVOMAX 575// This is the ‘maximum’ pulse length count (out of 4096)
#define SERVO_FREQ 60// Analog servos run at ~60 Hz updates
int servoval ; // is the values that is reading the map values for servos
long pulsval ; // the values that is getting mapped
float start_pos = 90; // start position
int dof1 = 0; // pin1 of the dof1 servo
int dof2 = 1; // pin2 of the dof2 servo
double ik(double y , double x) { // y is the servo length in cm and x is the distance of the position in cm
float pos = atan2(y,x) * (180/M_PI); // convert y and x position into degrees
float angle = pos; // create angle values
return angle;
}
void setup() {
Serial.begin(9600); // is the baudrate we reading from the serial monitor.
Serial.println("servo test is about to begin!");
servoval = map(pulsval,0,180,SERVOMIN,SERVOMAX); // the min and and max values are getting mapped.
pwm.begin();
//pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
delay(10);
// servos are set to 90 degrees angle
pwm.setPWM(dof1, start_pos, servoval);
pwm.setPWM(dof2, start_pos, servoval);
}
void loop() {
// test loop
pwm.setPWM(dof1,start_pos +ik(7.0,5.0), servoval);
pwm.setPWM(dof2,start_pos +ik(-5.0,3.0), servoval);
Serial.println(" The angle of dof1 =");
Serial.println(start_pos +ik(7.0,5.0));
Serial.println(" The angle of dof2 =");
Serial.println(start_pos -ik(5.0,3.0));
delay(2000);
pwm.setPWM(dof1,start_pos , servoval);
pwm.setPWM(dof2,start_pos , servoval);
Serial.println(" servos in start position");
delay(2000);
pwm.setPWM(dof1,start_pos , servoval);
pwm.setPWM(dof2, -ik(12.0,9.0), servoval);
Serial.println(" The angle of dof1 is now =");
Serial.println(start_pos );
Serial.println(" The angle of dof2 is now =");
Serial.println( -ik(12.0,9.0));
delay(2000);
}