e-puck_avoid_obstacles.zip

e-puck_avoid_obstacles.zip

/*
 * Description:  Default controller of the e-puck robot
 */

/* include headers */
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 

/* Device stuff */
#define DISTANCE_SENSORS_NUMBER 8
static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static double distance_sensors_values[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {
  “ps0“ “ps1“ “ps2“ “ps3“
  “ps4“ “ps5“ “ps6“ “ps7“
};

#define GROUND_SENSORS_NUMBER 3
static WbDeviceTag ground_sensors[GROUND_SENSORS_NUMBER];
static double ground_sensors_values[GROUND_SENSORS_NUMBER] = { 0.0 0.0 0.0 };
static const char *ground_sensors_names[GROUND_SENSORS_NUMBER] = {
  “gs0“ “gs1“ “gs2“
};

#define LEDS_NUMBER 10
static WbDeviceTag leds[LEDS_NUMBER];
static bool leds_values[LEDS_NUMBER];
static const char *leds_names[LEDS_NUMBER] = {
  “led0“ “led1“ “led2“ “led3“
  “led4“ “led5“ “led6“ “led7“
  “led8“ “led9“
};

static WbDeviceTag left_motor right_motor;

#define LEFT 0
#define RIGHT 1
#define MAX_SPEED 6.28
static double speeds[2];

/* Breitenberg stuff */
static double weights[DISTANCE_SENSORS_NUMBER][2] = {
  {-1.3 -1.0}
  {-1.3 -1.0}
  {-0.5 0.5}
  {0.0 0.0}
  {0.0 0.0}
  {0.05 -0.5}
  {-0.75 0}
  {-0.75 0}
};
static double offsets[2] = {
  0.5*MAX_SPEED 0.5*MAX_SPEED
};

static int get_time_step（） {
  static int time_step = -1;
  if （time_step == -1）
    time_step = （int） wb_robot_get_basic_time_step（）;
  return time_step;
}

static void step（） {
  if （wb_robot_step（get_time_step（）） == -1） {
    wb_robot_cleanup（）;
    exit（EXIT_SUCCESS）;
  }
}

static void passive_wait（double sec） {
  double start_time = wb_robot_get_time（）;
  do {
    step（）;
  } while（start_time + sec > wb_robot_get_time（））;
}

static void init_devices（） {
  int i;
  for （i=0; i    distance_sensors[i]=wb_robot_get_device（distance_sensors_names[i]）;
    wb_distance_sensor_enable（distance_sensors[i] get_time_step（））;
  }
  for （i=0; i    leds[i]=wb_robot_get_device（leds_names[i]）;

  // silently initialize the ground sensors if they exists
  for （i=0; i    ground_sensors[i] = （WbDeviceTag） 0;
  int ndevices = wb_robot_get_number_of_devices（）;
  for （i=0; i    WbDeviceTag dtag = wb_robot_get_device_by_index（i）;
    const char *dname = wb_device_get_name（dtag）;
    WbNodeType dtype = wb_device_get_node_type（dtag）;
    if （dtype == WB_NODE_DISTANCE_SENSOR && strlen（dname） == 3 && dname[0] == ‘g‘ && dname[1] == ‘s‘） {
      int id = dname[2] - ‘0‘;
      if （id >=0 && id < GROUND_SENSORS_NUMBER） {
        ground_sensors[id] = wb_robot_get_device（ground_sensors_names[id]）;
        wb_distance_sensor_enable（ground_sensors[id] get_time_step（））;
      }
    }
  }

  // get

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        5700  2018-03-02 13:36  e-puck_avoid_obstacles.c