MeArm

#include <config.h>
#include <meArm.h>
#include <ik.h>
#include <fk.h>
#include <Servo.h>
#include <math.h>

int shoulderPin = 7;
int elbowPin = 8;
MeArm arm;
Servo myservo;
Servo myservo2;//end-effector
int angle=0;
int pos=0;
int len=0;
int py_len=0;
void setup() {
  Serial.begin(9600);
  arm.begin(10, shoulderPin, elbowPin, 14);
  myservo.attach(9); 
  myservo2.attach(6); 
}
void loop() {
   //Serial.print(pos);
    myservo2.write(0);
    delay(500); 
    py_len=290;
    len=abs(py_len-215)/1.5+80;
    angle=117;
  if(pos==0){
      if(angle>90)
        angle+=9;
      for (pos = 0; pos <= angle; pos++) {
        myservo.write(pos);
        delay(60);
        } 
       
    }
  arm.moveToXYZ(0,90,len); 
  myservo2.write(170);
  delay(20000);
}