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

  Robot VolaTOR - Un modelo de globo dirigible y controlador que evita colisiones para Webots5.1.0 (Cyberbotics)
  
  Copyright (C) 2010 Hector Díaz Bachiller

  Instituto de Automática e Informática Industrial (ai2), UPV, Valencia
  
  Author: Hector Díaz Bachiller
  
  Email: hecdiaba@fiv.upv.es     Web: https://sites.google.com/site/torsweb/



  NOTA: Esta traducción de la GPL es informal y no ha sido aprobada 
  oficialmente por la Fundación para el Software Libre como válida. 
  Para estar completamente seguro de lo que está permitido, consulte 
  la GPL original (en inglés).

  Este programa es software libre: puedes redistribuirlo y/o modificarlo
  bajo los términos de GNU General Public License (GPL) como publica la Free
  Software Fundation, tanto en su versión 3, o (bajo tu criterio) cualquiera posterior.

  Este programa es distribuido con la esperanza de ser útil, pero SIN NINGUNA GARANTÍA;
  sin ni siquiera la garantía implícita de COMERCIABILIDAD o ADECUACIÓN A UN USO PARTICULAR.
  Ver la GNU General Public License para más detalles.

  Deberías haber recibido una copia de la GNU General Public License junto con este
  programa. Si no es así, visita <http://www.gnu.org/licenses/> o escribe a la
  Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA  02111-1307, USA.

  

  Este código se ha basado en:

  blimp_asl2 -- A blimp simulator and controller for Webots4 (Cyberbotics)
  Copyright (C) 2003 Autonomous Systems Lab 2, EPFL, Lausanne
  Authors: Jean-Christophe Zufferey & Alexis Guanella
  Email: name.surname@epfl.ch     Web: http://asl.epfl.ch


  ******************************** ////\\\\ ***********************************
  ******************************** \\\\//// ***********************************

  Robot VolaTOR - A blimp model and controller that avoids collisions for Webots5.1.0 (Cyberbotics)
  
  Copyright (C) 2010 Hector Díaz Bachiller

  Instituto de Automática e Informática Industrial (ai2), UPV, Valencia
  
  Author: Hector Díaz Bachiller
  
  Email: hecdiaba@fiv.upv.es     Web: https://sites.google.com/site/torsweb/  



  This program is free software: 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 3 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, see <http://www.gnu.org/licenses/> or
  write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA  02111-1307, USA.



  This code is based on:

  blimp_asl2 -- A blimp simulator and controller for Webots4 (Cyberbotics)
  Copyright (C) 2003 Autonomous Systems Lab 2, EPFL, Lausanne
  Authors: Jean-Christophe Zufferey & Alexis Guanella
  Email: name.surname@epfl.ch     Web: http://asl.epfl.ch

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



/****************************************************************************
  Important notice: 

    Set tab-size = 2 in your editor to view this file in good conditions.

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



#include <device/robot.h>
#include <device/custom_robot.h>
#include <device/distance_sensor.h>

#define SPEED 50
#define MANIOBRA_EVASION 0.5
#define ALTURA_MAX 2.0

DeviceTag ir2;

  static float incremento = 0.0;
  static float altitud = 0.0;
  static int cont = 1;


/*****************************************************************************/
/* This is the reset function called once upon initialization of the robot.  */
/*****************************************************************************/
static void reset(void) {

  ir2 = robot_get_device("ir2");
  robot_console_printf("Sensor de distancia vertical: %d\n",ir2);  
  distance_sensor_enable(ir2,64);
}


/*****************************************************************************/
/* This is the main control loop function, it is called repeatedly by Webots */
/*****************************************************************************/
static int run(int ms) {
  unsigned short ir2_value;
  cont = cont -1;
  
  ir2_value = distance_sensor_get_value(ir2);
  
  
  /***************************************************************************/
  /* Process sensor data here.                                               */
  /***************************************************************************/
  if(cont == 0){
 
  if(altitud <= MANIOBRA_EVASION)
    incremento = 0.01;
  else if(altitud > MANIOBRA_EVASION)
          incremento = 0.001;
  }
  
  if(altitud > ALTURA_MAX){
    incremento = -0.002;
    cont = 100;
  }
    
  altitud = altitud + incremento;
  

  custom_robot_move(0.0,incremento,0.0,0.0,0.0,0.0,0.0);
  /***************************************************************************/
  /* Enter here functions to send actuator commands,                         */
  /* like differential_wheels_set_speed().                                   */
  /***************************************************************************/

  return 64; /* this is the time step value, expressed in milliseconds. */
}

/*****************************************************************************/
/* This is the main program which sets up the reset and run function.        */
/*****************************************************************************/
int main() {
  robot_live(reset); /* initialization */
  robot_run(run);    /* starts the controller */
  return 0;
}
