The following code gets three input angles from serial communication and writes them as outputs to servo motors. It also does some equations with these input angles and stores the outputs in variables x,y, and z. I wanted to take the input angles from a 4x4 matrix keypad instead of serial communication.
Thank you in advance.
#include <LiquidCrystal.h>
#include <Servo.h>
#define basepin 9 //pins for the servo motors
#define rightpin 14
#define leftpin 15
#define grippin 16
Servo base;
Servo right;
Servo left;
Servo gripper;
#define rs 12 //pins for LCD display
#define en 11
#define d4 5
#define d5 4
#define d6 3
#define d7 2
LiquidCrystal lcd (rs,en,d4,d5,d6,d7);
#define l1 2.6 //length of links measured manually
#define l2 8.85
#define l3 11.4
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
lcd.begin(16,2); //lcd screen dimensions
base.attach(basepin); //allocating a servo for each pin
right.attach(rightpin);
left.attach(leftpin);
gripper.attach(grippin);
Serial.println("Enter Angle Values");
}
void loop() {
// put your main code here, to run repeatedly:
/* Transformation Matrix = [ c1*c2*c3 - c1*s2*s3, - c1*c2*s3 - c1*c3*s2, s1, c1*c2*l2 - c1*l3*s2*s3 + c1*c2*c3*l3]
[ c2*c3*s1 - s1*s2*s3, - c2*s1*s3 - c3*s1*s2, -c1, c2*l2*s1 + c2*c3*l3*s1 - l3*s1*s2*s3]
[ c2*s3 + c3*s2, c2*c3 - s2*s3, 0, l2*s2 + c2*l3*s3 + c3*l3*s2 + 13/5]
[ 0, 0, 0, 1]*/
int thetadeg1 = Serial.parseInt(); //input of angle values in degree from serial communication
int thetadeg2 = Serial.parseInt();
int thetadeg3 = Serial.parseInt();
float thetarad1 = thetadeg1*(PI/180); //degree to rad conversion
float thetarad2 = thetadeg2*(PI/180);
float thetarad3 = thetadeg3*(PI/180);
float c1 = cos(thetarad1); //renaming the functions for easy reading
float c2 = cos(thetarad2);
float c3 = cos(thetarad3);
float s1 = sin(thetarad1);
float s2 = sin(thetarad2);
float s3 = sin(thetarad3);
base.write(thetadeg1); //writing the input angles onto the servos
right.write(thetadeg2);
left.write(thetadeg3);
gripper.write(180);
float X = (c1*c2*l2) - (c1*l3*s2*s3) + (c1*c2*c3*l3) ; //values of x, y, and z from the transformation matrix
float Y = (c2*l2*s1) + (c2*c3*l3*s1) - (l3*s1*s2*s3) ;
float Z = (l2*s2) + (c2*l3*s3) + (c3*l3*s2) + l1 ;
Serial.print("X = "); //show the coordinates values on Serial monitor
Serial.println(X);
Serial.print("Y = ");
Serial.println(Y);
Serial.print("Z = ");
Serial.println(Z);
Serial.println(" ");
delay(10000);
int x = X; //show the cordinates on LCD display
lcd.print("X=");
lcd.print(x);
lcd.print(" ");
int y = Y;
lcd.print("Y=");
lcd.print(y);
lcd.print(" ");
int z = Z;
lcd.print("Z=");
lcd.print(z);
lcd.print(" ");
lcd.setCursor(0, 0);
}