Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*------------------------------------------ */
- /* programa seguidor de línea robot arduino */
- /*------------------------------------------ */
- #include <LEANTEC_ControlMotor.h> //Incluimos la libreria para el control de motores.
- // Conecta los sensores
- #define LEFT_SENSORPIN 11
- #define RIGHT_SENSORPIN 10
- // Configuramos los pines que vamos a usar de salida
- ControlMotor control(2,3,7,4,5,6);
- int MotorDer1=2;//El pin 2 de arduino se conecta con el pin In1 del L298N
- int MotorDer2=3;//El pin 3 de arduino se conecta con el pin In2 del L298N
- int MotorIzq1=7;//El pin 7 de arduino se conecta con el pin In3 del L298N
- int MotorIzq2=4;//El pin 4 de arduino se conecta con el pin In4 del L298N
- int PWM_Derecho=5;//El pin 5 de arduino se conecta con el pin EnA del L298N
- int PWM_Izquierdo=6;//El pin 6 de arduino se conecta con el pin EnB del L298N
- int velocidad=150;//Declaramos una variable para guardar la velocidad
- // Definimos variables de los sensores de entrada
- int sensor_R=0; // Variable para guardar el valor del sensor de la derecha
- int sensor_L=0; // Variable para guardar el valor del sensor de la izquierda
- void setup()
- {
- // Iniciamos el puerto serie para volcar datos al PC
- Serial.begin(9600);
- //Configuramos los pines como salida
- pinMode(MotorDer1, OUTPUT);
- pinMode(MotorDer2, OUTPUT);
- pinMode(MotorIzq1, OUTPUT);
- pinMode(MotorIzq2, OUTPUT);
- pinMode(PWM_Derecho, OUTPUT);
- pinMode(PWM_Izquierdo, OUTPUT);
- }
- void derecha_antihorario_izquierda_horario(){
- /*En esta fución la rueda derecha girará en sentido antihorario y la
- izquierda en sentido horario.
- En este caso, si las ruedas estuvieran en el chasis de un robot, el
- robot retrocederia.*/
- digitalWrite(MotorDer1,HIGH);
- digitalWrite(MotorDer2,LOW);
- digitalWrite(MotorIzq1,HIGH);
- digitalWrite(MotorIzq2,LOW);
- analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
- analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200
- }
- void derecha_horario_izquierda_antihorario(){
- /*En esta fución la rueda derecha girará en sentido horario y la
- izquierda en sentido antihorario.
- En este caso, si las ruedas estuvieran en el chasis de un robot, el
- robot avanzaría.*/
- digitalWrite(MotorDer1,LOW);
- digitalWrite(MotorDer2,HIGH);
- digitalWrite(MotorIzq1,LOW);
- digitalWrite(MotorIzq2,HIGH);
- analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
- analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200
- }
- void giro_horario(){
- /*En esta fución ambas ruedas girarán en sentido horario.
- En este caso, si las ruedas estuvieran en el chasis de un robot, el
- robot giraria a la derecha.*/
- digitalWrite(MotorDer1,HIGH);
- digitalWrite(MotorDer2,LOW);
- digitalWrite(MotorIzq1,LOW);
- digitalWrite(MotorIzq2,HIGH);
- analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
- analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200
- }
- void giro_antihorario(){
- /*En esta fución ambas ruedas girarán en sentido antihorario.
- En este caso, si las ruedas estuvieran en el chasis de un robot, el
- robot giraria a la izquierda.*/
- digitalWrite(MotorDer1,LOW);
- digitalWrite(MotorDer2,HIGH);
- digitalWrite(MotorIzq1,HIGH);
- digitalWrite(MotorIzq2,LOW);
- analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
- analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200
- }
- void parar(){
- /*Función para que las ruedas paren*/
- digitalWrite(MotorDer1,LOW);
- digitalWrite(MotorDer2,LOW);
- digitalWrite(MotorIzq1,LOW);
- digitalWrite(MotorIzq2,LOW);
- analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
- analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200
- }
- void loop()
- {
- // Leemos sensores L y H
- sensor_L=digitalRead(LEFT_SENSORPIN);
- sensor_R=digitalRead(RIGHT_SENSORPIN);
- // Visualizamos
- Serial.print(" Left : ");
- Serial.print(sensor_L);
- Serial.print(" Right : ");
- Serial.print(sensor_R);
- Serial.println();
- // En función del estado de los sensores seguimos recto, giramos o paramos
- // Estado: High->Negro , Low->blanco
- if ((sensor_L==LOW)&&(sensor_R==LOW)) {
- // Sigue recto
- Serial.println("Recto");
- giro_horario(); //Llamamos a la función giro_horario
- delay(10);
- }
- else if ((sensor_L==HIGH)&&(sensor_R==LOW)) {
- // Sensor izquierdo detecta negro -> Gira izquierda
- Serial.println("Giro izquierda");
- derecha_horario_izquierda_antihorario(); //Llamamos a la función derecha_horario_izquierda_antihorario
- delay(10);
- }
- else if ((sensor_L==LOW)&&(sensor_R==HIGH)) {
- // Sensor derecho detecta negro -> Gira derecha
- Serial.println("Giro derecha");
- derecha_antihorario_izquierda_horario(); //Llamamos a la función derecha_antihorario_izquierda_horario
- delay(10);
- }
- else {
- // los dos sensores leen negro. se para
- Serial.println("Paro");
- parar();//Llamamos a la función parar
- delay(10);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement