//initial co-ordinates should be in the range of 50 to 65
//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; 
int r;

//====================================================================================================================================================================
//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=47;
float Y=0;//initial Co-ordinates of the endeffector where you want to draw a line
double length1=10;//length of the line that you want to draw
double i=0;
int t;
//int i_i;
 int i_1;
int i_2;
int checker;

// 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);

Base.write(90); //set Base Servo at Angel 90 with speed of 20
delay(1000);
Elbow.write(0); //set Elbow Servo at Angel 0 with speed of 20
delay(1000);

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

Base.write(90); //set Base Servo at Angel 90 with speed of 20
delay(1000);
Elbow.write(0); //set Elbow Servo at Angel 0 with speed of 20
    
}





}
//====================================================================================================================================================================
void loop()
{
    


}





void move_arm()

{
    Serial.print("move arm is called");


for (t= 0; t<X; t++)           // Get t value
  {
   
    //Serial.println(t);
    //CalculateServoAngles();           // Calculate angle of servos
    theta2=acos((pow(t,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));       // Equation of the inverse kinematics for two link planar manipulator
    theta1=atan(Y/t)-atan((L2*sin(theta2))/(L1+L2*cos(theta2)));          //Equation of the inverse kinematics for two link planar manipulator
    theta1=-theta1;                                                       // Alignment of theta 1 for elbow up solution
    theta2=-theta2;                                                       //Alignment of theta 2 for elbow up solution
    theta1=theta1*(180/pi);                                             //Conversion of theta1(radian) to degree
    theta2=theta2*(180/pi);                                             //Conversion of theta2(radian) to degree
    theta2=90+theta2; 
        
   
   //Base.write(theta1);  //Writing value to the servo
   //Elbow.write(theta2); 
    //   int i_1;
    //   int i_2;
//     while( i_1 <theta1 and  i_2 < theta2){
//       Serial.print("i_1");
//       Serial.println(i_1);
//       Serial.print("i_2");
//       Serial.println(i_1);
     
//       Base.write(i_1);  //Writing value to the servo
//       Elbow.write(i_2); 


//checker= 1;
//i_1 =0;
      
      
   
// }
  }

      

  
    //}
        //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);

    delay(100);
}
  



//====================================================================================================================================================================
void CalculateServoAngles() //Inverse Kinamatics Calaulations
{
theta2=acos((pow(t,2)+pow(Y,2)-pow(L1,2)-pow(L2,2))/(2*L1*L2));       // Equation of the inverse kinematics for two link planar manipulator
theta1=atan(Y/t)-atan((L2*sin(theta2))/(L1+L2*cos(theta2)));          //Equation of the inverse kinematics for two link planar manipulator
theta1=-theta1;                                                       // Alignment of theta 1 for elbow up solution
theta2=-theta2;                                                       //Alignment of theta 2 for elbow up solution
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 of the servo
}