const int MAX_NEGRO=30,v_max=210,v_max_giro=250,v_min=40;
const int motor_izq_p=9, motor_izq_n=10, motor_der_p=5, motor_der_n=6,
sensor_izq=A1, sensor_der=A0;
int medicion_izq, medicion_der,v_actual=50,v_izq,v_der;
void doblar(const int motor_1,const int motor_2,int* vel_1,int *vel_2, int vmax,int vmin);
void setup() {
pinMode(sensor_izq, INPUT);
pinMode(sensor_der, INPUT);
digitalWrite(motor_izq_p,LOW);
digitalWrite(motor_izq_n,LOW);
digitalWrite(motor_der_p,LOW);
digitalWrite(motor_der_n,LOW);
}
void loop() {
medicion_izq = analogRead(sensor_izq);
medicion_der = analogRead(sensor_der);
if(medicion_izq<MAX_NEGRO && medicion_der<MAX_NEGRO)/*ADELANTE*/{
if(v_actual<=v_max){
v_izq=v_actual;
v_der=v_actual;
analogWrite(motor_der_p,v_der);
analogWrite(motor_izq_p,v_izq);
v_actual++;
}
if(v_actual > v_max)
v_actual = v_max;
}
if(medicion_izq>MAX_NEGRO && medicion_der<MAX_NEGRO)/*DOBLAR IZQUIERDA*/{
doblar(motor_izq_p,motor_der_p,&v_izq,&v_der,v_max_giro,v_min);
}
if(medicion_izq<MAX_NEGRO && medicion_der>MAX_NEGRO)/*DOBLAR DERECHA*/{
doblar(motor_der_p,motor_izq_p,&v_der,&v_izq,v_max_giro,v_min);
}
if(medicion_izq>MAX_NEGRO && medicion_der>MAX_NEGRO)/*BLANCO - BLANCO*/{
}
}
void doblar(const int motor_1,const int motor_2,int *vel_1,int *vel_2, int vmax,int vmin){
if((*vel_1)>vmin){
(*vel_1)*=0.3;
analogWrite(motor_1,*vel_1);
}
else if(*vel_2<=vmax){
(*vel_2)*=1.2;
analogWrite(motor_2,*vel_2);
}
}