// STUDIO SERVO PER OSCILLAZIONE BACINELLA
// 
// 2024/04/21  velocità regolata con potenziometro

#include <Servo.h>
Servo servo1;

#define TRIMMER_PIN A0

unsigned long timeServo=millis();
// verificare durataServo con oscillazione liquido nella bacinella
unsigned long durataServo; 
int oldval_S=-1;

// angoli da valutare
int ang;
int ang1=30;
int ang2=150;
bool FIRST_S=true;
bool flag_S=true; // true -> incrementa  false -> decrementa


void setup() {
  Serial.begin(9600);

  servo1.attach(9);
  servo1.write(ang1);
  
}

void loop() {
  
  settaVel_S();
  oscilla();

}

void settaVel_S(void) {
  int val_S=analogRead(TRIMMER_PIN);
  if(val_S!=oldval_S) {
  if(val_S==0) { // servo fermo
    Serial.println("FERMO");
    durataServo=100;
    RipPosIn(ang);
  } else {  // servo in movimento
    // setta velocità
    durataServo=(unsigned long)map(val_S,0,1023,50,5); 
    
    }
  oldval_S=val_S;
    }
  }

void oscilla(void) { 
  if(FIRST_S) {ang=ang1;} 
  
  
  if(durataServo!=100) {
  if((millis()-timeServo)>durataServo) {
      if(flag_S) {
      ang++;
      } else {
      ang--;
      }
      servo1.write(ang);
      if(ang>ang2) flag_S=false;
      if(ang<ang1) flag_S=true;
      timeServo=millis();
    }
  }
  /**
  timeServo=millis();
  int ang=ang1;
  do {
      do {
        if(millis()-time>durata) break;
        if(millis()-timeServo>durataServo) {
        servo1.write(ang); Serial.println(ang);
        ang++;
        timeServo=millis();
        }
      } while(ang<=ang2);
    ang=ang2;
    do {
        if(millis()-time>durata) break;
        if(millis()-timeServo>durataServo) {
        servo1.write(ang);Serial.println(ang);
        ang--;
        timeServo=millis();
        }
      } while(ang>=ang1);
  } while(millis()-time<durata);
  Serial.println(millis()-time);
  Serial.println(ang);
  // ripristina posizione iniziale servo
  for(int i=ang;i>=ang1;i--) {
    servo1.write(i);
    delay(durataServo);
  }*/
  
  
  FIRST_S=false;
}

void RipPosIn(int pos) {
  for(int i=pos;i>=ang1;i--) {
    servo1.write(i);
    delay(durataServo);
    ang=ang1;
  }
}