Compare commits
6 Commits
129e386bf9
...
e6dc731415
Author | SHA1 | Date | |
---|---|---|---|
e6dc731415 | |||
82327e4c29 | |||
b60e4ce93d | |||
3062d04e69 | |||
adfc9a2bff | |||
0c643db7fb |
@ -5,11 +5,12 @@ class Base_Robot
|
|||||||
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Motor_DC Motor_1;
|
Motor_DC Motor_1;
|
||||||
Motor_DC Motor_2;
|
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){}
|
Base_Robot(int IN1,int IN2,int ENA, int IN3, int IN4,int ENB):
|
||||||
void Inicializar_Robot();
|
Motor_1(IN1,IN2,ENA), Motor_2(IN3,IN4,ENB){}
|
||||||
void Adelante(int Velocidad_1,int Velocidad_2);
|
void Inicializar_Robot();
|
||||||
void Atras(int Velocidad_1,int Velocidad_2);
|
void Adelante(int Velocidad_1,int Velocidad_2);
|
||||||
void Stop();
|
void Atras(int Velocidad_1,int Velocidad_2);
|
||||||
|
void Stop();
|
||||||
};
|
};
|
||||||
|
@ -1,27 +1,27 @@
|
|||||||
#include <Arduino.h> //Permite utilizar los comandos de Arduino
|
#include <Arduino.h>
|
||||||
#include "Motor_DC.h"
|
#include "Motor_DC.h"
|
||||||
|
|
||||||
void Motor_DC::Inicializar_Motor()
|
void Motor_DC::Inicializar_Motor()
|
||||||
{
|
{
|
||||||
pinMode(A, OUTPUT);
|
pinMode(IN1, OUTPUT);
|
||||||
pinMode(B, OUTPUT);
|
pinMode(IN2, OUTPUT);
|
||||||
pinMode(Pwm, OUTPUT);
|
pinMode(ENA, OUTPUT);
|
||||||
}
|
}
|
||||||
void Motor_DC::Atras(int Velocidad)
|
void Motor_DC::Atras(int Velocidad)
|
||||||
{
|
{
|
||||||
analogWrite(Pwm,Velocidad );
|
analogWrite(ENA,Velocidad );
|
||||||
digitalWrite(A, HIGH);
|
digitalWrite(IN1, HIGH);
|
||||||
digitalWrite(B, LOW);
|
digitalWrite(IN2, LOW);
|
||||||
}
|
}
|
||||||
void Motor_DC::Adelante(int Velocidad)
|
void Motor_DC::Adelante(int Velocidad)
|
||||||
{
|
{
|
||||||
analogWrite(Pwm,Velocidad );
|
analogWrite(ENA,Velocidad );
|
||||||
digitalWrite(A, LOW);
|
digitalWrite(IN1, LOW);
|
||||||
digitalWrite(B, HIGH);
|
digitalWrite(IN2, HIGH);
|
||||||
}
|
}
|
||||||
void Motor_DC::Stop()
|
void Motor_DC::Stop()
|
||||||
{
|
{
|
||||||
analogWrite(0, Pwm);
|
analogWrite(ENA, 0);
|
||||||
digitalWrite(A, LOW);
|
digitalWrite(IN1, LOW);
|
||||||
digitalWrite(B, LOW);
|
digitalWrite(IN2, LOW);
|
||||||
}
|
}
|
||||||
|
@ -2,14 +2,14 @@
|
|||||||
class Motor_DC
|
class Motor_DC
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
int A;
|
int IN1;
|
||||||
int B;
|
int IN2;
|
||||||
int Pwm;
|
int ENA;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Motor_DC(int a,int b,int pwm): A(a), B(b),Pwm(pwm){}//Constructor
|
Motor_DC(int in1,int in2,int ena): IN1(in1), IN2(in2),ENA(ena){}//Constructor
|
||||||
void Inicializar_Motor();
|
void Inicializar_Motor();
|
||||||
void Adelante(int Velocidad);
|
void Adelante(int Velocidad);
|
||||||
void Atras(int Velocidad);
|
void Atras(int Velocidad);
|
||||||
void Stop();
|
void Stop();
|
||||||
};
|
};
|
||||||
|
@ -2,31 +2,37 @@
|
|||||||
#include "Sensor_TCRT5000.h"
|
#include "Sensor_TCRT5000.h"
|
||||||
class Robot_Seguidor
|
class Robot_Seguidor
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
Base_Robot Robot;
|
Base_Robot Robot;
|
||||||
Sensor Sensor_1;
|
Sensor Sensor_1;
|
||||||
Sensor Sensor_2;
|
Sensor Sensor_2;
|
||||||
Sensor Sensor_3;
|
Sensor Sensor_3;
|
||||||
Sensor Sensor_4;
|
Sensor Sensor_4;
|
||||||
Sensor Sensor_5;
|
Sensor Sensor_5;
|
||||||
int sensor[5];
|
int sensor[5];
|
||||||
float Error=0, P=0, I=0, D=0, PID=0;
|
float Error=0, P=0, I=0, D=0, PID=0;
|
||||||
float Error_Anterior=0, Anteriror_I=0;
|
float Error_Anterior=0, Anteriror_I=0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Robot_Seguidor(int Pin_Motor_1A,
|
Robot_Seguidor(int Pin_Motor_1A,
|
||||||
int Pin_Motor_1B,
|
int IN1,
|
||||||
int PWM_1,
|
int IN2,
|
||||||
int Pin_Motor_1C,
|
int ENA,
|
||||||
int Pin_Motor_1D,
|
int IN3,
|
||||||
int PWM_2,
|
int IN4,
|
||||||
int PinS1,
|
int ENB,
|
||||||
int PinS2,
|
int PinS1,
|
||||||
int PinS3,
|
int PinS2,
|
||||||
int PinS4,
|
int PinS3,
|
||||||
int PinS5)
|
int PinS4,
|
||||||
: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){}
|
int PinS5)
|
||||||
void Inicializar();
|
:Robot(IN1, IN2, ENA, IN3, IN4, ENB),
|
||||||
void Lectura_de_sensores();
|
Sensor_1(PinS1),
|
||||||
void Modo_Seguidor(int kP,int KI,int KD,int Velocidad);
|
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);
|
||||||
};
|
};
|
||||||
|
Loading…
x
Reference in New Issue
Block a user