Control Condensador mariposa arduino

/*
 * Control de un condensador mariposa 180°
 * Antonio Villanueva Segura
 *
 *  *Modelo motor 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

#define CAPACIDAD_MAX 220
#define PASOS 200
#define RPM 30

// Define el puerto del MOTOR M1 y sus pasos
AF_Stepper motor(200, 1);// M1 200 pasos

float capacidad =0 ;//Datos capacidad
float capacidad_mem = 0;//Direccion EEPROM para la capacidad uf,100.000 ciclos de escritura por cada posición de memoria
bool sentido =true;

/**************************************************************************************************************/
void setup() {
  Serial.begin(9600);
  Serial.println("\n A.Villanueva Ctrl. Condensador aire 180° \n");
  motor.setSpeed(RPM);  // 10 rpm
  //borrarDatos(); //Pide si se quiere borrar datos de capacidad
  capacidad=recuperaMemoria(); //Lee datos de la capacidad desde la EEPROM

  Serial.println ("Recuperada capacidad de la memoria = ");
  Serial.println (capacidad);
  delay (2000);

}
/**************************************************************************************************************/
void loop() { //Bucle principal
  float nuevaCapacidad=-1; //Seleccion
  //testMovCondensador( sentido );//  sentido = !sentido; //Rutina para efectuar test repetitivo 0 a 180°

  nuevaCapacidad = menu();//Muestra menu y lee la nueva capacidad

  Serial.print ("Lectura menu = ");
  Serial.println (nuevaCapacidad);
  nuevaCapacidad=normalizaCapacidad (nuevaCapacidad);

  if (nuevaCapacidad != capacidad){ //Si tenemos una nueva capacidad , traducimos la capacidad en pasos
    capacidadPasos(nuevaCapacidad) ;
  }

  delay(1000);//1 segundo retardi
}
/**************************************************************************************************************/
//Test movimiento de 0 a 180°(100 pasos ) del motor PAP EM-258
void testMovCondensador(bool sentido ){//Mueve 0 a 180° el condensador
  int pasos=100; //0 a 180°
  motores (&motor, pasos, sentido); //Envia los pasos al motor
}

/**************************************************************************************************************/
//Normaliza el valor de la capacidad para ser multiplo de 2.2 un paso , p.e 4u6f  se normalizara a 4u4f
float normalizaCapacidad (float capacidad){
  float cap=0;
  while ( ((capacidad)- (cap +2.2) )>=0.0 ){ cap += 2.2;}
  Serial.print ("Valor normalizado = ");Serial.println (cap);

  return cap;
}

