// Styring av høyre og venstre servomotor for rettlinjet bevegelse.

#include <Servo.h> 
 
Servo myleftservo;  // Definer venstre servomotor 
Servo myrightservo; // Definer høyre servomotor 
Servo mytiltservo;  // Definerer tiltservoen
int V_x1, V_y1, V_x2;
int korV_x1 = 508; // Korreksjonsverdi for Joy stick V_x1 i hvileposisjon legges inn her
int korV_y1 = 514; // Korreksjonsverdi for Joy stick V_y1 i hvileposisjon legges inn her
int korV_x2 = 500; // Korreksjonsverdi for Joy stick V_x2 i hvileposisjon legges inn her

void setup() 
{ 
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  
  myrightservo.attach(9);  // attaches the right servo on pin 9 to the servo object
  myleftservo.attach(11);  // attaches the left servo on pin 10 to the servo object  
  mytiltservo.attach(10);  // attaches the tilt servo on pin 11 to the servo object 
  Serial.begin(9600);
} 

 
void loop() 
{
    V_x1 = analogRead(1)-korV_x1; // Korreksjonsverdi for Joy stick V_x1 i hvileposisjon
    V_y1 = analogRead(2)-korV_y1; // Korreksjonsverdi for Joy stick V_y1 i hvileposisjon
    V_x2 = analogRead(3)-korV_x2; // Korreksjonsverdi for Joy stick V_x2 i hvileposisjon
    
    // Skriv Joy stick verdier til monitor
    Serial.print("V_x1: ");
    Serial.print(V_x1);
    Serial.print(", V_y1: ");
    Serial.print(V_y1);
    Serial.print(", V_x2: ");
    Serial.println(V_x2);
    
    delay(20);
    mytiltservo.writeMicroseconds(1500+V_x2);       // Kjør tiltservo
    myleftservo.writeMicroseconds(1500+V_x1-V_y1);  // Kjør venstre servo
    myrightservo.writeMicroseconds(1500+V_x1+V_y1); // Kjør høyre servo
}