follow2sensores.c

/*
 * File:          segue_linha.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like <webots/distance_sensor.h> or
 * <webots/motor.h>, etc.
 */
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/distance_sensor.h>
#include <stdio.h>

/*
 * You may want to add macros here.
 */
#define TIME_STEP 64


#define MAX_SPEED 6.28
#define REGULAR_SPEED 2
/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  wb_robot_init();
  
  // get a handler to the motors and set target position to infinity (speed control)
  WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");
  WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
  
  WbDeviceTag dl = wb_robot_get_device("dl");
  wb_distance_sensor_enable(dl, TIME_STEP);
  
  WbDeviceTag dr = wb_robot_get_device("dr");
  wb_distance_sensor_enable(dr, TIME_STEP);

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  double sl, sr, er, el, leftPower, rightPower; 
  
  while (wb_robot_step(TIME_STEP) != -1) {
    sl = wb_distance_sensor_get_value(dl);
    sr = wb_distance_sensor_get_value(dr);
    el = (sl)*0.5;
    er = (sr)*0.5;
    
    printf("valor leitura => %f %f\n", sl, sr);
    printf("valor error=> %f %f\n", el, er);
    
    if(sl > 20 || sr > 20){
      leftPower = rightPower = 0;
    }
    else{
    leftPower = REGULAR_SPEED+er;
    rightPower = REGULAR_SPEED+el;
    }
    if(leftPower > MAX_SPEED){
      leftPower = MAX_SPEED;
    }
    if(rightPower > MAX_SPEED){
      rightPower = MAX_SPEED;
    }
    
    printf("valor leftPower => %f\n",leftPower);
    printf("valor rightPower=> %f\n", rightPower);
    wb_motor_set_velocity(left_motor, leftPower);
    wb_motor_set_velocity(right_motor, rightPower);
  }

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}