#include <Arduino.h>
#include <ESP32Servo.h>
#include <math.h>
// Define servo objects
Servo shoulder_servo;
Servo arm_servo;
Servo wrist_servo;
double G, shoulder_angle, wrist_angle, arm_angle;
float X = 0;
float Y = 110;
float Z = 0;
float D = 0;
float x = 0;
float arm_len = 108.31;
float wrist_len = 138;
float Degree(float radians)
{
return radians * 180 / M_PI;
}
double square(double i)
{
return i * i;
}
void setServoPosition(int shoulder_degrees, int arm_degrees, int wrist_degrees)
{
shoulder_servo.write(shoulder_degrees);
arm_servo.write(arm_degrees);
wrist_servo.write(wrist_degrees);
// Add a delay to allow servos to reach the desired position
delay(500);
}
void setup()
{
Serial.begin(115200);
// Attach each servo to its corresponding pin
shoulder_servo.attach(26);
arm_servo.attach(27);
wrist_servo.attach(13);
}
void loop()
{
float a = arm_len;
float b = wrist_len;
// Check for commands to modify Y
if (Serial.available() > 0)
{
char command = Serial.read();
switch (command)
{
case ‘p’: // Increase Y
Y += 1;
break;
case ‘l’: // Decrease Y
Y -= 1;
break;
case ‘o’: // Increase X
X += 1;
break;
case ‘k’: // Decrease X
X -= 1;
break;
}
}
float tetha = atan(X / Y);
G = Y / cos(tetha);
float A = acos((pow(b, 2) + pow(G, 2) – pow(a, 2)) / (2 * b * G));
arm_angle = acos((pow(a, 2) + pow(G, 2) – pow(b, 2)) / (2 * a * G));
wrist_angle = acos((pow(a, 2) + pow(b, 2) – pow(G, 2)) / (2 * a * b));
setServoPosition(90 + Degree(arm_angle + tetha), 90 + Degree(shoulder_angle), Degree(wrist_angle));
Serial.print(“Current X value: “);
Serial.println(X);
Serial.print(“Current Y value: “);
Serial.println(Y);
Serial.print(“Wrist angle:”);
Serial.println(Degree(wrist_angle));
Serial.print(“Arm angle:”);
Serial.println(Degree(arm_angle + tetha));
delay(500);
}