Aquí os contaremos como hemos programado un robot de sumo para competir amistosamente con nuestros compañeros de clase.
Bueno, el robot ya lo teníamos montado, lo único que nos faltaba era programarlo para que pudiese competir y gracias al profesor que nos dio un programa Arduino, fuimos modificándolo (aumentado la velocidad, disminuyendo el tiempo de giro, etc) para conseguir lo mejor de él.
Os dejamos el programa utilizado y unas batallas que hicimos los compañeros de clase en un tatami.
https://youtu.be/dDLac5-2O_0
Nuestro robot es el de la derecha que, como podéis ver va más rápido y con más velocidad, eso es debido a que decidimos poner esos valores al máximo para poder derrotar a nuestros compañeros.
Pero fue una mala idea, ya que al final se salió del tatami.
Bueno pues así es como hicimos su programa, investigando para ver como reaccionaría el robot, hasta conseguir este programa (no pudimos grabarlo, el móvil de Pablo estaba estropeado):
#include <Ultrasonic.h> // Instalar antes la librería ultrasonidos https://github.com/ErickSimoes/Ultrasonic
Ultrasonic ultrasonic(3,2); //ultrasonic(trigPIN,echoPIN) trig pin D3 echo pin D2
int sensorvalor = 0; //hacer lo mismo
int inicio = 0; // 0 si es la rutina de comienzo de programa, 1 si es la rutina normal.
int SensorT = A0; //Pin del sensor trasero
int SensorAI = A1; //Pin del sensor Alante izquierdo
int SensorAD = A2; //Pin del sensor Alante derecho
//Motor izquierda
int EnB = 6 ; //PWM motor derecho, EnB del LM298
int In4 = 7; //In4 LM298
int In3 = 8; //In3 LM298
//Motor derecha
int EnA = 11; //PWM motor Izquierdo, EnA del LM298
int In2 = 9; //In2 del LM298
int In1 = 10; //In1 Del LM298
int led = 4; // led para indicar si detecta enemigo
byte sensor = 0;
int distance ;
int speed = 160;
void setup() {
Serial.begin(9600);
//Configuración pines de los sensores
delay (5000); // Reglas de la competición SUMO: Esperar 5 segundos antes de combatir
pinMode(SensorT, INPUT);
pinMode(SensorAD, INPUT);
pinMode(SensorAI, INPUT);
//Configuración pines de los motores
pinMode(In4, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In1, OUTPUT);
pinMode(EnA, OUTPUT);
pinMode(EnB, OUTPUT);
//Configuración pines potenciometro(masa y tensión)
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
}
///////////////////////////////////////////////////////
void loop() {
///////////////// RUTINAS DE PRUEBA DE MOTORES , SENSORES y Ultrasonidos //////////////////////
//MotorIzquierdo_avance();
//MotorIzquierdo_retroceso();
//MotorIzquierdo_paro();
//MotorDerecho_avance();
//MotorDerecho_retroceso();
//MotorDerecho_paro();
//Retroceso();
//Avance();
//giroDerecha();
//giroIzquierda();
//spinDerecha();
//spinIzquierda();
//leer_sensores();
Serial.print(inicio);
Serial.println(sensorvalor);
// delay(3000);
//////////////////////////////////////////////////////////////////
leer_sensores();
distance = ultrasonic.read();
speed = 100;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
digitalWrite(led,LOW);
//Serial.print(sensorvalor);
//Serial.println(distance);
while (inicio == 0 ) //Ir hacia atrás del tatami antes de empezar el combate
{
leer_sensores();
speed = 80;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Retroceso();
if ( sensorvalor == 3)
{
//leer_sensores();
speed = 120;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
delay (600);
inicio = 1; // comienza el combate
}
}
speed = 120;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
spinDerecha(); // start rotate
if (distance < 30)
{
digitalWrite(led,HIGH);
Stop();
while (distance < 30 ) {
digitalWrite(led,HIGH);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
distance = ultrasonic.read();
leer_sensores();
if ( sensorvalor == 1 || sensorvalor == 2) { break;} // Algún sensor lee línea negra
delay(100);
}
}
if (sensorvalor == 1 || sensorvalor == 2 || sensorvalor == 4 ) // Algún sensor delantero lee negro
{
Stop();
delay (50);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Retroceso();
delay (500);
}
if (sensorvalor == 3 ) // Sensor trasero lee negro
{
Stop();
delay (50);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
delay (500);
}
}
///////////////////////////FUNCIONES DE MOVIMIENTO //////////////////
void MotorIzquierdo_avance()
{
digitalWrite(In1, HIGH);
digitalWrite(In2, LOW);
}
void MotorIzquierdo_retroceso()
{
digitalWrite(In1, LOW);
digitalWrite(In2, HIGH);
}
void MotorIzquierdo_paro()
{
digitalWrite(In1, LOW);
digitalWrite(In2, LOW);
}
void MotorDerecho_avance()
{
digitalWrite(In3, LOW);
digitalWrite(In4, HIGH);
}
void MotorDerecho_retroceso()
{
digitalWrite(In3, HIGH);
digitalWrite(In4, LOW);
}
void MotorDerecho_paro()
{
digitalWrite(In3, LOW);
digitalWrite(In4, LOW);
}
//////////////////////////////////////////////////////
void Retroceso()
{
MotorIzquierdo_retroceso();
MotorDerecho_retroceso();
}
void Avance()
{
MotorIzquierdo_avance();
MotorDerecho_avance();
}
void giroIzquierda()
{
MotorIzquierdo_paro();
MotorDerecho_avance();
}
void giroDerecha()
{
MotorIzquierdo_avance();
MotorDerecho_paro();
}
void spinDerecha()
{
MotorIzquierdo_avance();
MotorDerecho_retroceso();
}
void spinIzquierda()
{
MotorIzquierdo_retroceso();
MotorDerecho_avance();
}
void Stop()
{
MotorIzquierdo_paro();
MotorDerecho_paro();
}
/////////////////////////////////////////////////
void leer_sensores()
{
sensor = PINC; //Lectura del estado del puerto Blanco = 1,, Negro = 0 (000 AD AI T)
//Lectura del estado del puerto Blanco = 1,, Negro = 0 (000 A2 A1 A0)
if (sensor == B000111) sensorvalor = 0; // Los tres sensores leen blanco. Dentro del tatami.
else if (sensor == B000011) sensorvalor = 1; // Sensor AD (Delante) lee negro (valor 0)
else if (sensor == B000101) sensorvalor = 2; // Sensor AI (Delante)lee negro
else if (sensor == B000110) sensorvalor = 3; // Sensor Trasero lee negro
else if (sensor == B000001) sensorvalor = 4; // Sensores AD y AI leen negro cambio de sentido
else if (sensor == B000000) sensorvalor = 5; // Sensores AD,AI,T leen negro (posible vuelco)
else sensorvalor = 22; // cualquier otro error
}
//
Bueno, el robot ya lo teníamos montado, lo único que nos faltaba era programarlo para que pudiese competir y gracias al profesor que nos dio un programa Arduino, fuimos modificándolo (aumentado la velocidad, disminuyendo el tiempo de giro, etc) para conseguir lo mejor de él.
Os dejamos el programa utilizado y unas batallas que hicimos los compañeros de clase en un tatami.
https://youtu.be/dDLac5-2O_0
Nuestro robot es el de la derecha que, como podéis ver va más rápido y con más velocidad, eso es debido a que decidimos poner esos valores al máximo para poder derrotar a nuestros compañeros.
Pero fue una mala idea, ya que al final se salió del tatami.
Bueno pues así es como hicimos su programa, investigando para ver como reaccionaría el robot, hasta conseguir este programa (no pudimos grabarlo, el móvil de Pablo estaba estropeado):
#include <Ultrasonic.h> // Instalar antes la librería ultrasonidos https://github.com/ErickSimoes/Ultrasonic
Ultrasonic ultrasonic(3,2); //ultrasonic(trigPIN,echoPIN) trig pin D3 echo pin D2
int sensorvalor = 0; //hacer lo mismo
int inicio = 0; // 0 si es la rutina de comienzo de programa, 1 si es la rutina normal.
int SensorT = A0; //Pin del sensor trasero
int SensorAI = A1; //Pin del sensor Alante izquierdo
int SensorAD = A2; //Pin del sensor Alante derecho
//Motor izquierda
int EnB = 6 ; //PWM motor derecho, EnB del LM298
int In4 = 7; //In4 LM298
int In3 = 8; //In3 LM298
//Motor derecha
int EnA = 11; //PWM motor Izquierdo, EnA del LM298
int In2 = 9; //In2 del LM298
int In1 = 10; //In1 Del LM298
int led = 4; // led para indicar si detecta enemigo
byte sensor = 0;
int distance ;
int speed = 160;
void setup() {
Serial.begin(9600);
//Configuración pines de los sensores
delay (5000); // Reglas de la competición SUMO: Esperar 5 segundos antes de combatir
pinMode(SensorT, INPUT);
pinMode(SensorAD, INPUT);
pinMode(SensorAI, INPUT);
//Configuración pines de los motores
pinMode(In4, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In1, OUTPUT);
pinMode(EnA, OUTPUT);
pinMode(EnB, OUTPUT);
//Configuración pines potenciometro(masa y tensión)
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
}
///////////////////////////////////////////////////////
void loop() {
///////////////// RUTINAS DE PRUEBA DE MOTORES , SENSORES y Ultrasonidos //////////////////////
//MotorIzquierdo_avance();
//MotorIzquierdo_retroceso();
//MotorIzquierdo_paro();
//MotorDerecho_avance();
//MotorDerecho_retroceso();
//MotorDerecho_paro();
//Retroceso();
//Avance();
//giroDerecha();
//giroIzquierda();
//spinDerecha();
//spinIzquierda();
//leer_sensores();
Serial.print(inicio);
Serial.println(sensorvalor);
// delay(3000);
//////////////////////////////////////////////////////////////////
leer_sensores();
distance = ultrasonic.read();
speed = 100;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
digitalWrite(led,LOW);
//Serial.print(sensorvalor);
//Serial.println(distance);
while (inicio == 0 ) //Ir hacia atrás del tatami antes de empezar el combate
{
leer_sensores();
speed = 80;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Retroceso();
if ( sensorvalor == 3)
{
//leer_sensores();
speed = 120;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
delay (600);
inicio = 1; // comienza el combate
}
}
speed = 120;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
spinDerecha(); // start rotate
if (distance < 30)
{
digitalWrite(led,HIGH);
Stop();
while (distance < 30 ) {
digitalWrite(led,HIGH);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
distance = ultrasonic.read();
leer_sensores();
if ( sensorvalor == 1 || sensorvalor == 2) { break;} // Algún sensor lee línea negra
delay(100);
}
}
if (sensorvalor == 1 || sensorvalor == 2 || sensorvalor == 4 ) // Algún sensor delantero lee negro
{
Stop();
delay (50);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Retroceso();
delay (500);
}
if (sensorvalor == 3 ) // Sensor trasero lee negro
{
Stop();
delay (50);
speed = 200;
analogWrite(EnB, speed); //Left Motor Speed
analogWrite(EnA, speed); //Right Motor Speed
Avance();
delay (500);
}
}
///////////////////////////FUNCIONES DE MOVIMIENTO //////////////////
void MotorIzquierdo_avance()
{
digitalWrite(In1, HIGH);
digitalWrite(In2, LOW);
}
void MotorIzquierdo_retroceso()
{
digitalWrite(In1, LOW);
digitalWrite(In2, HIGH);
}
void MotorIzquierdo_paro()
{
digitalWrite(In1, LOW);
digitalWrite(In2, LOW);
}
void MotorDerecho_avance()
{
digitalWrite(In3, LOW);
digitalWrite(In4, HIGH);
}
void MotorDerecho_retroceso()
{
digitalWrite(In3, HIGH);
digitalWrite(In4, LOW);
}
void MotorDerecho_paro()
{
digitalWrite(In3, LOW);
digitalWrite(In4, LOW);
}
//////////////////////////////////////////////////////
void Retroceso()
{
MotorIzquierdo_retroceso();
MotorDerecho_retroceso();
}
void Avance()
{
MotorIzquierdo_avance();
MotorDerecho_avance();
}
void giroIzquierda()
{
MotorIzquierdo_paro();
MotorDerecho_avance();
}
void giroDerecha()
{
MotorIzquierdo_avance();
MotorDerecho_paro();
}
void spinDerecha()
{
MotorIzquierdo_avance();
MotorDerecho_retroceso();
}
void spinIzquierda()
{
MotorIzquierdo_retroceso();
MotorDerecho_avance();
}
void Stop()
{
MotorIzquierdo_paro();
MotorDerecho_paro();
}
/////////////////////////////////////////////////
void leer_sensores()
{
sensor = PINC; //Lectura del estado del puerto Blanco = 1,, Negro = 0 (000 AD AI T)
//Lectura del estado del puerto Blanco = 1,, Negro = 0 (000 A2 A1 A0)
if (sensor == B000111) sensorvalor = 0; // Los tres sensores leen blanco. Dentro del tatami.
else if (sensor == B000011) sensorvalor = 1; // Sensor AD (Delante) lee negro (valor 0)
else if (sensor == B000101) sensorvalor = 2; // Sensor AI (Delante)lee negro
else if (sensor == B000110) sensorvalor = 3; // Sensor Trasero lee negro
else if (sensor == B000001) sensorvalor = 4; // Sensores AD y AI leen negro cambio de sentido
else if (sensor == B000000) sensorvalor = 5; // Sensores AD,AI,T leen negro (posible vuelco)
else sensorvalor = 22; // cualquier otro error
}
//
Comentarios
Publicar un comentario