SERVO CONTROL SYSTEM

Elenco funzioni:

• Macchina a stati
• Accelerazione esponenziale
• Filtro IIR
• EEPROM integrata
• Comandi seriali avanzati
• Attach/Detach intelligente

1. DESCRIZIONE DEL SISTEMA

• controllo manuale tramite potenziometro filtrato
• richiamo di due posizioni memorizzate
• velocita regolabile
• comandi seriali per regolazioni fini
• movimento con accelerazione esponenziale
• gestione intelligente del servo (attach/detach)
• macchina a stati deterministica
• nessun movimento all accensione

2. INGRESSI DIGITALI

• WORK - Abilita movimento (0 = fermo)
• MODE - 0 = Manuale, 1 = Memorie
• MEMPOS1 - Richiama posizione memorizzata 1
• MEMPOS2 - Richiama posizione memorizzata 2
• MODOTEST - Salva velocita (fronte 0→1)
• DETACH - 0 = detach a fine movimento, 1 = servo sempre attivo
• SERIALDEBUG - Attiva messaggi di debug
• SOFTMODE - Seleziona curva di accelerazione

3. MACCHINA A STATI

• ST_IDLE - Nessun movimento, servo detach
• ST_MANUAL - Movimento da potenziometro
• ST_MEM1 - Movimento verso pos1
• ST_MEM2 - Movimento verso pos2
• ST_TEST - Movimento manuale + salvataggio velocita

• WORK = 0 → ST_IDLE
• MODOTEST = 1 → ST_TEST
• MODE = 0 → ST_MANUAL
• MEMPOS1 = 0 → ST_MEM1
• MEMPOS2 = 0 → ST_MEM2
• Nessuna condizione → ST_IDLE

4. MEMORIE EEPROM

• pos1 - Posizione memorizzata 1
• pos2 - Posizione memorizzata 2
• potVelEE - Velocita salvata (1-255)

5. MODALITA OPERATIVE

• Il servo segue il potenziometro filtrato
• Velocita = potVelEE

• MEMPOS1 → pos1
• MEMPOS2 → pos2

• Movimento manuale
• Salvataggio velocita

• Nessun movimento
• Servo detach se permesso

6. COMANDI SERIALI (1-8)

• 1 - Salva posizione attuale → pos1
• 2 - Salva posizione attuale → pos2
• 3 - Salva velocita da potenziometro
• 4 - Stampa valori EEPROM

• 5 - -10°
• 6 - -1°
• 7 - +1°
• 8 - +10°

7. PROCEDURE RAPIDE

1. Portare il servo nella posizione desiderata
2. Inviare 1 (pos1) o 2 (pos2)

1. Regolare il potenziometro
2. Inviare 3

1. MODE = 1
2. Premere MEMPOS1 o MEMPOS2

8. NOTE OPERATIVE

• Il servo non si muove se WORK = 0
• Jog sempre disponibile via seriale
• Debug attivo solo con SERIALDEBUG = 1
• DETACH = 0 → servo disattivato a fine movimento

9. SICUREZZA

• Non forzare il servo
• Non superare 0-180°
• Usare alimentazione adeguata
• Non scollegare durante il movimento

/*
 * SERVO due posizioni con WATCHDOG e Rampa Soft
 * Logica: Al power-on ignora D2 per 2 secondi (lo forza HIGH/Aperto)
 * per evitare guizzi meccanici e letture spurie.
 */

#include 
#include 

// --- Costanti Pin ---
const int SERVO_PIN = 9;
const int INPUT_MOVE = 2;
const int INPUT_DETACH = 7;
const int LED_PIN = LED_BUILTIN;

// --- Configurazione Movimento ---
const int StartPosition = 170; //era 40  invertito il 5/5/26;
const int EndPosition = 80;    //era 170 invertito direzione 5/5/26;
const float ACCELERAZIONE = 0.01;
const float MAX_SPEED = 5.0;

