cosenza987

Untitled

Oct 11th, 2021
855
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <string.h>
  4. #include <webots/camera.h>
  5. #include <webots/position_sensor.h>
  6. #include <webots/distance_sensor.h>
  7. #include <webots/motor.h>
  8. #include <webots/robot.h>
  9. #include <webots/utils/system.h>
  10.  
  11. #define TIME_STEP 32
  12.  
  13. int main(int argc, char **argv) {
  14.     WbDeviceTag right_camera, left_camera, left_motor, right_motor, empilhadeira, frds, flds, ltds, lbds, pos;
  15.     int width, height;
  16.     int pause_counter = 0;
  17.     int left_speed, right_speed;
  18.     int i, j;
  19.     int red, blue, green;
  20.     wb_robot_init();
  21.  
  22.     frds = wb_robot_get_device("FrontRightDistanceSensor");
  23.     flds = wb_robot_get_device("FrontLeftDistanceSensor");
  24.     ltds = wb_robot_get_device("LeftTopDistanceSensor");
  25.     lbds = wb_robot_get_device("LeftBottomDistanceSensor");
  26.     wb_distance_sensor_enable(frds, TIME_STEP);
  27.     wb_distance_sensor_enable(flds, TIME_STEP);
  28.     wb_distance_sensor_enable(ltds, TIME_STEP);
  29.     wb_distance_sensor_enable(lbds, TIME_STEP);
  30.     pos = wb_robot_get_device("EMPPositionSensor");
  31.     wb_position_sensor_enable(pos, TIME_STEP);
  32.     right_camera = wb_robot_get_device("RightCamera");
  33.     left_camera = wb_robot_get_device("LeftCamera");
  34.     wb_camera_enable(right_camera, TIME_STEP);
  35.     wb_camera_enable(left_camera, TIME_STEP);
  36.     width = wb_camera_get_width(right_camera);
  37.     height = wb_camera_get_height(right_camera);
  38.     left_motor = wb_robot_get_device("LeftWheel");
  39.     right_motor = wb_robot_get_device("RightWheel");
  40.     empilhadeira = wb_robot_get_device("EMPLinearMotor");
  41.     wb_motor_set_position(left_motor, INFINITY);
  42.     wb_motor_set_position(right_motor, INFINITY);
  43.     wb_motor_set_velocity(left_motor, 0.0);
  44.     wb_motor_set_velocity(right_motor, 0.0);
  45.     while (wb_robot_step(TIME_STEP) != -1) {
  46.         wb_motor_set_velocity(lm, 2);
  47.         wb_motor_set_velocity(rm, 2);
  48.         const unsigned char *image = wb_camera_get_image(rc);
  49.         red = 0, green = 0, blue = 0;
  50.         for(int i = 0; i < 8; i++) {
  51.             for(int j = 0; j < 8; j++) {
  52.             red += wb_camera_image_get_red(image, width, i, j);
  53.               blue += wb_camera_image_get_blue(image, width, i, j);
  54.               green += wb_camera_image_get_green(image, width, i, j);
  55.             }
  56.           }
  57.         printf("%d %d %d\n", red, green, blue);
  58.     };
  59.    
  60.     wb_robot_cleanup();
  61.     return 0;
  62. }
RAW Paste Data