//initial co-ordinates should be in the range of 47 to 68
//length of line that you want to draw should be in the range of 0 to 15
#include <Servo.h>
#include<math.h>
// create servo object to control a servo
Servo Base;
Servo Elbow;
//===================================================================================
//Unknown (Servo Angles)
float theta1=0; // for the Base Servo
float theta2=0; // for the Elbow Servo
const float pi = M_PI; //Store pi in a less annoying format
//===================================================================================
//known // Distance variables
float X=37;
float Y=0;//initial Co-ordinates of the endeffector where you want to draw a line
double length1=15;//length of the line that you want to draw
double i=0;
//===================================================================================
// Define arm Constants
int L1=31;//length of link 1
int L2=37;//length of link2
//===================================================================================
void setup() {
Base.attach(9); // attaches the servo 1 on pin 9 to the servo object
Elbow.attach(10);//attaches the servo 2 on pin 10 to the servo object
Serial.begin(9600);
//take to home position

for ( int i; i < 5; i++){
  move_arm();


Elbow.write(0);
delay(500);
Base.write(90);
delay(500);
  delay(1000);



}

//move_arm();


// Elbow.write(0);
// delay(500);
// Base.write(90);
// delay(500);
//===================================================================================
}
void loop(){


}


//void(* resetFunc) (void) = 0;  // declare reset fuction at address 0




void move_arm()
{
  i=0;
while(i<=1)
 {
    Serial.println(i);

Inverse_Kinamatics_Calaulations();
i=(length1/15);
X=X+i;
Base.write(theta1); //Writing value to the servo
Elbow.write(theta2); // sets the servo position according to the scaled value
delay(75);
//Priny info for Serial Monitor
Serial.print("X is:");
Serial.println(X);
Serial.print("Y is:");
Serial.println(Y);
Serial.print("theta1:");
Serial.println(theta1);
Serial.print("theta2:");
Serial.println(theta2);
if (X>67)
{i=2;}
 }
// delay(500);
// //take to home position
// delay(500);
// Base.write(90);
// delay(500);
// Elbow.write(0);
// delay(500);
}
//===================================================================================
//Inverse Kinamatics Calaulations
void Inverse_Kinamatics_Calaulations()
{
theta2=acos((pow(X,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2)); // Equation of the inversekinematics for two link planar manipulator
theta1=atan(Y/X)-atan((L2*sin(theta2))/(L1+L2*cos(theta2))); //Equation of the inverse kinematicsfor two link planar manipulator
theta1=-theta1; // Alignment of theta 1 for elbow up solution - quadrant
theta2=-theta2; //Alignment of theta 2 for elbow up solution - quadrant
theta1=theta1*(180/pi); //Conversion of theta1(radian) to degree
theta2=theta2*(180/pi); //Conversion of theta2(radian) to degree
theta2=90+theta2; // Adding the Correction Factor due to zero position ofthe servo
theta1= round(theta1);
theta2= round(theta2);

 // move_arm();
 //resetFunc(); //call reset

}