int main()
{
/* necessary to initialize webots stuff */
wb_robot_init();
reset();
int result=0;
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
while (wb_robot_step(TIME_STEP) != -1) {
result=run();
if (result!=TIME_STEP) break;
};
closeFiles();
// quit the sim
wb_supervisor_simulation_quit(EXIT_SUCCESS);
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
int main() {
double buffer[255];
double fit;
double *rbuffer;
int i;
wb_robot_init();
reset();
receiver_enable(rec,32);
differential_wheels_enable_encoders(64);
robot_step(64);
while (1) {
// Wait for data
while (receiver_get_queue_length(rec) == 0) {
robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(rec);
// Check for pre-programmed avoidance behavior
if (rbuffer[DATASIZE] == -1.0) {
fitfunc(gwaypoints,100);
// Otherwise, run provided controller
} else {
fit = fitfunc(rbuffer,rbuffer[DATASIZE]);
buffer[0] = fit;
wb_emitter_send(emitter,(void *)buffer,sizeof(double));
}
wb_receiver_next_packet(rec);
}
return 0;
}
int main() {
double w_left, w_right;
wb_robot_init();
WbDeviceTag human02_wheel;
human02_wheel = wb_robot_get_device("human02_plate");
int time_step = wb_robot_get_basic_time_step();
wb_differential_wheels_enable_encoders(time_step);
for (;;)
{
w_left = 10.;
w_right = 10.;
wb_differential_wheels_set_speed(w_left, w_right);
//printf(" left_encoder=%7.5f\n",wb_differential_wheels_get_left_encoder());
wb_robot_step(32);
}
return 0;
}
int main(int argc, char *argv[]) {
wb_robot_init();
WbNodeRef robot_node = wb_supervisor_node_get_from_def("rob0");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
double count = 0;
while(1) {
char time[10];
sprintf(time,"%f sec",count);
wb_supervisor_set_label(0,time,0,0,0.1,0xff0000,0);
const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field);
//printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]);
if (x==1 && trans[0]<-1.7 && trans[2]<-0.65) {
wb_supervisor_export_image ("/home/afroze/Desktop/photo.jpg",50);
printf("\n\n\n\nPHOTOOOOOO\n\n\n\n\n");
return 0;
}
wb_robot_step(TIME_STEP);
count+=0.064;
}
return 0;
}
static void reset(void) {
wb_robot_init(); // Initialization
char s[4]="ps0";
int i;
for(i=0; i<NB_SENSORS;i++) {
ps[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
gps=wb_robot_get_device("gps");
}
int main() {
const char *SERVO_NAMES[NUM_SERVOS] = {
"servo_r0",
"servo_r1",
"servo_r2",
"servo_l0",
"servo_l1",
"servo_l2"};
WbDeviceTag servos[NUM_SERVOS];
int elapsed = 0;
int state, i;
long double pos = 0,lastPos = 0; // int 害死了 0-6.28
int flag = 0;
const int dir[] = {1,1,1,1,1,1};//{1,1,1,-1,-1,-1}
// 增加在地面上的时间，就会更加稳定
const double rate = 1.0/6;// 1/4
wb_robot_init();
for (i = 0; i < NUM_SERVOS; i++) {
servos[i] = wb_robot_get_device(SERVO_NAMES[i]);
if (!servos[i]) {
printf("could not find servo: %s\n",SERVO_NAMES[i]);
}
}
pos = M_PI * rate ;
flag = 0;
while(wb_robot_step(TIME_STEP)!=-1) {
elapsed++;
state = (elapsed / 25 + 1) % NUM_STATES;
lastPos = pos;
if(flag) pos += 2*M_PI * rate;
else pos += 2*M_PI * (1-rate);
flag = !flag;
for (i = 0; i < 6; i+=2 ){
wb_servo_set_position(servos[i], dir[i]*pos);
}
for (i = 1; i < 6; i+=2 ) {
wb_servo_set_position(servos[i], dir[i]*lastPos);
}
}
wb_robot_cleanup();
return 0;
}
/*
* Initialize flock position and devices
*/
void reset(void) {
wb_robot_init();
char rob[7] = "epuck0";
int i;
for (i=0;i<FLOCK_SIZE;i++) {
sprintf(rob,"epuck%d",i+offset);
robs[i] = wb_supervisor_node_get_from_def(rob);
robs_trans[i] = wb_supervisor_node_get_field(robs[i],"translation");
robs_rotation[i] = wb_supervisor_node_get_field(robs[i],"rotation");
}
}
int main() {
wb_robot_init();
printf("hello from NAO\n");
time_step = wb_robot_get_basic_time_step();
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, time_step);
emitter = wb_robot_get_device("emitter2");
wb_receiver_enable(emitter, time_step);
find_and_enable_devices();
wb_motor_set_position (RShoulderPitch, 1.57079633);
wb_motor_set_position (LShoulderPitch, 1.57079633);
wb_motor_set_position (HeadYaw, 0.0);
//wb_motor_set_position (HeadPitch, 0.1); //10 degrees
// run until simulation is restarted
while (wb_robot_step(time_step) != -1) {
//check_for_new_genes();
//sense_compute_and_actuate();
//int samples = wb_camera_get_width(LaserHead);
//double field_of_view = wb_camera_get_fov(LaserHead);
//const float *values = wb_camera_get_range_image(LaserHead);
report_step_state_to_supervisor();
//printf("%f\n", field_of_view);
//printf("size: %d\n", sizeof(values));
//int i;
//for(i=0; i<samples;i++){
// printf("value %d is %f\n",i, values[i]);
//}
if(wb_robot_step(time_step) == 0){
//report_step_state_to_supervisor();
}
}
wb_robot_cleanup();
return 0;
}
int main() {
//const char *name;
int left_speed, right_speed;
left_speed = 50;
right_speed = 50;
wb_robot_init();
//name = wb_robot_get_name();
wb_differential_wheels_set_speed(left_speed, right_speed);
wb_robot_cleanup();
return 0;
}
int main() {
wb_robot_init();
reset();
while(wb_robot_step(TIME_STEP) != -1) {
run();
}
wb_robot_cleanup();
return 0;
}
int main() {
int duration = TIME_STEP;
wb_robot_init(); // controller initialization
reset();
while (wb_robot_step(duration) != -1) {
duration = run(duration);
}
wb_robot_cleanup();
return 0;
}
static void initialize()
{
wb_robot_init();
currentBehavior = RANDOM_MOVER;
ps0 = wb_robot_get_device("ps0");
ps1 = wb_robot_get_device("ps1");
ps2 = wb_robot_get_device("ps2");
ps3 = wb_robot_get_device("ps3");
ps4 = wb_robot_get_device("ps4");
ps5 = wb_robot_get_device("ps5");
ps6 = wb_robot_get_device("ps6");
ps7 = wb_robot_get_device("ps7");
ls0 = wb_robot_get_device("ls0");
ls1 = wb_robot_get_device("ls1");
ls2 = wb_robot_get_device("ls2");
ls3 = wb_robot_get_device("ls3");
ls4 = wb_robot_get_device("ls4");
ls5 = wb_robot_get_device("ls5");
ls6 = wb_robot_get_device("ls6");
ls7 = wb_robot_get_device("ls7");
led4 = wb_robot_get_device("led4");
int sensorTimeout = 100;
wb_distance_sensor_enable(ps0, sensorTimeout);
wb_distance_sensor_enable(ps1, sensorTimeout);
wb_distance_sensor_enable(ps2, sensorTimeout);
wb_distance_sensor_enable(ps3, sensorTimeout);
wb_distance_sensor_enable(ps4, sensorTimeout);
wb_distance_sensor_enable(ps5, sensorTimeout);
wb_distance_sensor_enable(ps6, sensorTimeout);
wb_distance_sensor_enable(ps7, sensorTimeout);
wb_light_sensor_enable(ls0, sensorTimeout);
wb_light_sensor_enable(ls1, sensorTimeout);
wb_light_sensor_enable(ls2, sensorTimeout);
wb_light_sensor_enable(ls3, sensorTimeout);
wb_light_sensor_enable(ls4, sensorTimeout);
wb_light_sensor_enable(ls5, sensorTimeout);
wb_light_sensor_enable(ls6, sensorTimeout);
wb_light_sensor_enable(ls7, sensorTimeout);
wb_led_set(led4, 1);
}
int main(int argc, char *argv[]) {
/* define variables */
WbDeviceTag turret_sensor;
double perf_data[3];
double clock = 0;
/* initialize Webots */
wb_robot_init();
// read robot id from the robot's name
char* robot_name;
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"e-puck%d",&robot_id);
emitter = wb_robot_get_device("emitter");
wb_emitter_set_range(emitter,COM_RANGE);
receiver = wb_robot_get_device("receiver");
wb_receiver_set_channel(receiver,0);
wb_receiver_enable(receiver,TIME_STEP);
wb_robot_step(TIME_STEP);
turret_sensor = wb_robot_get_device("ts");
wb_light_sensor_enable(turret_sensor,TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
clock += (double)TIME_STEP/1000;
/* get light sensor value */
sensor_value = wb_light_sensor_get_value(turret_sensor);
if(wb_receiver_get_queue_length(receiver) > 0) {
wb_emitter_set_channel(emitter,3);
perf_data[0] = (double)robot_id;
perf_data[1] = (double)(pkt_count-1);
perf_data[2] = 0;
wb_emitter_send(emitter,perf_data,3*sizeof(double));
break; // stop node
}
else {
send_data(clock); // send measurement data
}
}
wb_robot_cleanup();
return 0;
}
/*
* Reset the robot's devices and get its ID
*/
static void reset()
{
int i;
wb_robot_init();
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
for(i=0; i<FLOCK_SIZE; i++) {
timestamp[i] = 0;
relative_pos[i][0] = 0;
relative_pos[i][1] = 0;
relative_pos[i][2] = 0;
}
maxTimestamp = 1;
robot_name=(char*) wb_robot_get_name();
for(i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
my_position[0] = 0;
my_position[1] = 0;
my_position[2] = 0;
//printf("reset: my pos: %f, %f\n", my_position[0], my_position[1]);
wb_differential_wheels_enable_encoders(64);
//Reading the robot's name. Pay attention to name specification when adding robots to the simulation!
sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name
robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1
sprintf(robot_number, "%d", robot_id_u);
receiver = wb_robot_get_device("receiver");
receiver0 = wb_robot_get_device("receiver0");
emitter = wb_robot_get_device("emitter");
emitter0 = wb_robot_get_device("emitter0");
wb_receiver_enable(receiver,32);
wb_receiver_enable(receiver0,32);
for(i=0; i<FLOCK_SIZE; i++) {
initialized[i] = 0; // Set initialization to 0 (= not yet initialized)
}
}
int main() {
srand ( time(NULL) );
wb_robot_init();
wb_robot_step(TIME_STEP);
wb_robot_step(TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
wb_differential_wheels_set_speed(1000,1000);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char *argv[]) {
/* initialize Webots */
wb_robot_init();
reset();
/* main loop */
for (;;) {
getSensorValues(distances);
run();
/* perform a simulation step */
wb_robot_step(TIME_STEP);
}
return 0;
}
int main(int argc, char *argv[]) {
/* initialize Webots */
wb_robot_init();
reset();
/* perform a simulation step */
wb_robot_step(TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
run();
}
return 0;
}
int main(int argc, char **argv)
{
//robot_live(reset);
// robot_run(run);
/* necessary to initialize webots stuff */
wb_robot_init();
//TIME_STEP = (int)wb_robot_get_basic_time_step();
int n=reset();
printf("did reset: %d\n",n);
/*
* 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
*/
while (wb_robot_step(TIME_STEP) != -1) {
/*
* Read the sensors :
* Enter here functions to read sensor data, like:
* double val = wb_distance_sensor_get_value(my_sensor);
*/
run(TIME_STEP);
/* Process sensor data here */
/*
* Enter here functions to send actuator commands, like:
* wb_differential_wheels_set_speed(100.0,100.0);
*/
};
/* Enter your cleanup code here */
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
/*
* Reset the robot's devices and get its ID
*/
static void reset()
{
wb_robot_init();
receiver = wb_robot_get_device("receiver");
emitter = wb_robot_get_device("emitter");
compass2 = wb_robot_get_device("compass2");
receiver0 = wb_robot_get_device("receiver0");
emitter0 = wb_robot_get_device("emitter0");
int i;
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
robot_name=(char*) wb_robot_get_name();
for(i=0; i<FLOCK_SIZE; i++) {
relative_pos[i][0] = my_position[0] = 0;
relative_pos[i][1] = my_position[1] = 0;
relative_pos[i][2] = my_position[2] = 0;
}
for(i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
wb_receiver_enable(receiver,64);
wb_receiver_enable(receiver0,64);
wb_compass_enable(compass2, 64);
//Reading the robot's name. Pay attention to name specification when adding robots to the simulation!
sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name
robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1
for(i=0; i<FLOCK_SIZE; i++) {
initialized[i] = 0; // Set initialization to 0 (= not yet initialized)
}
}
// main loop
int main(void)
{
srand(time(NULL));
// initialization
wb_robot_init();
int i;
for (i=0;i<ROBOTS;i++) {
char aux[15];
sprintf(aux,"%s%d",rob_prefix,i+1);
rob[i] = wb_supervisor_node_get_from_def(aux);
loc[i] = wb_supervisor_field_get_sf_vec3f(wb_supervisor_node_get_field(rob[i],"translation"));
initLoc[i][0] = loc[i][0];
initLoc[i][1] = loc[i][1];
initLoc[i][2] = loc[i][2];
rot[i] = wb_supervisor_field_get_sf_rotation(wb_supervisor_node_get_field(rob[i],"rotation"));
initRot[i][0] = rot[i][0];
initRot[i][1] = rot[i][1];
initRot[i][2] = rot[i][2];
initRot[i][3] = rot[i][3];
}
reset();
wb_robot_step(2*STEP_SIZE);
// start the controller
outfile = fopen("../../../matlab/output.m","w");
printf("Starting main loop...\n");
while (wb_robot_step(STEP_SIZE) != -1)
{
run(STEP_SIZE);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char **argv)
{
wb_robot_init();
int time_step = wb_robot_get_basic_time_step();
WbDeviceTag gps = wb_robot_get_device("GPS");
wb_gps_enable(gps, time_step);
WbDeviceTag emitter = wb_robot_get_device("emitter");
wb_emitter_set_channel(emitter,13);
double* gps_value;
char message[32];
while (wb_robot_step(time_step) != -1){
gps_value = wb_gps_get_values(gps);
//Something's wrong with deserialization
//So we make a fixed width string here
sprintf(message,"{%0.3f,%0.3f}",gps_value[0]/2+5.0,-gps_value[2]/2+5.0);
wb_emitter_send(emitter,message,strlen(message)+1);
// printf("%s\n",message);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char **argv)
{
wb_robot_init();
int i;
WbDeviceTag servos[NSERVOS];
WbDeviceTag head_led;
for (i=0; i<NSERVOS; i++)
servos[i] = wb_robot_get_device(servo_names[i]);
head_led = wb_robot_get_device("HeadLed");
wb_led_set(head_led, 0x40C040);
do {
double t = wb_robot_get_time();
for (i=0; i<6; i++)
wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]);
} while (wb_robot_step(TIME_STEP) != -1);
wb_robot_cleanup();
return 0;
}
int main(int argc, char **argv)
{
printf("Comenzo todo\n");
wb_robot_init();
printf("Iniciando el sistema\n");
ros::init(argc, argv, "NEURONAL");
ros::NodeHandle n,nh,nh2;
ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback);
// find front wheels
left_front_wheel = wb_robot_get_device("left_front_wheel");
right_front_wheel = wb_robot_get_device("right_front_wheel");
wb_servo_set_position(left_front_wheel, INFINITY);
wb_servo_set_position(right_front_wheel, INFINITY);
// get steering motors
left_steer = wb_robot_get_device("left_steer");
right_steer = wb_robot_get_device("right_steer");
// camera device
camera = wb_robot_get_device("camera");
wb_camera_enable(camera, TIME_STEP);
camera_width = wb_camera_get_width(camera);
camera_height = wb_camera_get_height(camera);
camera_fov = wb_camera_get_fov(camera);
// initialize gps
gps = wb_robot_get_device("gps");
wb_gps_enable(gps, TIME_STEP);
// initialize display (speedometer)
display = wb_robot_get_device("display");
speedometer_image = wb_display_image_load(display, "/root/research/PROYECTOTP/controllers/destiny_controller/speedometer.png");
wb_display_set_color(display, 0xffffff);
// SICK sensor
sick = wb_robot_get_device("lms291");
wb_camera_enable(sick, TIME_STEP);
sick_width = wb_camera_get_width(sick);
sick_range = wb_camera_get_max_range(sick);
sick_fov = wb_camera_get_fov(sick);
// start engine
speed=64.0; // km/h
// main loop
printf("Iniciando el ciclo de peticiones\n");
// set_autodrive(false);z
while (1) {
const unsigned char *camera_image = wb_camera_get_image(camera);
compute_gps_speed();
update_display();
ros::spinOnce();
wb_robot_step(TIME_STEP);
}
wb_robot_cleanup();
return 0; // ignored
}
int main() {
wb_robot_init();
// do this once only
WbNodeRef robot_node1 = wb_supervisor_node_get_from_def("epuck1");
WbFieldRef trans_field1 = wb_supervisor_node_get_field(robot_node1, "translation");
/*WbNodeRef robot_node2 = wb_supervisor_node_get_from_def("epuck2");
WbFieldRef trans_field2 = wb_supervisor_node_get_field(robot_node2, "translation");
WbNodeRef robot_node3 = wb_supervisor_node_get_from_def("epuck3");
WbFieldRef trans_field3 = wb_supervisor_node_get_field(robot_node3, "translation");
WbNodeRef robot_node4 = wb_supervisor_node_get_from_def("epuck4");
WbFieldRef trans_field4 = wb_supervisor_node_get_field(robot_node4, "translation");
WbNodeRef robot_node5 = wb_supervisor_node_get_from_def("epuck5");
WbFieldRef trans_field5 = wb_supervisor_node_get_field(robot_node5, "translation");
WbNodeRef robot_node6 = wb_supervisor_node_get_from_def("epuck6");
WbFieldRef trans_field6 = wb_supervisor_node_get_field(robot_node6, "translation");
WbNodeRef robot_node7 = wb_supervisor_node_get_from_def("epuck7");
WbFieldRef trans_field7 = wb_supervisor_node_get_field(robot_node7, "translation");
*/
time_t now;
struct tm *today;
char date[23];
//get current date
time(&now);
today = localtime(&now);
//print it in DD.MM.YY format.
strftime(date, 23, "sim%Y%m%d.%H%M%S.txt", today);
FILE *fp;
fp=fopen(date, "w");
double time = 0.0;
for (time = 0.0; time < 3600.0; time += TIME_STEP / 1000.0) {
// this is done repeatedly
const double *trans1 = wb_supervisor_field_get_sf_vec3f(trans_field1);
fprintf(fp, "%g,%g",trans1[0], trans1[2]);
/*const double *trans2 = wb_supervisor_field_get_sf_vec3f(trans_field2);
fprintf(fp, ",%g,%g",trans2[0], trans2[2]);
const double *trans3 = wb_supervisor_field_get_sf_vec3f(trans_field3);
fprintf(fp, ",%g,%g",trans3[0], trans3[2]);
const double *trans4 = wb_supervisor_field_get_sf_vec3f(trans_field4);
fprintf(fp, ",%g,%g",trans4[0], trans4[2]);
const double *trans5 = wb_supervisor_field_get_sf_vec3f(trans_field5);
fprintf(fp, ",%g,%g",trans5[0], trans5[2]);
const double *trans6 = wb_supervisor_field_get_sf_vec3f(trans_field6);
fprintf(fp, ",%g,%g",trans6[0], trans6[2]);
const double *trans7 = wb_supervisor_field_get_sf_vec3f(trans_field7);
fprintf(fp, ",%g,%g",trans7[0], trans7[2]);*/
fprintf(fp, "\n");
wb_robot_step(TIME_STEP);
}
wb_supervisor_simulation_quit(EXIT_SUCCESS);
fclose(fp);
wb_robot_cleanup();
wb_robot_step(TIME_STEP);
wb_robot_step(TIME_STEP);
wb_robot_step(TIME_STEP);
return 0;
}
// initialize devices and data
static void initialize() {
// necessary to initialize Webots
wb_robot_init();
// initialize game control data
memset(&control_data, 0, sizeof(control_data));
memcpy(control_data.header, GAMECONTROLLER_STRUCT_HEADER, sizeof(GAMECONTROLLER_STRUCT_HEADER));
control_data.version = GAMECONTROLLER_STRUCT_VERSION;
control_data.playersPerTeam = NUM_ROBOTS / 2;
control_data.state = STATE_INITIAL;
control_data.secondaryState = STATE2_NORMAL;
control_data.teams[0].teamColour = TEAM_BLUE;
control_data.teams[1].teamColour = TEAM_RED;
// emitter for sending game control data and receiving 'move' requests
//emitter = wb_robot_get_device("emitter");
//receiver = wb_robot_get_device("receiver");
//wb_receiver_enable(receiver, TIME_STEP);
// get robot field tags for getting/setting their positions
int i;
for(i = 0; i < NUM_ROBOTS; i++) {
WbNodeRef robot = wb_supervisor_node_get_from_def(ROBOT_DEF_NAMES[i]);
if (robot) {
robot_translation[i] = wb_supervisor_node_get_field(robot, "translation");
robot_rotation[i] = wb_supervisor_node_get_field(robot, "rotation");
robot_controller[i] = wb_supervisor_node_get_field(robot, "controller");
}
else {
robot_translation[i] = NULL;
robot_rotation[i] = NULL;
robot_controller[i] = NULL;
}
}
// to keep track of ball position
WbNodeRef ball = wb_supervisor_node_get_from_def("BALL");
if (ball)
ball_translation = wb_supervisor_node_get_field(ball, "translation");
// eventually read teams names from file
FILE *file = fopen("teams.txt", "r");
if (file) {
fscanf(file, "%[^\n]\n%[^\n]", team_names[TEAM_BLUE], team_names[TEAM_RED]);
fclose(file);
}
// variable set during official matches
const char *WEBOTS_ROBOTSTADIUM = getenv("WEBOTS_ROBOTSTADIUM");
if (WEBOTS_ROBOTSTADIUM) {
if (strcmp(WEBOTS_ROBOTSTADIUM, "ROUND") == 0) {
match_type = ROUND;
printf("Running Robotstadium ROUND match\n");
}
else if (strcmp(WEBOTS_ROBOTSTADIUM, "FINAL") == 0) {
match_type = FINAL;
printf("Running Robotstadium FINAL match\n");
}
}
// make video
if (match_type != DEMO) {
// format=640x480, type=MPEG4, quality=75%
wb_supervisor_start_movie("movie.avi", 640, 480, 0, 75);
}
// enable keyboard for manual score control
wb_robot_keyboard_enable(TIME_STEP * 10);
}
/*
* 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)
{
//int cont=0;
using_shared_memory();
/* necessary to initialize webots stuff */
wb_robot_init();
// do this once only Darwin
WbNodeRef robot_node = wb_supervisor_node_get_from_def("Darwin");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation");
int caiu_cont = 0;
// do this once only Darwin2
WbNodeRef robot_node_2 = wb_supervisor_node_get_from_def("Darwin2");
WbFieldRef trans_field_2 = wb_supervisor_node_get_field(robot_node_2, "translation");
WbFieldRef rot_field_2 = wb_supervisor_node_get_field(robot_node_2, "rotation");
// do this once only Darwin3
WbNodeRef robot_node_3 = wb_supervisor_node_get_from_def("Darwin3");
WbFieldRef trans_field_3 = wb_supervisor_node_get_field(robot_node_3, "translation");
WbFieldRef rot_field_3 = wb_supervisor_node_get_field(robot_node_3, "rotation");
while (1) {
// this is done repeatedly
const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field);
const double *trans_2 = wb_supervisor_field_get_sf_vec3f(trans_field_2);
const double *trans_3 = wb_supervisor_field_get_sf_vec3f(trans_field_3);
TRANS1 = trans[0];
TRANS2 = trans[1];
TRANS3 = trans[2];
TRANS1_2 = trans_2[0];
TRANS2_2 = trans_2[1];
TRANS3_2 = trans_2[2];
TRANS1_3 = trans_3[0];
TRANS2_3 = trans_3[1];
TRANS3_3 = trans_3[2];
//printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]);
wb_robot_step(32);
if(RESET_ROBOT == 1)
{
caiu_cont++;
CAIU_CONT = caiu_cont;
// reset robot position
const double INITIAL[3] = { 0, 0.32004, 0 };
const double INITIAL_ROT[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL);
wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT = 0;
if(caiu_cont > 25)
{
caiu_cont = 0;
wb_supervisor_simulation_revert(); // restart the simulation
// A medida que o tempo vai passando o robo Darwin vai ficando
// cada vez mais devagar como se houvesse algum tipo de desgaste fisico
//entao e necessario restarta a simulaçao
}
}
if(RESET_ROBOT_2 == 1)
{
// reset robot position
const double INITIAL_2[3] = { 0.2, 0.32004, 4.7 };
const double INITIAL_ROT_2[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_2, INITIAL_2);
wb_supervisor_field_set_sf_rotation(rot_field_2, INITIAL_ROT_2);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_2 = 0;
}
if(RESET_ROBOT_3 == 1)
{
// reset robot position
const double INITIAL_3[3] = { 3.2, 0.32004, -3.7 };
const double INITIAL_ROT_3[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_3, INITIAL_3);
wb_supervisor_field_set_sf_rotation(rot_field_3, INITIAL_ROT_3);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_3 = 0;
}
}
/*
* 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");
*/
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
////////////////////////////////////////////
// Main
int main()
{
int i, speed[2], ps_offset[NB_DIST_SENS]={0,0,0,0,0,0,0,0}, Mode=1;
/* intialize Webots */
wb_robot_init();
/* initialization */
char name[20];
for (i = 0; i < NB_LEDS; i++) {
sprintf(name, "led%d", i);
led[i] = wb_robot_get_device(name); /* get a handler to the sensor */
}
for (i = 0; i < NB_DIST_SENS; i++) {
sprintf(name, "ps%d", i);
ps[i] = wb_robot_get_device(name); /* proximity sensors */
wb_distance_sensor_enable(ps[i],TIME_STEP);
}
for (i = 0; i < NB_GROUND_SENS; i++) {
sprintf(name, "gs%d", i);
gs[i] = wb_robot_get_device(name); /* ground sensors */
wb_distance_sensor_enable(gs[i],TIME_STEP);
}
for(;;) // Main loop
{
// Run one simulation step
wb_robot_step(TIME_STEP);
// Reset all BB variables when switching from simulation to real robot and back
if (Mode!=wb_robot_get_mode())
{
oam_reset = TRUE;
llm_active = FALSE;
llm_past_side = NO_SIDE;
ofm_active = FALSE;
lem_active = FALSE;
lem_state = LEM_STATE_STANDBY;
Mode = wb_robot_get_mode();
if (Mode == SIMULATION) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i];
wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values
printf("\n\n\nSwitching to SIMULATION and reseting all BB variables.\n\n");
} else if (Mode == REALITY) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i];
wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values
printf("\n\n\nSwitching to REALITY and reseting all BB variables.\n\n");
}
}
// read sensors value
for(i=0;i<NB_DIST_SENS;i++) ps_value[i] = (((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i])<0)?0:((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i]);
for(i=0;i<NB_GROUND_SENS;i++) gs_value[i] = wb_distance_sensor_get_value(gs[i]);
// Reset all BB variables when switching from simulation to real robot and back
if (Mode!=wb_robot_get_mode())
{
oam_reset = TRUE;
llm_active = FALSE;
llm_past_side = NO_SIDE;
ofm_active = FALSE;
lem_active = FALSE;
lem_state = LEM_STATE_STANDBY;
Mode = wb_robot_get_mode();
if (Mode == SIMULATION) printf("\nSwitching to SIMULATION and reseting all BB variables.\n\n");
else if (Mode == REALITY) printf("\nSwitching to REALITY and reseting all BB variables.\n\n");
}
// Speed initialization
speed[LEFT] = 0;
speed[RIGHT] = 0;
// *** START OF SUBSUMPTION ARCHITECTURE ***
// LFM - Line Following Module
LineFollowingModule();
// Speed computation
speed[LEFT] = lfm_speed[LEFT];
speed[RIGHT] = lfm_speed[RIGHT];
// *** END OF SUBSUMPTION ARCHITECTURE ***
// Debug display
printf("OAM %d side %d \n", oam_active, oam_side);
// Set wheel speeds
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
return 0;
}
int main(int argc, char **argv) {
wb_robot_init();
// check if roslaunch is properly installed
if (system("which roslaunch > /dev/null")!=0) {
fprintf(stderr,"Cannot find roslaunch in PATH. Please check that ROS is properly installed.\n");
wb_robot_cleanup();
return 0;
}
// launch the joy ROS node
int roslaunch=fork();
if (roslaunch==0) { // child process
execlp("roslaunch","roslaunch","joy.launch",NULL);
return 0;
}
// From this point we assume the ROS_ROOT, ROS_MASTER_URI and
// ROS_PACKAGE_PATH environment variables are set appropriately.
// See ROS manuals to set them properly.
ros::init(argc, argv, "joystick");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe("joy", 10, joy_callback);
// ros::Subscriber sub=nh.subscribe<int32>("joy", 10, joy_callback);
double left_speed = 0;
double right_speed = 0;
ROS_INFO("Joypad connected and running.");
ROS_INFO("Commands:");
ROS_INFO("arrow up: move forward, speed up");
ROS_INFO("arrow down: move backward, slow down");
ROS_INFO("arrow left: turn left");
ROS_INFO("arrow right: turn right");
// control loop: simulation steps of 32 ms
while(wb_robot_step(32) != -1) {
// get callback called
ros::spinOnce();
switch(cmd) {
case JOYPAD_UP:
left_speed=SPEED;
right_speed=SPEED;
break;
case JOYPAD_DOWN:
left_speed=-SPEED;
right_speed=-SPEED;
break;
case JOYPAD_LEFT:
left_speed=-SPEED;
right_speed=SPEED;
break;
case JOYPAD_RIGHT:
left_speed=SPEED;
right_speed=-SPEED;
break;
case JOYPAD_STOP:
left_speed = 0;
right_speed = 0;
break;
}
// set motor speeds
wb_differential_wheels_set_speed(left_speed, right_speed);
}
ros::shutdown();
kill(roslaunch,SIGINT); // terminate roslaunch for joy ROS node as with Ctrl-C
wb_robot_cleanup();
return 0;
}
int main() {
printf("hello from supervisor\n");
const char *robot_name[ROBOTS] = {"NAO"};
WbNodeRef node;
WbFieldRef robot_translation_field[ROBOTS],robot_rotation_field[ROBOTS],ball_translation_field;
//WbDeviceTag emitter, receiver;
int i,j;
int score[2] = { 0, 0 };
double time = 10 * 60; /* a match lasts for 10 minutes */
double ball_reset_timer = 0;
double ball_initial_translation[3] = { -2.5, 0.0324568, 0 };
double robot_initial_translation[ROBOTS][3] = {
{-4.49515, 0.234045, -0.0112415},
{0.000574037, 0.332859, -0.00000133636}};
double robot_initial_rotation[ROBOTS][4] = {
{0.0604202, 0.996035, -0.0652942, 1.55047},
{0.000568956, 0.70711, 0.707104, 3.14045}};
double packet[ROBOTS * 3 + 2];
char time_string[64];
const double *robot_translation[ROBOTS], *robot_rotation[ROBOTS], *ball_translation;
wb_robot_init();
time_step = wb_robot_get_basic_time_step();
emitter = wb_robot_get_device("emitter");
wb_receiver_enable(emitter, time_step);
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, time_step);
for (i = 0; i < ROBOTS; i++) {
node = wb_supervisor_node_get_from_def(robot_name[i]);
robot_translation_field[i] = wb_supervisor_node_get_field(node,"translation");
robot_translation[i] = wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
for(j=0;j<3;j++) robot_initial_translation[i][j]=robot_translation[i][j];
robot_rotation_field[i] = wb_supervisor_node_get_field(node,"rotation");
robot_rotation[i] = wb_supervisor_field_get_sf_rotation(robot_rotation_field[i]);
for(j=0;j<4;j++) robot_initial_rotation[i][j]=robot_rotation[i][j];
}
node = wb_supervisor_node_get_from_def("BALL");
ball_translation_field = wb_supervisor_node_get_field(node,"translation");
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for(j=0;j<3;j++) ball_initial_translation[j]=ball_translation[j];
/* printf("ball initial translation = %g %g %g\n",ball_translation[0],ball_translation[1],ball_translation[2]); */
set_scores(0, 0);
while(wb_robot_step(TIME_STEP)!=-1) {
//printf("supervisor commands START!\n");
check_for_slaves_data();
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for (i = 0; i < ROBOTS; i++) {
robot_translation[i]=wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
/* printf("coords for robot %d: %g %g %g\n",i,robot_translation[i][0],robot_translation[i][1],robot_translation[i][2]); */
packet[3 * i] = robot_translation[i][0]; /* robot i: X */
packet[3 * i + 1] = robot_translation[i][2]; /* robot i: Z */
if (robot_rotation[i][1] > 0) { /* robot i: rotation Ry axis */
packet[3 * i + 2] = robot_rotation[i][3]; /* robot i: alpha */
} else { /* Ry axis was inverted */
packet[3 * i + 2] = -robot_rotation[i][3];
}
}
packet[3 * ROBOTS] = ball_translation[0]; /* ball X */
packet[3 * ROBOTS + 1] = ball_translation[2]; /* ball Z */
wb_emitter_send(emitter, packet, sizeof(packet));
/* Adds TIME_STEP ms to the time */
time -= (double) TIME_STEP / 1000;
if (time < 0) {
time = 10 * 60; /* restart */
}
sprintf(time_string, "%02d:%02d", (int) (time / 60), (int) time % 60);
wb_supervisor_set_label(2, time_string, 0.45, 0.01, 0.1, 0x000000, 0.0); /* black */
if (ball_reset_timer == 0) {
if (ball_translation[0] > GOAL_X_LIMIT) { /* ball in the blue goal */
set_scores(++score[0], score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
} else if (ball_translation[0] < -GOAL_X_LIMIT) { /* ball in the yellow goal */
set_scores(score[0], ++score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
}
} else {
ball_reset_timer -= (double) TIME_STEP / 1000.0;
if (ball_reset_timer <= 0) {
ball_reset_timer = 0;
wb_supervisor_field_set_sf_vec3f(ball_translation_field, ball_initial_translation);
for (i = 0; i < ROBOTS; i++) {
wb_supervisor_field_set_sf_vec3f(robot_translation_field[i], robot_initial_translation[i]);
wb_supervisor_field_set_sf_rotation(robot_rotation_field[i], robot_initial_rotation[i]);
}
}
}
}
wb_robot_cleanup();
return 0;
}
int main(int argc, const char *argv[]) {
WbDeviceTag speaker, microphone;
WbDeviceTag sensors[NUM_SENSORS];
int role = UNKNOWN;
int prev_audible = -1;
int i, j; // for loops
// initialize webots
wb_robot_init();
// determine role
if (argc >= 2) {
if (! strcmp(argv[1], "speaker"))
role = SPEAKER;
else if (! strcmp(argv[1], "microphone"))
role = MICROPHONE;
}
// use speaker or microphone according to specified role
if (role == SPEAKER)
speaker = wb_robot_get_device("speaker");
else if (role == MICROPHONE) {
// we only use "mic0" in this demo
microphone = wb_robot_get_device("mic0");
wb_microphone_enable(microphone, TIME_STEP / 2);
}
// find distance sensors
for (i = 0; i < NUM_SENSORS; i++) {
char sensor_name[64];
sprintf(sensor_name, "ps%d", i);
sensors[i] = wb_robot_get_device(sensor_name);
wb_distance_sensor_enable(sensors[i], TIME_STEP);
}
// main loop
for (;;) {
if (role == SPEAKER) {
wb_speaker_emit_sample(speaker, SAMPLE, sizeof(SAMPLE));
}
else if (role == MICROPHONE) {
const short *rec_buffer = (const short *)wb_microphone_get_sample_data(microphone);
int numSamples = wb_microphone_get_sample_size(microphone) / sizeof(SAMPLE[0]);
if (rec_buffer) {
int audible = 0;
for (i = 0; i < numSamples; i++) {
// warning this demo assumes no noise and therefore depends on the sound plugin configuration !
if (rec_buffer[i] != 0)
audible = 1;
}
if (audible != prev_audible) {
printf(audible ? "I hear you now !\n" : "I can't hear you !\n");
prev_audible = audible;
}
}
else
printf("received: nothing this time ...\n");
}
// read distance sensor values
double sensor_values[NUM_SENSORS];
for (i = 0; i < NUM_SENSORS; i++)
sensor_values[i] = wb_distance_sensor_get_value(sensors[i]);
// compute braitenberg collision avoidance
double speed[2];
for (i = 0; i < 2; i++) {
speed[i] = 0.0;
for (j = 0; j < NUM_SENSORS; j++)
speed[i] += EPUCK_MATRIX[j][i] * (1 - (sensor_values[j] / RANGE));
}
// set the motors speed
wb_differential_wheels_set_speed(speed[0], speed[1]);
// simulation step
wb_robot_step(TIME_STEP);
}
return 0;
}