/**************************************************************************************************************/
//Traduce la capacidad a una secuencia de pasos
void capacidadPasos(float cap_new){
  //100 pasos equivalen a 180° y a una capacidad asociada de 220 pf , 220/100 =1paso =2p2f

  float capacidadDesplazamiento= cap_new - capacidad   ;//Cuanto se desplaza y en que direccion ?

  bool sentido = ( capacidadDesplazamiento < 0 ) ? true : false; //Sentido de desplazamiento

 if (! limitesCap(capacidadDesplazamiento, sentido)){return; } //Nueva capacidad fuera de limites
  float pasos = abs((capacidadDesplazamiento) / 2.2 ); //Multiplos de 2.2 , cada paso es 2u2F

  if (abs (pasos)  > 0){ //Si hay pasos actuo sobre el motor
    motores (&motor, abs (pasos), sentido); //Envia los pasos al motor
    capacidad=cap_new;//Actualiza la capacidad actual en memoria RAM
    guardaMemoria( capacidad);//Guarda en memoria EEPROM la capacidad por si hay problema 100.000 ciclos de escritura por cada posición de memoria
  }

}
/**************************************************************************************************************/
void borrarDatos (){//Borra datos EEPROM
  String lectura="";

    Serial.println ("Quieres borrar datos ? ");
  while (lectura== ""  ){lectura=leeCadena();}//Espera una lectura no nula
  if (lectura[0]=='S' or lectura[0]=='s' ){
    Serial.println ("Datos Borrados , ahora estamos en capacidad 0pf ");
    //clearMemory();
    guardaMemoria(0);//Guarda 0pf
  }
}
/**************************************************************************************************************/
//Lee una cadena de texto del puerto serie
String leeCadena(){
  String cadena="";
  if (Serial.available()) {cadena = Serial.readStringUntil('\n');}
  return cadena;
}
/**************************************************************************************************************/
//Menu lectura Serie, devuelve un int valor de capacidad
float menu(){
  String lectura="";

  Serial.println ("\n --- Menu ---  ");
  Serial.println (" C clear EEPROM");
  Serial.println (" R clear capacidad RAM = 0");
  Serial.println (" A Avanza     5 pasos  10 pf");
  Serial.println (" Q Retrocede  5 pasos -10 pf");
  Serial.println (" Z avanza     10 pasos 22 pf");
  Serial.println (" S Retrocede  10 pasos -22 pf");
  Serial.print   (" Capacidad actual = ");
  Serial.println(capacidad);

  Serial.print (" o Introduzca capacidad de 0 a ");
  Serial.println (CAPACIDAD_MAX);

  while (lectura== "" ){lectura=leeCadena();}//Espera una lectura no nula del serie

  return opciones (lectura);
}
/**************************************************************************************************************/
//Analiza si la nueva capacidad  esta dentro de los margenes del condensador
bool  limitesCap(float new_cap,bool sentido ){
  if (! sentido && (abs ( new_cap) < CAPACIDAD_MAX)){return true;} //Dentro de limite superior CAPACIDAD_MAXIMA
  if (sentido && (abs (new_cap)) >= 0){return true;}//Limite inferior 0

  return false;//Fuera de limites
}
/**************************************************************************************************************/
float opciones(String lectura){
  //Opciones

  if ( lectura[0]== 'C' || lectura[0]== 'c' ){clearMemory();}//Clear EEPROM
  if ( lectura[0]== 'R' || lectura[0]== 'r' ){capacidad=0; }//Clear RAM este punto es 0pf
  if ( lectura[0]== 'A' || lectura[0]== 'a' ){ lectura = ( capacidad + 10) ; }//AVANZA 5 pasos
  if ( lectura[0]== 'Q' || lectura[0]== 'q' ){ lectura = ( capacidad - 10 >= 0 ? capacidad - 10 :capacidad ) ;}//RETROCEDE 5 pasos
  if ( lectura[0]== 'Z' || lectura[0]== 'z' ){ lectura = ( capacidad + 22) ;}//AVANZA 10 pasos
  if ( lectura[0]== 'S' || lectura[0]== 's' ){ lectura = ( capacidad - 22 >= 0? capacidad - 22 :capacidad ) ;}//RETROCEDE 10 pasos


  //Se trata de una capacidad analiza si esta en los limites
  //Analiza el uso de un valor capacitivo directamente
  //Analiza que la capcidad leida sea correcta si es erroneo devuelve -1

  if ( lectura.toFloat() >220 or lectura.toFloat() <0){
    Serial.println("ERROR ! Seleccion fuera de limites !");
    return capacidad;
  }

  return lectura.toFloat();

}
/**************************************************************************************************************/
//Guarda en EEPROM la capacidad
void guardaMemoria( float capacidad){
  Serial.print("EEPROM save =");
  Serial.println (capacidad);
  EEPROM.put( capacidad_mem, capacidad );  //Grabamos el valor
}
/**************************************************************************************************************/
//Lee datos de la capacidad en la memoria EEPROM
float recuperaMemoria( ){
  return EEPROM.get( capacidad_mem, capacidad );//Recupera valor capacidad actual
}
/**************************************************************************************************************/
void clearMemory(){//Limpia la Memoria EEPROM
  Serial.println ("\n Clear EEPROM y RAM capacidad a 0 ! ");
  capacidad=0;//Limpia RAM
  for (int i = 0 ; i < EEPROM.length() ; i++) {
    EEPROM.write(i, 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
  }else{
     (*motor).step(pasos, BACKWARD, SINGLE); //Giro Izquierda
  }

}
/**************************************************************************************************************/