Estoy haciendo un programa captar los pulsos de un sensor (solo 1 y 0) y contar cuantos fueron, una vez los cuenta, verificar que este en un rango aceptado, de no ser asi, deberan moverse los servos. si esta en un rango aceptado, volvera a medir hasta que salga del rango y se acomode solo. hasta el momento el programa funciona un para de ciclos, pero luego se descontrola.
#include <Servo.h> //Libreria de control del servo
Servo ServoMotor; //LLamado del servo
int tiempo; //Contador de tiempo
int tiempo2; //Comparacion de tiempo
int dif=1000; //Diferencia en milisegundos con respecto al tiempo
int c; //Contador
int c2; //Valor enviado a JAVA
int value; //Valor del sensor
int value2; //Comparacion anti-rebote del sensor
int velmin=200; //Velocidad minima aceptada
int velmax=500; //Velocidad maxima aceptada
int angulo=0; //Pocision inical del servo
int angmin=0; //Pocision minima del servo
int angmax=180; //Pocision maxima del servo
int angvar=30; //Aumento o decremento de angulo en cada correccion
void setup()
{
ServoMotor.attach(9); //Puerto del servomotor
Serial.begin(9600); //Iniciar puerto serie
pinMode(2,INPUT); //Entrada de datos del sensor
ServoMotor.write(angulo); //Pocisionamiento inicial del servo en angulo inical
}
void loop()
{
millis();
c = 0;
Serial.println("Inicio etapa de medicion:");
Serial.print("Angulo= ");
Serial.println(angulo);
tiempo = millis();
tiempo2 = ((tiempo + dif) - millis());
while(millis() <= (tiempo + dif))
{
tiempo2 = ((tiempo + dif) - millis());
Serial.print("Tiempo= ");
Serial.println(tiempo2);
value = digitalRead(2); //lectura digital de pin
value2 = value;
if (value == HIGH)
{
c++;
Serial.print("Contador= ");
Serial.println(c);
}
while(value == value2) //comparacion de cambios en la lectura
{
value = digitalRead(2);
if(millis() >= (tiempo + dif))
{
break;
}
}
}
c2=c;
//Aqui poner lo de JAVA
Serial.println("fin de la etapa de medicion");
while((c < velmin) || (c > velmax))
{
Serial.println("Correccion de angulo");
if ((c < velmin) || (c > velmax))
{
c = 0;
for(angulo = 0; angulo < angmax; angulo += angvar)
{
ServoMotor.write(angulo);
Serial.print("Angulo= ");
Serial.println(angulo);
tiempo = millis();
while(millis() < (tiempo + dif))
{
tiempo2 = ((tiempo + dif) - millis());
Serial.print("Tiempo= ");
Serial.println(tiempo2);
value = digitalRead(2); //lectura digital de pin
value2 = value;
if (value == HIGH)
{
c++;
Serial.print("Contador= ");
Serial.println(c);
}
while(value == value2) //comparacion de cambios en la lectura
{
value = digitalRead(2);
if(millis() >= (tiempo + dif))
{
break;
}
}
}
if ((c>velmin) && (c<velmax))
{
break;
}
c2 = c;
//Aqui poner lo de JAVA
c = 0;
Serial.println("Correccion de angulo");
}
}
if ((c < velmin) || (c > velmax))
{
c = 0;
for(angulo = 180; angulo > angmin; angulo -= angvar)
{
ServoMotor.write(angulo);
Serial.print("Angulo= ");
Serial.println(angulo);
tiempo = millis();
while(millis() <= (tiempo + dif))
{
tiempo2 = ((tiempo + dif) - millis());
Serial.print("Tiempo= ");
Serial.println(tiempo2);
value = digitalRead(2); //lectura digital de pin
value2 = value;
if (value == HIGH)
{
c++;
Serial.print("Contador= ");
Serial.println(c);
}
while(value == value2) //comparacion de cambios en la lectura
{
value = digitalRead(2);
if(millis() >= (tiempo + dif))
{
break;
}
}
}
if ((c > velmin) && (c < velmax))
{
break;
}
c2 = c;
//Aqui poner lo de JAVA
c = 0;
Serial.println("Correccion de angulo");
}
}
}
Serial.println("Correccion de velocidad realizada");
}
proyecto.ino (3.96 KB)