diff --git a/robot_arduino/Diagramas/Diagrama_Puente_H_2018_bb.png b/robot_arduino/Diagramas/Diagrama_Puente_H_2018_bb.png new file mode 100644 index 0000000..11a6a66 Binary files /dev/null and b/robot_arduino/Diagramas/Diagrama_Puente_H_2018_bb.png differ diff --git a/robot_arduino/Diagramas/Diagrama_Robot-Seguidor-sensores_bb.png.jpg b/robot_arduino/Diagramas/Diagrama_Robot-Seguidor-sensores_bb.png.jpg new file mode 100644 index 0000000..dc682d9 Binary files /dev/null and b/robot_arduino/Diagramas/Diagrama_Robot-Seguidor-sensores_bb.png.jpg differ diff --git a/robot_arduino/Robot_Seguidor/Base_Robot.cpp b/robot_arduino/Robot_Seguidor/Base_Robot.cpp new file mode 100644 index 0000000..9ed5692 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Base_Robot.cpp @@ -0,0 +1,24 @@ +#include //Permite utilizar los comandos de Arduino +#include "Base_Robot.h" + +void Base_Robot::Inicializar_Robot() +{ + Motor_1.Inicializar_Motor(); + Motor_2.Inicializar_Motor(); +} + +void Base_Robot::Atras(int Velocidad_1,int Velocidad_2) +{ + Motor_1.Atras(Velocidad_1); + Motor_2.Atras(Velocidad_2); +} +void Base_Robot::Adelante(int Velocidad_1,int Velocidad_2) +{ + Motor_1.Adelante(Velocidad_1); + Motor_2.Adelante(Velocidad_2); +} +void Base_Robot::Stop() +{ + Motor_1.Stop(); + Motor_2.Stop(); +} diff --git a/robot_arduino/Robot_Seguidor/Base_Robot.h b/robot_arduino/Robot_Seguidor/Base_Robot.h new file mode 100644 index 0000000..3923b1b --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Base_Robot.h @@ -0,0 +1,15 @@ +#include "Motor_DC.h" +class Base_Robot +{ + private: + + + public: + Motor_DC Motor_1; + Motor_DC Motor_2; + Base_Robot(int Pin_Motor_1A,int Pin_Motor_1B,int PWM_1, int Pin_Motor_1C, int Pin_Motor_1D,int PWM_2): Motor_1(Pin_Motor_1A,Pin_Motor_1B,PWM_1), Motor_2(Pin_Motor_1C,Pin_Motor_1D,PWM_2){} + void Inicializar_Robot(); + void Adelante(int Velocidad_1,int Velocidad_2); + void Atras(int Velocidad_1,int Velocidad_2); + void Stop(); +}; diff --git a/robot_arduino/Robot_Seguidor/Motor_DC.cpp b/robot_arduino/Robot_Seguidor/Motor_DC.cpp new file mode 100644 index 0000000..d8aa0ce --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Motor_DC.cpp @@ -0,0 +1,27 @@ +#include //Permite utilizar los comandos de Arduino +#include "Motor_DC.h" + +void Motor_DC::Inicializar_Motor() +{ + pinMode(A, OUTPUT); + pinMode(B, OUTPUT); + pinMode(Pwm, OUTPUT); +} +void Motor_DC::Atras(int Velocidad) +{ + analogWrite(Pwm,Velocidad ); + digitalWrite(A, HIGH); + digitalWrite(B, LOW); +} +void Motor_DC::Adelante(int Velocidad) +{ + analogWrite(Pwm,Velocidad ); + digitalWrite(A, LOW); + digitalWrite(B, HIGH); +} +void Motor_DC::Stop() +{ + analogWrite(0, Pwm); + digitalWrite(A, LOW); + digitalWrite(B, LOW); +} diff --git a/robot_arduino/Robot_Seguidor/Motor_DC.h b/robot_arduino/Robot_Seguidor/Motor_DC.h new file mode 100644 index 0000000..b323cd1 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Motor_DC.h @@ -0,0 +1,15 @@ + +class Motor_DC +{ + private: + int A; + int B; + int Pwm; + + public: + Motor_DC(int a,int b,int pwm): A(a), B(b),Pwm(pwm){}//Constructor + void Inicializar_Motor(); + void Adelante(int Velocidad); + void Atras(int Velocidad); + void Stop(); +}; diff --git a/robot_arduino/Robot_Seguidor/Robot_Seguidor.cpp b/robot_arduino/Robot_Seguidor/Robot_Seguidor.cpp new file mode 100644 index 0000000..77686f8 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Robot_Seguidor.cpp @@ -0,0 +1,68 @@ +#include "Robot_Seguidor.h" +#include //Permite utilizar los comandos de Arduino + +void Robot_Seguidor::Inicializar() +{ + Robot.Inicializar_Robot(); + Sensor_1.Inicializar(); + Sensor_2.Inicializar(); + Sensor_3.Inicializar(); + Sensor_4.Inicializar(); + Sensor_5.Inicializar(); +} +void Robot_Seguidor::Modo_Seguidor(int Kp,int Ki,int Kd,int Velocidad) +{ + Lectura_de_sensores(); + P = Error; + I = I + Anteriror_I; + D = Error-Error_Anterior; + + PID = (Kp*P) + (Ki*I) + (Kd*D); + + Anteriror_I=I; + Error_Anterior=Error; + int Velocidad_motor_izquierdo = Velocidad-PID; + int Velocidad_motor_derecho = Velocidad+PID; + + + constrain(Velocidad_motor_izquierdo,0,255); + constrain(Velocidad_motor_derecho,0,255); + Robot.Adelante(Velocidad_motor_izquierdo,Velocidad_motor_derecho); + +} +void Robot_Seguidor::Lectura_de_sensores() +{ + + sensor[0]=Sensor_1.Leer_sensor(); + sensor[1]=Sensor_2.Leer_sensor(); + sensor[2]=Sensor_3.Leer_sensor(); + sensor[3]=Sensor_4.Leer_sensor(); + sensor[4]=Sensor_5.Leer_sensor(); + + //Detectar la desviacion ("Error") del seguidor de linea + if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0)) + { + Error=Error; + } + else + if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==1)) + Error=4; + else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==1)) + Error=3; + else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==0)) + Error=2; + else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==1)&&(sensor[4]==0)) + Error=1; + else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0)) + Error=0; + else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0)) + Error=-1; + else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0)) + Error=-2; + else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0)) + Error=-3; + else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0)) + Error=-4; + +} + diff --git a/robot_arduino/Robot_Seguidor/Robot_Seguidor.h b/robot_arduino/Robot_Seguidor/Robot_Seguidor.h new file mode 100644 index 0000000..a38d867 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Robot_Seguidor.h @@ -0,0 +1,32 @@ +#include "Base_Robot.h" +#include "Sensor_TCRT5000.h" +class Robot_Seguidor +{ +private: + Base_Robot Robot; + Sensor Sensor_1; + Sensor Sensor_2; + Sensor Sensor_3; + Sensor Sensor_4; + Sensor Sensor_5; + int sensor[5]; + float Error=0, P=0, I=0, D=0, PID=0; + float Error_Anterior=0, Anteriror_I=0; + +public: + Robot_Seguidor(int Pin_Motor_1A, + int Pin_Motor_1B, + int PWM_1, + int Pin_Motor_1C, + int Pin_Motor_1D, + int PWM_2, + int PinS1, + int PinS2, + int PinS3, + int PinS4, + int PinS5) + :Robot(Pin_Motor_1A, Pin_Motor_1B, PWM_1, Pin_Motor_1C, Pin_Motor_1D, PWM_2),Sensor_1(PinS1),Sensor_2(PinS2),Sensor_3(PinS3),Sensor_4(PinS4),Sensor_5(PinS5){} + void Inicializar(); + void Lectura_de_sensores(); + void Modo_Seguidor(int kP,int KI,int KD,int Velocidad); +}; diff --git a/robot_arduino/Robot_Seguidor/Robot_Seguidor.ino b/robot_arduino/Robot_Seguidor/Robot_Seguidor.ino new file mode 100644 index 0000000..84e28c2 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Robot_Seguidor.ino @@ -0,0 +1,44 @@ +/* + * Talos Electronics + * Tgo. Rafael Lozano Rolon + * Modificado por ultima vez 15 Diciembre 2016 + * Para inicializar el Robot se necesita definir los pines de la siguiente manera + * Robot_Seguidor Nombre_Robot(In1,In2,Vss1-2,IN3,IN4,Vss3-4,S1,S2,S3,S4,S5,S6) + * + * + * /////////////////Parametros/////////////// + * / Puente H / + * / Parametro -> Numero de pin / + * / IN1 -> 2 / + * / In2 -> 7 / + * / Vss1-2 -> 1 / + * / In3 -> 10 / + * / In4 -> 15 / + * / Vss3-4 -> 9 / + * / ----------------------------------------/ + * / Modulo seguidor de linea / + * / Parametro -> Numero de pin / + * / S1 -> Salida 1 del modulo / + * / seguidor de linea / + * / S2 -> Salida 2 .. / + * / S3 -> Salida 3 ... / + * / S4 -> Salida 4 .... / + * / S5 -> Salida 5 ..... / + * / S6 -> Salida 6 ...... / + * ////////////////////////////////////////// + + TU CODIGO NO COMPILA MIRA EL SIGUIENTE ENLACE +https://www.taloselectronics.com/blogs/tutoriales/faq-kit-seguidor-de-linea + */ +#include "Robot_Seguidor.h" +Robot_Seguidor Robot(13,12,11,10,8,9,2,3,4,5,6); + +void setup() +{ + Robot.Inicializar(); +} + +void loop() +{ + Robot.Modo_Seguidor(8,5,1,230);//(int kP,int KI,int KD,int Velocidad) +} diff --git a/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.cpp b/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.cpp new file mode 100644 index 0000000..175a218 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.cpp @@ -0,0 +1,12 @@ +#include //Permite utilizar los comandos de Arduino +#include "Sensor_TCRT5000.h" + +void Sensor::Inicializar() +{ + pinMode(Pin_sensor, INPUT); +} + +int Sensor::Leer_sensor() +{ + return digitalRead(Pin_sensor); +} \ No newline at end of file diff --git a/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.h b/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.h new file mode 100644 index 0000000..4211a19 --- /dev/null +++ b/robot_arduino/Robot_Seguidor/Sensor_TCRT5000.h @@ -0,0 +1,11 @@ + +class Sensor +{ + private: + int Pin_sensor; + + public: + Sensor(int pin): Pin_sensor(pin){}//Constructor + void Inicializar(); + int Leer_sensor(); +}; diff --git a/vista_camara/.pyenv b/vista_camara/.pyenv new file mode 100644 index 0000000..56bb660 --- /dev/null +++ b/vista_camara/.pyenv @@ -0,0 +1 @@ +3.12.7