69 lines
2.1 KiB
C++
69 lines
2.1 KiB
C++
|
#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;
|
||
|
|
|
||
|
|
}
|
||
|
|
|