#include <Wire.h>
#include <Servo.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "U8glib.h"
//#include <Adafruit_PWMServoDriver.h>
//#include <PID_v1.h>
//#include <Arduino.h>                       
//#include <Ramp.h> 

#define A1 110.5
#define A2 150
#define A3 150
#define Servo1 = 0;
#define Servo2 = 1;
#define Servo3 = 2;
#define Servo3 = 3;

#define SERVOMIN_1  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_1 600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVOMIN_2  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_2 600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVOMIN_3  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_3 600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVOMIN_4  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX_4 600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

float X = 0; // posizione X
float Y = 0; // posizione Y
float Z = 0; // posizione Z
float ang1;
float ang2;
float ang3;
float ang4;
float dX = 0;
float dY = 0;
float dZ = 0;
float risoluzione = 50;

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;

int precstato = 0;

void setup() {

  Serial.begin(115200);
  
  pinMode(13, OUTPUT);
  pinMode(25,INPUT_PULLUP);
  myservo1.attach(2);
  myservo2.attach(3);
  myservo3.attach(4);
  myservo4.attach(5);
  
  delay(10);

  setRisoluzione(20);
  moveLinear(0,0,0);
  moveCircular(200,0,0,100,100,0);
  //moveCircular(100,100,0,150,50,0);
  //moveCircular(100,0,0,50,150,0);
  
  home();
}

void loop() {
  int stato = digitalRead(25);
  
  switch(stato){
    case 0:      // PROGRAMMAZIONE
      if(precstato != stato){
        //comandi da eseguire una volta
        Serial.println("Modalità programmazione");
        delay(1000);
        
      }
    precstato = 0;        
    break;

    case 1:   // MEMORIZZAZIONE
      if(stato != precstato){
        //comandi da eseguire una volta
        Serial.println("Modalità memorizzazione");
        delay(1000);
      }
    precstato = 1;      
    break;

  }

}