werbot_p_follow

werobot.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 WHITE_VALUE 3.5
/*
 * 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 ir1Sensor = wb_robot_get_device("ir1");
  wb_distance_sensor_enable(ir1Sensor, TIME_STEP);
  WbDeviceTag ir2Sensor = wb_robot_get_device("ir2");
  wb_distance_sensor_enable(ir2Sensor, 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 ir1Value,ir2Value, leftPower, rightPower; 
  
  while (wb_robot_step(TIME_STEP) != -1) {
    ir1Value = wb_distance_sensor_get_value(ir1Sensor);
    ir2Value = wb_distance_sensor_get_value(ir2Sensor);
    printf("valor ir0=> %f\n",ir1Value);
    printf("valor ir1=> %f\n",ir2Value);
    if(ir1Value < 8 && ir2Value < 8){
      leftPower = MAX_SPEED + WHITE_VALUE - 1.2*ir2Value;
      rightPower = MAX_SPEED + WHITE_VALUE - 1.2*ir1Value;
      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;
}