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() {
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(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() {
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;
}
