Sevos.c

#include <Servo.h>
#include <math.h>
const int servo1 = 3;       // first servo
const int servo2 = 10;       // second servo
const int joyH = 5;        // L/R Parallax Thumbstick
const int joyV = 4;        // U/D Parallax Thumbstick
int l1 = 5;
int a= 0;
int b =0;
int servoVal;  // variable to read the value from the analog pin
double angle1=0;
double angle2=0;

Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo



void setup() {

  // Servo  
  myservo1.attach(servo1);  // attaches the servo
  myservo2.attach(servo2);  // attaches the servo

  // Inizialize Serial
  Serial.begin(9600);
}


void loop(){

    // Display Joystick values using the serial monitor
    outputJoystick();

    // Read the horizontal joystick value  (value between 0 and 1023)
    servoVal = analogRead(joyH);          
    servoVal = map(servoVal, 0, 1023, 0, 180);     // scale it to use it with the servo (result  between 0 and 180)
    a = servoVal;
    myservo2.write(servoVal);                         // sets the servo position according to the scaled value    

    // Read the horizontal joystick value  (value between 0 and 1023)
    servoVal = analogRead(joyV);           
    servoVal = map(servoVal, 0, 1023, 70, 180);     // scale it to use it with the servo (result between 70 and 180)
    b = servoVal;
    myservo1.write(servoVal);                           // sets the servo position according to the scaled value
    angle1 = l1*cos(a+b)+l1*cos(a);
    angle2 = l1*sin(a+b)+l1*sin(a);
    outputJoystick();
    delay(15);                                       // waits for the servo to get there

}


/**
* Display joystick values
*/
void outputJoystick(){

    Serial.print(analogRead(angle1));
    Serial.print ("---"); 
    Serial.print(analogRead(angle2));
    Serial.println ("----------------");
}