/*
?*?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[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