// --- Variabili di stato ---
Servo myServo;
float currentPos;
float currentSpeed = 0;
int targetPos;
bool isAttached = false;
bool inMovement = false;
int lastTarget = -1;

unsigned long startTime;
unsigned long ignoreD2Until = 0;

// Funzione per estrarre solo il nome del file
const char* getFileName(const char* path) {
  const char* file = path;
  while (*path) {
    if (*path == '/' || *path == '\\') file = path + 1;
    path++;
  }
  return file;
}

void attachServo() {
  if (!isAttached) {
    myServo.attach(SERVO_PIN, 500, 2500);
    myServo.write((int)currentPos);
    isAttached = true;
    Serial.println(F("--- Servo Attached"));
  }
}

void detachServo() {
  if (isAttached) {
    delay(500); // Piccolo buffer prima di staccare
    myServo.detach();
    isAttached = false;
    Serial.println(F("--- Servo Detached"));
  }
}

void setup() {
  wdt_disable(); // Disabilita WD durante il boot
  
  pinMode(SERVO_PIN, OUTPUT);
  digitalWrite(SERVO_PIN, LOW);
  pinMode(LED_PIN, OUTPUT);

  Serial.begin(115200);
  Serial.println(F("------------------------------------"));
  Serial.print(F("File: "));
  Serial.println(getFileName(__FILE__));
  Serial.println(F("--- Reset Sistema Targa"));
  Serial.println(F("------------------------------------"));

  pinMode(INPUT_MOVE, INPUT_PULLUP);
  pinMode(INPUT_DETACH, INPUT_PULLUP);

  // --- FIX GUIZZO POWER-ON ---
  // Invece di leggere D2, forziamo la posizione iniziale a StartPosition
  currentPos = StartPosition;
  targetPos = StartPosition;
  lastTarget = StartPosition;
  
  // Posizioniamo virtualmente il servo prima di fare attach
  myServo.write((int)currentPos);
  attachServo();

  // Impostiamo il timer di blocco (2 secondi)
  ignoreD2Until = millis() + 2000;

  wdt_enable(WDTO_2S); // Attiva Watchdog 2 secondi
}

void loop() {
  wdt_reset(); // Reset del cane da guardia

  unsigned long currentMillis = millis();

  // --- LOGICA DOPPIO BLINK (100on-100off-100on-700off) ---
  unsigned long blinkModulo = currentMillis % 1000;
  if (blinkModulo < 100) {
    digitalWrite(LED_PIN, HIGH);
  } else if (blinkModulo < 200) {
    digitalWrite(LED_PIN, LOW);
  } else if (blinkModulo < 300) {
    digitalWrite(LED_PIN, HIGH);
  } else {
    digitalWrite(LED_PIN, LOW);
  }

  // --- DEBOUNCE INPUT_MOVE (D2) ---
  static int lastStable = HIGH;
  static int lastRaw = HIGH;
  static unsigned long lastChange = 0;

  int raw = digitalRead(INPUT_MOVE);
  if (raw != lastRaw) {
    lastRaw = raw;
    lastChange = millis();
  }
  if (millis() - lastChange > 40) {
    lastStable = raw;
  }

  // --- LOGICA DI FORZATURA D2 ---
  int newTarget;
  if (millis() < ignoreD2Until) {
    // Nei primi 2 secondi ignoriamo D2 e restiamo in StartPosition
    newTarget = StartPosition;
  } else {
    // Dopo 2 secondi usiamo il valore reale
    newTarget = (lastStable == HIGH) ? StartPosition : EndPosition;
  }

  // --- GESTIONE CAMBIO TARGET ---
  if (newTarget != lastTarget) {
    Serial.print(F("--- Cambio stato: "));
    Serial.println(newTarget == StartPosition ? F("APRI") : F("CHIUDI"));
    
    targetPos = newTarget;
    lastTarget = newTarget;
    
    if (!inMovement) {
      currentSpeed = 0; // Reset velocità per rampa morbida
      startTime = millis();
      inMovement = true;
      attachServo();
    }
  }
}