CPU ATmega328 Arduino UNO
#include <Servo.h>
#include <math.h>
#include <EEPROM.h>
#define MAX_RECORDS 10
double L1 = 50;
double L2 = 135;
double L_tip = 147;
Servo myservo1; // create servo object to control servo 1
Servo myservo2; // create servo object to control servo 2
Servo myservo3; // create servo object to control servo 3
double mapDouble(double x, double in_min, double in_max, double out_min, double out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void setup()
{
myservo1.attach(2); // attaches servo 1 on pin 2 to the servo object
myservo2.attach(3); // attaches servo 2 on pin 3 to the servo object
myservo3.attach(4); // attaches servo 3 on pin 4 to the servo object
Serial.begin(115200);
read_eeprom();
}
void loop()
{
if (Serial.available() > 0) {
char command = Serial.read();
double x, y, z, t1, t2, t3;
switch (command) {
case 'f':
// Forward kinematics
t1 = myservo1.read();
t2 = myservo2.read();
t3 = myservo3.read();
double x1, y1, z1;
forward_kinematics(mapDouble(t1, 0, 180, -M_PI, M_PI), mapDouble(t2, 0, 180, -M_PI, M_PI), mapDouble(t3, 0, 180, -M_PI, M_PI), x1, y1, z1);
// Print the end-effector position
Serial.print("x1: ");
Serial.print(x1);
Serial.print(", y1: ");
Serial.print(y1);
Serial.print(", z1: ");
Serial.println(z1);
break;
case 'i':
// Inverse kinematics
x = Serial.parseFloat();
y = Serial.parseFloat();
z = Serial.parseFloat();
inverse_kinematics(x, y, z, t1, t2, t3);
// Map the angles to servo values
int servo1Value = mapDouble(t1, -M_PI, M_PI, 0, 180);
int servo2Value = mapDouble(t2, -M_PI, M_PI, 0, 180);
int servo3Value = mapDouble(t3, -M_PI, M_PI, 0, 180);
// Set the servo positions
myservo1.write(servo1Value);
delay(50);
myservo2.write(servo2Value);
delay(50);
myservo3.write(servo3Value);
delay(50);
Serial.print("servo1Value: ");
Serial.print(servo1Value);
Serial.print(", servo2Value: ");
Serial.print(servo2Value);
Serial.print(", servo3Value: ");
Serial.println(servo3Value);
break;
case 't':
// Teaching mode
int record_num = Serial.parseInt();
if (record_num >= 0 && record_num <MAX_RECORDS)
{
double t1, t2, t3;
t1 = mapDouble(myservo1.read(), 0, 180, -M_PI, M_PI);
t2 = mapDouble(myservo2.read(), 0, 180, -M_PI, M_PI);
t3 = mapDouble(myservo3.read(), 0, 180, -M_PI, M_PI);
EEPROM.put(record_num * sizeof(double) * 3, t1);
EEPROM.put(record_num * sizeof(double) * 3 + sizeof(double), t2);
EEPROM.put(record_num * sizeof(double) * 3 + sizeof(double) * 2, t3);
Serial.print("Recorded servo positions for record number ");
Serial.println(record_num);
} else {
Serial.println("Invalid record number!");
}
break;
case 'r':
// Playback mode
int playback_num = Serial.parseInt();
if (playback_num >= 0 && playback_num < MAX_RECORDS) {
double t1, t2, t3;
EEPROM.get(playback_num * sizeof(double) * 3, t1);
EEPROM.get(playback_num * sizeof(double) * 3 + sizeof(double), t2);
EEPROM.get(playback_num * sizeof(double) * 3 + sizeof(double) * 2, t3);
// Map the angles to servo values
int servo1Value = mapDouble(t1, -M_PI, M_PI, 0, 180);
int servo2Value = mapDouble(t2, -M_PI, M_PI, 0, 180);
int servo3Value = mapDouble(t3, -M_PI, M_PI, 0, 180);
// Set the servo positions
myservo1.write(servo1Value);
delay(500);
myservo2.write(servo2Value);
delay(500);
myservo3.write(servo3Value);
delay(500);
Serial.print("Playback servo positions for record number ");
Serial.println(playback_num);
} else {
Serial.println("Invalid record number!");
}
break;
default:
Serial.println("Invalid command!");
}
}
}
void inverse_kinematics(double x, double y, double z, double &theta1, double &theta2, double &theta3)
{
// Compute theta1
theta1 = atan2(y, x);
// Compute theta3
//double P_2 = pow(x + y, 2) + pow(z - 50, 2);
long P_2 = pow(x + y, 2) + pow(z - 50, 2);
//double C3 = (P_2 - pow(135, 2) - pow(147, 2)) / (2 * 135 * 147);
long C3 = (P_2 - pow(135, 2) - pow(147, 2)) / (2 * 135 * 147);
theta3 = atan2(-sqrt(1 - pow(C3, 2)), C3);
// Compute theta2
double r = 135 + 147 * C3;
double a = z - 50;
double b = sqrt(pow(x, 2) + pow(y, 2)) - 50 * cos(theta1);
double D = (pow(b, 2) + pow(a, 2) - pow(r, 2)) / (2 * b * a);
theta2 = atan2(-a, b) - atan2(-sqrt(1 - pow(D, 2)), D);
}
void forward_kinematics(double theta1, double theta2, double theta3, double &x1, double &y1, double &z1)
{
double C1 = cos(theta1);
double C2 = cos(theta2);
double C3 = cos(theta3);
double S1 = sin(theta1);
double S2 = sin(theta2);
double S3 = sin(theta3);
x1 = L2*C1*S2 + L_tip*C1*(C2*S3 + S2*C3);
y1 = L2*S1*S2 + L_tip*S1*(S2*C3 + C2*S3);
z1 = L1 + L_tip*(C2*C3 - S2*S3) + L2*C2;
}
void read_eeprom() {
Serial.println("Reading values from EEPROM:");
for (int i = 0; i < MAX_RECORDS; i++) {
double t1, t2, t3;
EEPROM.get(i * sizeof(double) * 3, t1);
EEPROM.get(i * sizeof(double) * 3 + sizeof(double), t2);
EEPROM.get(i * sizeof(double) * 3 + sizeof(double) * 2, t3);
Serial.print("Record ");
Serial.print(i);
Serial.print(": ");
if (!isnan(t1) && !isnan(t2) && !isnan(t3)) {
Serial.print(t1);
Serial.print(", ");
Serial.print(t2);
Serial.print(", ");
Serial.println(t3);
} else {
Serial.println("Invalid record!");
}
}
Serial.println("Finished reading values from EEPROM.");
}
ความคิดเห็น
แสดงความคิดเห็น