/* * This is the main program. * The arguments of the main function can be specified by the * "controllerArgs" field of the Robot node */ int main() { /* necessary to initialize webots stuff */ initialize(); //Initially, the puck just heads straight. headStraight(); /* 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) { //As long as there is time, keep taking lightTrackerSteps; lightTrackerStep(); }; /* Enter your cleanup code here */ /* This is necessary to cleanup webots resources */ wb_robot_cleanup(); return 0; }
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() { 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; }
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; }
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; }
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) { //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; }
// 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) { 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() { 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; }
int main(int argc, char **argv) { printf("Comenzo todo\n"); wb_robot_init(); printf("Iniciando el sistema\n"); // Ros initialization ros::init(argc, argv, "Simulador"); ros::init(argc, argv, "Imagenes_de_Simulador"); ros::NodeHandle n,nh,nh2; // ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback); ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback); image_transport::ImageTransport it(nh); //OpenCV HighGUI call to destroy a display window on shut-down. image_transport::ImageTransport it2(nh2); image_transport::Publisher pub2 = it.advertise("/webot/camera", 1); //ros::spin(); // 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); // start engine speed=64.0; // km/h //print_help(); // allow to switch to manual control //wb_robot_keyboard_enable(TIME_STEP); // 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(); //wb_camera_save_image (camera, "/tmp/imagen.png", 100); //cv::WImageBuffer3_b image(cvLoadImage("/tmp/imagen.png", CV_LOAD_IMAGE_COLOR)); IplImage* imagen=save_camera_image(camera_image); sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(imagen, "bgr8"); pub2.publish(msg); cvReleaseImage(&imagen ); ros::spinOnce(); // wb_servo_set_position(left_steer, 0); // wb_servo_set_position(right_steer, 0); wb_robot_step(TIME_STEP); } wb_robot_cleanup(); return 0; // ignored }
// Main function int main(int argc, char **argv) { // Initialize webots wb_robot_init(); // GPS tick data int red_line_tick = 0; int green_circle_tick = 0; // Get robot devices WbDeviceTag left_wheel = wb_robot_get_device("left_wheel"); WbDeviceTag right_wheel = wb_robot_get_device("right_wheel"); // Get robot sensors WbDeviceTag forward_left_sensor = wb_robot_get_device("so3"); wb_distance_sensor_enable(forward_left_sensor, TIME_STEP); WbDeviceTag forward_right_sensor = wb_robot_get_device("so4"); wb_distance_sensor_enable(forward_right_sensor, TIME_STEP); WbDeviceTag left_sensor = wb_robot_get_device("so1"); wb_distance_sensor_enable(left_sensor, TIME_STEP); // Get the compass WbDeviceTag compass = wb_robot_get_device("compass"); wb_compass_enable(compass, TIME_STEP); // Get the GPS data WbDeviceTag gps = wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); // Prepare robot for velocity commands wb_motor_set_position(left_wheel, INFINITY); wb_motor_set_position(right_wheel, INFINITY); wb_motor_set_velocity(left_wheel, 0.0); wb_motor_set_velocity(right_wheel, 0.0); // Begin in mode 0, moving forward int mode = 0; // Main loop while (wb_robot_step(TIME_STEP) != -1) { // Get the sensor data double forward_left_value = wb_distance_sensor_get_value(forward_left_sensor); double forward_right_value = wb_distance_sensor_get_value(forward_right_sensor); double left_value = wb_distance_sensor_get_value(left_sensor); // Read compass and convert to angle const double *compass_val = wb_compass_get_values(compass); double compass_angle = convert_bearing_to_degrees(compass_val); // Read in the GPS data const double *gps_val = wb_gps_get_values(gps); // Debug printf("Sensor input values:\n"); printf("- Forward left: %.2f.\n",forward_left_value); printf("- Forward right: %.2f.\n",forward_right_value); printf("- Right: %.2f.\n",left_value); printf("- Compass angle (degrees): %.2f.\n", compass_angle); printf("- GPS values (x,z): %.2f, %.2f.\n", gps_val[0], gps_val[2]); // Send acuator commands double left_speed, right_speed; left_speed = 0; right_speed = 0; // List the current modes printf("Mode: %d.\n", mode); printf("Action: "); /* * There are four modes for this controller. * They are listed as below: * 0: Finding initial correct angle * 1: Moving forward * 2: Wall following * 3: Rotating after correct point * 4: Finding green circle + moving on */ // If it reaches GPS coords past the red line if (gps_val[2] > 8.0) { // Up the GPS tick red_line_tick = red_line_tick + 1; } // If the red line tick tolerance reaches // more than 10 per cycle, begin mode 3 if (red_line_tick > 10) { mode = 3; } if (mode == 0) { // Mode 0: Find correct angle printf("Finding correct angle\n"); if (compass_angle < (DESIRED_ANGLE - 1.0)) { // Turn right left_speed = MAX_SPEED; right_speed = 0; } else if (compass_angle > (DESIRED_ANGLE + 1.0)) { // Turn left left_speed = 0; right_speed = MAX_SPEED; } else { // Reached the desired angle, move in a straight line mode = 1; } } else if(mode == 1) { // Mode 1: Move forward printf("Moving forward.\n"); left_speed = MAX_SPEED; right_speed = MAX_SPEED; // When sufficiently close to a wall in front of robot, switch mode to wall following if ((forward_right_value > 500) || (forward_left_value > 500)) { mode = 2; } } else if (mode == 2) { // Mode 2: Wall following if ((forward_right_value > 200) || (forward_left_value > 200)) { printf("Backing up and turning right.\n"); left_speed = MAX_SPEED / 4.0; right_speed = - MAX_SPEED / 2.0; } else { if (left_value > 300) { printf("Turning right away from wall.\n"); left_speed = MAX_SPEED; right_speed = MAX_SPEED / 1.75; } else { if (left_value < 200) { printf("Turning left towards wall.\n"); left_speed = MAX_SPEED / 1.75; right_speed = MAX_SPEED; } else { printf("Moving forward along wall.\n"); left_speed = MAX_SPEED; right_speed = MAX_SPEED; } } } } else if (mode == 3) { // Once arrived, turn to the right printf("Finding correct angle (again)\n"); if (compass_angle < (90 - 1.0)) { // Turn right left_speed = MAX_SPEED; right_speed = MAX_SPEED / 1.75; } else if (compass_angle > (90 + 1.0)) { // Turn left left_speed = MAX_SPEED / 1.75; right_speed = MAX_SPEED; } else { // Reached the desired angle, move in a straight line if (green_circle_tick > 10) { left_speed = 0; right_speed = 0; } else { left_speed = MAX_SPEED; right_speed = MAX_SPEED; } if (gps_val[0] < -3.2) { green_circle_tick = green_circle_tick + 1; } } } // Set the motor speeds. wb_motor_set_velocity(left_wheel, left_speed); wb_motor_set_velocity(right_wheel, right_speed); // Perform simple simulation step } while (wb_robot_step(TIME_STEP) != -1); // Clean up webots 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; }
// entry point of the controller int main(int argc, char **argv) { // initialize the Webots API wb_robot_init(); // internal variables int i; WbDeviceTag ps[8]; char ps_names[8][4] = { "ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7" }; // initialize devices for (i=0; i<8 ; i++) { ps[i] = wb_robot_get_device(ps_names[i]); wb_distance_sensor_enable(ps[i], TIME_STEP); } WbDeviceTag ls[8]; char ls_names[8][4] = { "ls0", "ls1", "ls2", "ls3", "ls4", "ls5", "ls6", "ls7" }; // initialize devices for (i=0; i<8 ; i++) { ls[i] = wb_robot_get_device(ls_names[i]); wb_light_sensor_enable(ls[i], TIME_STEP); } WbDeviceTag led[8]; char led_names[8][5] = { "led0", "led1", "led2", "led3", "led4", "led5", "led6", "led7" }; // initialize devices for (i=0; i<8 ; i++) { led[i] = wb_robot_get_device(led_names[i]); } // feedback loop while (1) { // step simulation int delay = wb_robot_step(TIME_STEP); if (delay == -1) // exit event from webots break; // read sensors outputs double ps_values[8]; for (i=0; i<8 ; i++) ps_values[i] = wb_distance_sensor_get_value(ps[i]); update_search_speed(ps_values, 250); // set speeds double left_speed = get_search_left_wheel_speed(); double right_speed = get_search_right_wheel_speed(); // read IR sensors outputs double ls_values[8]; for (i=0; i<8 ; i++){ ls_values[i] = wb_light_sensor_get_value(ls[i]); } int active_ir = FALSE; for(i=0; i<8; i++){ if(ls_values[i] < 2275){ active_ir = TRUE; } } if(active_ir == TRUE){ swarm_retrieval(ls_values, ps_values, 2275); left_speed = get_retrieval_left_wheel_speed(); right_speed = get_retrieval_right_wheel_speed(); } if(is_pushing() == TRUE || stagnation == TRUE){ // check for stagnation stagnation_counter = stagnation_counter + 1; if(stagnation_counter == min((50 + positive_feedback * 50), 300) && stagnation == FALSE){ stagnation_counter = 0; // reset counter stagnation_check = TRUE; for(i=0; i<8; i++) prev_dist_values[i] = ps_values[i]; } if(stagnation_check == TRUE){ left_speed = 0; right_speed = 0; } if(stagnation_check == TRUE && stagnation_counter == 5){ stagnation_counter = 0; // reset counter reset_stagnation(); valuate_pushing(ps_values, prev_dist_values); stagnation = get_stagnation_state(); stagnation_check = FALSE; if(stagnation == TRUE) positive_feedback = 0; else positive_feedback = positive_feedback + 1; } if(stagnation == TRUE){ stagnation_recovery(ps_values, 300); left_speed = get_stagnation_left_wheel_speed(); right_speed = get_stagnation_right_wheel_speed(); if(get_stagnation_state() == FALSE){ reset_stagnation(); stagnation = FALSE; stagnation_counter = 0; } } } // write actuators inputs wb_differential_wheels_set_speed(left_speed, right_speed); for(i=0; i<8; i++){ wb_led_set(led[i], get_LED_state(i)); } } // cleanup the Webots API wb_robot_cleanup(); return 0; //EXIT_SUCCESS }
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 }
/* * 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; }