#include <math.h>
#include <stdio.h>
#include <Servo.h>
#define buttonPin 5 // Buttons are defined
#define buttonPin2 7 // Buttons are defined
// ------------------------------------------------------------------Servos
Servo motor1;
Servo motor2;
Servo motor3;
// We define motors in this part.
// ------------------------------------------------------------------Servos
// ------------------------------------------------------------------Variables
const int BUFFER_SIZE = 64;
char inputBuffer[BUFFER_SIZE];
const float u1[3] = { 1, 0, 0 }; // unit vector function of x axis
const float u2[3] = { 0, 1, 0 }; // unit vector function of y axis
const float u3[3] = { 0, 0, 1 }; // unit vector function of z axis
const float O[3] = { 0, 0, 0 }; // position of ground frame (initial frame)
const float Bb0[3] = { 0, 0, 50 }; // position of base
float Ap0[3]; // is used in calculate function
float rot_matrix[3][3]; // matrix to keep rotation matrix of the system
const float sigma[3] = { -1, -1, -1 }; // sigma value to find the rotation angles of motors
const float sigma_p[3] = { 1, 1, 1 }; // sigma value to find theta p angle (for singularities)
float f[3]; // array to keep fk values for each motor
float theta[3] = { 0, 0, 0 }; // Initial theta angles of motors (matrix or array to keep the motor angles)
float theta_d[3]; // is used in calculate function
float theta_p[3]; // is used in calculate function (matrix of theta p)
float gamma[3]; // is used in calculate function
float result[3]; // is used in calculate function
const float beta[3] = { 0, 2 * PI / 3, -2 * PI / 3 }; // is used in calculate function
float h0 = 50; // height of the base
float h7 = 0; // end effector height
float b0 = 7.4806; // vertical distance of motor rotation axis to the base frame (diameter of the base)
float b1 = 10; // bicep length
float b2 = 20; // forearm length
float b7 = 3.5159; // diameter of end effector
float x = 0, y = 0, z = 30; // x,y and z represents the distance from that axis of the end effector
String inputString = ""; // to keep the given inputs
float p[3]; // to keep the position vector of end effector
float r[3][3]; // position vector from the beginning of the bicep and end of the forearm
int bSwitch = -1; // switch variable to switch between joystick and serial monitor to move the arm
int bSwitch2 = 1; // switch variable to switch between x-y axis and z axis to move the arm
// In variables part, we're creating necessary variables to used in calculations. This part includes arrays,
// button switch variables, link dimensinos, coordinates, matrices etc.
// ------------------------------------------------------------------Variables
// ------------------------------------------------------------------Rotation Matrix Creator
void updateRotMatrix(float th) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
rot_matrix[i][j] = 0; // creates a 3x3 matrix and makes each element equal 0
}
}
rot_matrix[0][0] = cos(th); // fills the necessary elements (from Computational Multi-body Dynamics lectures)
rot_matrix[0][1] = -sin(th); // fills the necessary elements (from Computational Multi-body Dynamics lectures)
rot_matrix[1][0] = sin(th); // fills the necessary elements (from Computational Multi-body Dynamics lectures)
rot_matrix[1][1] = cos(th); // fills the necessary elements (from Computational Multi-body Dynamics lectures)
rot_matrix[2][2] = 1.0; // fills the necessary elements (from Computational Multi-body Dynamics lectures)
}
// rotation matrix is defined with respect to given angle can be created by this function.
// ------------------------------------------------------------------Rotation Matrix Creator
// ------------------------------------------------------------------3x3 and 3x1 Matrix Multiplier
void matrixMultiply(float mat3x3[3][3], float mat3x1[3], float result[3]) {
for (int o = 0; o < 3; o++) {
float keeper = 0;
for (int p = 0; p < 3; p++) {
keeper += mat3x3[o][p] * mat3x1[p];
}
result[o] = keeper;
}
Serial.print("\n");
}
// Since there is no matrix multiplication function available, we created one to multiply 3x3 matrix by 3x1 matrix.
// This function can only multiply 3x3 and 3x1 matrices (matrices should be given respectively).
// ------------------------------------------------------------------3x3 and 3x1 Matrix Multiplier
// ------------------------------------------------------------------Inverser
void inverse(float mat[][3]) {
float keeper[3][3]; // creating a matrix to keep the inverted elements of given matrix
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
keeper[j][i] = mat[i][j];
}
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
mat[i][j] = keeper[i][j]; // equates the given matrix to its inverted version
}
}
}
// This function takes the inverse of a given matrix.
// ------------------------------------------------------------------Inverser
// ------------------------------------------------------------------Boolean Function for Errors
bool checkError(float theta[3], float z) {
if (theta[0] > 180 || theta[1] > 180 || theta[2] > 180) {
Serial.print("Motors can't rotate over 180 degree");
return 0;
} else if (theta[0] < 0 || theta[1] < 0 || theta[2] < 0) {
Serial.print("Motors can't rotate under 0 degree\n");
return 0;
} else if ((theta[0] == theta[1] && theta[1] == theta[2]) && (theta[0] < 92 && theta[0] > 88)) {
Serial.print("Singularity, please give different coordinates\n");
return 0;
} else if (theta[0] == theta_p[0] || theta[1] == theta_p[1] || theta[2] == theta_p[2]) {
Serial.print("Singularity, please give different coordinates\n");
return 0;
} else if (z > 40 || z < 22) {
Serial.print("Z coordinate cannot be equal\n");
return 0;
} else if (isnan(theta[0]) || isnan(theta[1]) || isnan(theta[2])) {
Serial.print("Cannot go there");
return 0;
} else {
return 1;
}
}
// This error function is used to detect if there is something wrong with the calculated motor angles.
// This function also includes the singularities.
// ------------------------------------------------------------------Boolean Function for Errors
// ------------------------------------------------------------------Calculation Function, Gives degrees of Motors
void calculate(float x, float y, float z) {
Ap0[0] = x; // Ap0 matrix keeps the given x,y and z values
Ap0[1] = y;
Ap0[2] = z;
for (int i = 0; i < 3; i++) {
p[i] = Ap0[i] - O[i]; // position vector of end effector
}
for (int i = 0; i < 3; i++) {
updateRotMatrix(beta[i]); // creates the rotation matrix for a given beta angle
inverse(rot_matrix); // takes the inverse of the rotatin matrix
matrixMultiply(rot_matrix, p, result); // multiplies the rotation matrix by the position matrix
for (int j = 0; j < 3; j++) {
r[i][j] = result[j] - u1[j] * (b0 - b7) - u3[j] * (h0 - h7); // calculates r vector for each axis
}
f[i] = (r[i][0] * r[i][0] + r[i][1] * r[i][1] + r[i][2] * r[i][2] + b1 * b1 - b2 * b2) / (2 * b1);
theta_d[i] = -atan2(r[i][2], r[i][0]);
gamma[i] = atan2(sqrt(r[i][0] * r[i][0] + r[i][2] * r[i][2] - f[i] * f[i]), f[i]);
theta[i] = theta_d[i] + sigma[i] * gamma[i];
theta[i] = theta[i] * 180 / PI;
theta[i] = 90 - theta[i];
theta_p[i] = -atan2(sigma_p[i] * (r[i][2] + b1 * sin(theta[i])), sigma_p[i] * (r[i][0] - b1 * cos(theta[i])));
// this equations are adapted from the Selçuk Hoca's notes
Serial.print(theta[i]); // prints the theta values for each motor
Serial.print(" ");
}
Serial.print("\n");
}
// calculate function is used to find the angles of motors at the given positions of end effector.
// the output of calculate function is the theta values of motors (in degree)
// ------------------------------------------------------------------Calculation Function, Gives degrees of Motors
// ------------------------------------------------------------------Motor Manipualation Function
void motorWrite(float ti[3]) {
int tf = 40;
float f;
float tc;
motor1.write(ti[0]);
motor2.write(ti[1]);
motor3.write(ti[2]);
delay(500);
for (int t = 0; t <= tf; t++) {
for (int i = 0; i < 3; i++) {
if (ti[i] < theta[i]) {
tc = (theta[i] - ti[i]) / 2 * cos(PI * t / tf + PI) + (theta[i] + ti[i]) / 2;
} else {
tc = (-theta[i] + ti[i]) / 2 * cos(PI * t / tf) + (theta[i] + ti[i]) / 2;
}
int a = i % 3;
switch (a) {
case 0:
motor1.write(tc);
break;
case 1:
motor2.write(tc);
break;
case 2:
motor3.write(tc);
break;
}
Serial.print(tc);
Serial.print(" ");
}
Serial.print("\n");
delay(20);
}
}
// This function provides a smooth rotation for motors.
// The function is adapted from the calculations in Selçuk Hoca's lecture notes.
// ------------------------------------------------------------------Motor Manipualation Function
void setup() {
Serial.begin(9600);
pinMode(buttonPin, INPUT);
pinMode(buttonPin2, INPUT);
motor1.attach(9); // Connection of the motors to arduino
motor2.attach(10); // Connection of the motors to arduino
motor3.attach(11); // Connection of the motors to arduino
calculate(x, y, z);
motor1.write(theta[0]); // Initial positions of motors
motor2.write(theta[1]); // Initial positions of motors
motor3.write(theta[2]); // Initial positions of motors
Serial.print("Arduino is ready!\n");
delay(1000);
}
void loop() {
float xi = x, yi = y, zi = z; // keeping the initial position of end effector
float ti[3] = { theta[0], theta[1], theta[2] }; // keeping the initial angles of motors
// If there occurs an error, the previous angles and position will be recalled.
byte buttonState = digitalRead(buttonPin);
// -------------------------------------------------- checks if the button is pressed or not
if (buttonState == LOW) {
//Serial.print("Burdayım");
//x = 0;
//y = 0;
//calculate(x, y, z);
//motorWrite(ti);
//xi = x;
//yi = y;
//zi = z;
//bSwitch = bSwitch * -1;
//Serial.print("Mode Switched. \n");
//delay(1000);
}
// --------------------------------------------------
// If button is pressed, switch variable and the mode changes.
if (bSwitch == 1) { // joystick mode (joystick should be used to move the arm)
byte buttonState2 = digitalRead(buttonPin2);
if (buttonState2 == LOW) {
x = 0;
y = 0;
calculate(x, y, z);
motorWrite(ti);
xi = x;
yi = y;
zi = z;
bSwitch2 = bSwitch2 * -1;
delay(100);
}
if (bSwitch2 == 1) { // moving along x and y axis
int x1 = analogRead(A0);
int y1 = analogRead(A1);
if (x1 > 612) {
x = x + 1;
} else if (x1 < 411) {
x = x - 1;
}
if (y1 > 612) {
y = y + 1;
} else if (y1 < 411) {
y = y - 1;
}
} else { // moving along z axis
int z1 = analogRead(A1);
if (z1 > 612) {
z = z + 0.5;
} else if (z1 < 411) {
z = z - 0.5;
}
}
calculate(x, y, z);
if (checkError(theta, z)) { // checks error, if there is no error, arm is moved to destination
motor1.write(theta[0]);
motor2.write(theta[1]);
motor3.write(theta[2]);
} else { // if there is an error, previous position and angles are recalled
x = xi;
y = yi;
z = zi;
theta[0] = ti[0];
theta[1] = ti[1];
theta[2] = ti[2];
Serial.print("Can't go there");
delay(1000);
}
delay(300);
} else { // serial monitor mod (the position of end effector should be given by using serial monitor)
if (Serial.available() > 0) { // checks if position inputs are given, if they are, gets the values
char a[5], b[5], c[5];
String inputString = Serial.readStringUntil('\n');
inputString.trim(); // Remove leading/trailing whitespaces
inputString.toCharArray(inputBuffer, BUFFER_SIZE); // Convert to C-style string
int parsed = sscanf(inputBuffer, "%s %s %s", a, b, c);
x = atoi(a); // converts the given string to integers and takes as the coordinates of end effector
y = atoi(b); // converts the given string to integers and takes as the coordinates of end effector
z = atoi(c); // converts the given string to integers and takes as the coordinates of end effector
delay(200);
}
calculate(x, y, z);
if (checkError(theta, z)) { // checks error, if there is no error, arm is moved to destination
if (x == xi && y == yi && zi == z) {
} else {
motorWrite(ti);
}
} else { // if there is an error, previous position and angles are recalled
Serial.print("Hata var");
x = xi;
y = yi;
z = zi;
theta[0] = ti[0];
theta[1] = ti[1];
theta[2] = ti[2];
delay(5000);
}
delay(400);
}
}