Edu Ü Rodriix
Hace 5 meses
#include <Arduino.h> //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();
}
#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();
};
#include <Arduino.h> //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);
}
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();
};
#include "Robot_Seguidor.h"
#include <Arduino.h> //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;
}
#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);
};
/*
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 ...... /
//////////////////////////////////////////
*/
#include "Robot_Seguidor.h"
Robot_Seguidor Robot(13, 12, 11, 10, 9, 8, 2, 3, 4, 5, 6);
void setup()
{
Robot.Inicializar();
}
void loop()
{
Robot.Modo_Seguidor(12, 0.25, 10, 170); //(int kP,int KI,int KD,int Velocidad)
}
#include <Arduino.h> //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);
}
class Sensor
{
private:
int Pin_sensor;
public:
Sensor(int pin): Pin_sensor(pin) {} //Constructor
void Inicializar();
int Leer_sensor();