Mueve un condensador multivuelta con un PAP simula capacidad y vueltas

/* Mueve un motor PAP de 0 a VUELTAS_MAX 3 aqui en el ejemplo pero
 *  puede variarse , muestra el contaje de vueltas por el puerto serie
 *  y una prediccion de la capacidad de la capacidad del condensador basado
 *  en CAPACIDAD_MAX 220 , guarda los datos en MEMORIA EEPROM
 *
 *  Antonio Villanueva Segura
 *
 * EM 258 200 pasos en 360° 360/200= 1.8° cada paso Nominal Voltage 7
 * https://cdn-learn.adafruit.com/downloads/pdf/afmotor-library-reference.pdf
 * http://www.motors.wrobots.com/EM-258_17PM-K212-PIT.php
 */
#include <AFMotor.h>
#include <EEPROM.h>   //Uso de memoria EEPROM

int vuelta_mem = 0;   //Direccion EEPROM para las vueltas
int sentido_mem = 2;  //Direccion EEPROM para el sentido de giro
int capacidad_mem = 4;//Direccion EEPROM para la capacidad uf


#define VUELTAS_MAX 3
#define CAPACIDAD_MAX 220.0
#define PASOS_MOTOR 200

#define CAP_PASO  ( CAPACIDAD_MAX / (PASOS_MOTOR * (VUELTAS_MAX + 1) ) )

// to motor port #2 (M3 and M4)
AF_Stepper motor(200, 1);// M1 200 pasos

bool sentido=false;//Sentido de giro
int pasos=1;//Pasos que realiza el PAP
int vueltas=0;//Cuenta vueltas
float capacidad=0.0 ;//Capacidad condensador

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

  pinMode(A5, INPUT);  //A5 como entrada
  pinMode(A5, INPUT_PULLUP);

  Serial.println("Icaro TEST PAP!");

  motor.setSpeed(30);  // Velocidad RPM

  //clearMemory () ;//Clear EEPROM y HOMING  UTILIZAR PARA RESETEAR MEMORIA Y POSICION

  recuperaMemoria();//Lee datos de la memoria si existen ?
  Serial.println ("Intento recuperar datos de EEPROM");
  informacionSerie(vueltas, sentido , capacidad);//imprime datos EEPROM , luego mira si son correctos
    delay(1000);

  if ( vueltas < 0 ||(vueltas > VUELTAS_MAX) || capacidad > CAPACIDAD_MAX || capacidad <0 ){ //Analiza si los datos recuperados son correctos
    vueltas=0;
    sentido=false;
    capacidad=0.0;
    Serial.println ("No hay datos en memoria , busqueda HOME");
    delay (1000);
    HOME();

  }
}

void HOME (){ //Efectua HOME busca final de carrera a 0
      while (digitalRead(A5)!=0){
        Serial.println ("HOMING");
        motores (&motor,pasos,sentido);
        } //Home Busca el sensor a 0
}

//Funcion giro de motor PAP , argumento pasos y el sentido de giro
void motores (AF_Stepper *motor,int pasos,bool sentido){
    if (sentido ==true){
      (*motor).step(pasos, FORWARD, SINGLE); //Giro derecha
      capacidad = CAP_PASO + capacidad;
  }else{
     (*motor).step(pasos, BACKWARD, SINGLE); //Giro Izquierda
     capacidad =  capacidad -CAP_PASO;
  }

}

//Print informacion DEBUG vuelta actual , sentido , capacidad ...
void informacionSerie(int vueltas, bool sentido ,  float capacidad){

 Serial.print("Vuelta = ");//Debug
 Serial.print (vueltas);//Debug

 Serial.print(" , sentido = ");//Debug
 Serial.print (sentido);//Debug

 Serial.print(" , capacidad = ");//Debug
 Serial.print(capacidad,2);//Debug
 Serial.println(" uF");//Debug
}

void clearMemory(){//Limpia la Memoria EEPROM y efectua HOME
  for (int i = 0 ; i < EEPROM.length() ; i++) {
    EEPROM.write(i, 0);
  }
  HOME() ;//Efectua HOME tras limpiar datos de memoria
}

//Guarda en EEPROM los datos de las vueltas el sentido y la capacidad
void guardarEnMemoria( int vueltas, bool sentido , float capacidad){
  EEPROM.put( vuelta_mem, vueltas );  //Grabamos el valor
  EEPROM.put( sentido_mem, sentido );  //Grabamos el valor
  EEPROM.put( capacidad_mem, capacidad );  //Grabamos el valor
}

//Lee datos de la memoria EEPROM
void recuperaMemoria( ){
  EEPROM.get( vuelta_mem, vueltas );  //Grabamos el valor
  EEPROM.get( sentido_mem, sentido );  //Grabamos el valor
  EEPROM.get( capacidad_mem, capacidad );  //Grabamos el valor
}

void loop() {//Main LOOP

  if (digitalRead(A5)==0  ){ //Sensor una vuelta completa

    if ( sentido && vueltas < ( VUELTAS_MAX +1 ) ){vueltas++;} //Depende del sentido cuenta o descuenta vueltas
    else {vueltas--;}

    while (digitalRead(A5)==0){motores (&motor,pasos,sentido);};//Tiene que salir de la posicion 0 del sensor

    }

 informacionSerie(vueltas, sentido , capacidad); //Informacion serie RS232 ...

  //Ha llegado al limite de vueltas debe de cambiar de sentido y la forma de contar
  if ( vueltas>VUELTAS_MAX && sentido  ){ sentido = !sentido;} //Cambio de sentido en el limite
  if ( vueltas<=0  && ! sentido  ){ sentido = !sentido;capacidad=0.0;} //Cambio de sentido en el limite

  motores (&motor,pasos,sentido); //PAP

  guardarEnMemoria(vueltas, sentido , capacidad);//Guarda en memoria EEPROM los datos actuales

}