Scribd
Upload
35 views

Algoritmo Roboticfest

Uploaded by

Adrián Paredes
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
35 views

Algoritmo Roboticfest

Uploaded by

Adrián Paredes
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 5

/*

* Copyright 1996-2020 Cyberbotics Ltd.


*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

/***************************************************************************

e-puck_line -- Base code for a practical assignment on behavior-based


robotics. When completed, the behavior-based controller should allow
the e-puck robot to follow the black line, avoid obstacles and
recover its path afterwards.
Copyright (C) 2006 Laboratory of Intelligent Systems (LIS), EPFL
Authors: Jean-Christophe Zufferey
Email: [email protected]
Web: http://lis.epfl.ch

This program is free software; any publications presenting results


obtained with this program must mention it and its origin. You
can redistribute it and/or modify it under the terms of the GNU
General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option)
any later version.

This program is distributed in the hope that it will be useful,


but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
USA.

***************************************************************************/

//librerias
#include <stdio.h>
#include <webots/distance_sensor.h>
#include <webots/light_sensor.h>
#include <webots/motor.h>
#include <webots/robot.h>

// Definiciones
#define TRUE 1
#define FALSE 0
#define IZQUIERDA 0
#define DERECHA 1
#define BLANCO 0
#define NEGRO 1
#define TIME_STEP 32 // [ms]

// Definiciones para uso de sensor IR de piso para seguir linea


#define NUMERO_SENSORES_LINEA 3
#define VALOR_BLANCO 600
#define SENSOR_LINEA_IZQUIERDO 0
#define SENSOR_LINEA_CENTRO 1
#define SENSOR_LINEA_DERECHO 2

// Motores
WbDeviceTag motor_izquierdo, motor_derecho;

// Sensor de linea
WbDeviceTag sensor_linea_centro;

//Sensor de proximidad
WbDeviceTag sensor_proximidad_siete;
WbDeviceTag sensor_proximidad_cero;

//Le asignamos los valores a los sensores


int valor_sensor_linea_centro = 0;
int valor_sensor_proximidad_siete = 0;
int valor_sensor_proximidad_cero = 0;

//------------------------------------------------------------------------------
//
// CONTROLLER
//
//------------------------------------------------------------------------------

////////////////////////////////////////////
// Main

int main() {

/* inicializamos la simulacion de Webots */


wb_robot_init();

// motores y sensores
motor_izquierdo = wb_robot_get_device("left wheel motor");
motor_derecho = wb_robot_get_device("right wheel motor");
sensor_linea_centro = wb_robot_get_device("gs1");
sensor_proximidad_cero = wb_robot_get_device("ps0");
sensor_proximidad_siete = wb_robot_get_device("ps7");

/* Inicializamos los motores */


wb_motor_set_position(motor_izquierdo, INFINITY);
wb_motor_set_position(motor_derecho, INFINITY);
wb_motor_set_velocity(motor_izquierdo, 0.0);
wb_motor_set_velocity(motor_derecho, 0.0);
/* Inicializamos los sensores */
wb_distance_sensor_enable(sensor_linea_centro, TIME_STEP);
wb_distance_sensor_enable(sensor_proximidad_cero, TIME_STEP);
wb_distance_sensor_enable(sensor_proximidad_siete, TIME_STEP);

/* Ciclo infinito: For mistico */


for (;;) {

/* Leemos los valores del sensor */

valor_sensor_linea_centro =
wb_distance_sensor_get_value(sensor_linea_centro); // gs1
printf("Sensor central = %d | ", valor_sensor_linea_centro); // valor leido por
gs1

valor_sensor_proximidad_cero =
wb_distance_sensor_get_value(sensor_proximidad_cero); // ps0
printf("Sensor proximidad cero = %d | ", valor_sensor_proximidad_cero); //
valor leido por ps0

valor_sensor_proximidad_siete =
wb_distance_sensor_get_value(sensor_proximidad_siete); // ps7
printf("Sensor proximidad siete = %d | \n", valor_sensor_proximidad_siete); //
valor leido por ps7

int velocidad_izquierda = 0;
int velocidad_derecha = 0;

/* Evación de obstaculos */

if (valor_sensor_proximidad_cero > 110 || valor_sensor_proximidad_siete > 110){

velocidad_izquierda = 0;
velocidad_derecha = 0;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(1000);

velocidad_izquierda = -200;
velocidad_derecha = -200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(700);

velocidad_izquierda = 200;
velocidad_derecha = -200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(1800);

velocidad_izquierda = 200;
velocidad_derecha = 200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(2600);

velocidad_izquierda = -200;
velocidad_derecha = 200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(1800);

velocidad_izquierda = 300;
velocidad_derecha = 300;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(4100);

velocidad_izquierda = -200;
velocidad_derecha = 200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(1800);

velocidad_izquierda = 200;
velocidad_derecha = 200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(2600);

velocidad_izquierda = 200;
velocidad_derecha = -200;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);
wb_robot_step(1800);

/* Seguidor de línea */

}else {

if (valor_sensor_linea_centro < VALOR_BLANCO){

velocidad_izquierda = 140;
velocidad_derecha = -25;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);

}else {

velocidad_izquierda = -25;
velocidad_derecha = 140;
wb_motor_set_velocity(motor_izquierdo, 0.00628 * velocidad_izquierda);
wb_motor_set_velocity(motor_derecho, 0.00628 * velocidad_derecha);

}
wb_robot_step(250);

}
}

return 0;
}

You might also like