API tools faq
paste
Advertisement
cosenza987

Untitled

cosenza987
Oct 11th, 2021
1,068
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 15.96 KB | None | 0 0
raw download clone embed print report
  1. #include <stdio.h>
  2. #include <time.h>
  3. #include <stdlib.h>
  4. #include <string.h>
  5. #include <webots/camera.h>
  6. #include <webots/position_sensor.h>
  7. #include <webots/distance_sensor.h>
  8. #include <webots/motor.h>
  9. #include <webots/robot.h>
  10. #include <webots/utils/system.h>
  11. #ifdef _WIN32
  12. #include <Windows.h>
  13. #else
  14. #include <unistd.h>
  15. #endif
  16.  
  17. #define TIME_STEP 32
  18.  
  19. int width, height, tamanhodorobo = 27;
  20. WbDeviceTag right_camera, left_camera, left_motor, right_motor, empilhadeira, frds, flds, ltds, lbds, pos;
  21.  
  22. typedef struct _rgb {
  23.     int r, g, b;
  24. } rgb;
  25.  
  26. rgb verde_min = {1000, 12000, 1000}, verde_max = {10000, 14000, 10000};
  27.  
  28. rgb getrgbs(WbDeviceTag camera) {
  29.     const unsigned char *image = wb_camera_get_image(camera);
  30.     rgb ans = {0, 0, 0};
  31.     for(int i = 0; i < width; i++) {
  32.         for(int j = 0; j < height; j++) {
  33.             ans.r += wb_camera_image_get_red(image, width, i, j);
  34.             ans.g += wb_camera_image_get_green(image, width, i, j);
  35.             ans.b += wb_camera_image_get_blue(image, width, i, j);
  36.         }
  37.     }
  38.     return ans;
  39. }
  40.  
  41. void pegar_tubo(){
  42.    
  43.     while(wb_robot_step(TIME_STEP) != -1) {
  44.         wb_motor_set_velocity(left_motor, 2);
  45.         wb_motor_set_velocity(right_motor, 2);
  46.  
  47.         double frdsvalue = wb_distance_sensor_get_value(frds);
  48.         double fldsvalue = wb_distance_sensor_get_value(flds);
  49.  
  50.         if(frdsvalue < 60 && fldsvalue < 60) {
  51.             wb_motor_set_velocity(left_motor, 0);
  52.             wb_motor_set_velocity(right_motor, 0);
  53.  
  54.             while(wb_position_sensor_get_value(pos) > 0) {
  55.                 wb_motor_set_velocity(empilhadeira, -2);
  56.             }
  57.             wb_motor_set_velocity(left_motor, 2);
  58.             wb_motor_set_velocity(right_motor, 2);
  59.            
  60.             while(wb_position_sensor_get_value(pos) < 0.3) {
  61.                 wb_motor_set_velocity(empilhadeira, 2);
  62.             }
  63.             return;
  64.         }
  65.     }
  66. }
  67.  
  68. void entregar_tubo(){
  69.     while(wb_robot_step(TIME_STEP) != -1) {
  70.  
  71.         double frdsvalue = wb_distance_sensor_get_value(frds);
  72.         double fldsvalue = wb_distance_sensor_get_value(flds);
  73.  
  74.         while(wb_position_sensor_get_value(pos) > 0) {
  75.             wb_motor_set_velocity(empilhadeira, -2);
  76.         }
  77.        
  78.         wb_motor_set_velocity(left_motor, -2);
  79.         wb_motor_set_velocity(right_motor, -2);
  80.  
  81.         if(frdsvalue > 60 && fldsvalue > 60){
  82.             wb_motor_set_velocity(left_motor, 0);
  83.             wb_motor_set_velocity(right_motor, 0);
  84.  
  85.             while(wb_position_sensor_get_value(pos) < 0.3) {
  86.                 wb_motor_set_velocity(empilhadeira, 2);
  87.             }
  88.             return;
  89.         }
  90.     }
  91. }
  92.  
  93. double odometria() {
  94.     double tube;
  95.     clock_t start = clock();
  96.  
  97.     while(wb_robot_step(TIME_STEP) != -1) {
  98.         double ltdsvalue = wb_distance_sensor_get_value(ltds);
  99.        
  100.         wb_motor_set_velocity(left_motor, 2);
  101.         wb_motor_set_velocity(right_motor, 2);
  102.  
  103.         if(ltdsvalue < 100) {
  104.             wb_motor_set_velocity(left_motor, 0);  
  105.             wb_motor_set_velocity(right_motor, 0);
  106.  
  107.             break;
  108.         }
  109.  
  110.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  111.  
  112.         if(rc.b < 7000 && rc.r < 9000 && rc.g < 8000) {
  113.             wb_motor_set_velocity(left_motor, 0);  
  114.             wb_motor_set_velocity(right_motor, 0);
  115.  
  116.             clock_t end = clock();
  117.             tube = 2.0*(end - start) + 27;
  118.  
  119.             return tube;
  120.         }
  121.  
  122.     }
  123.  
  124.     clock_t end = clock();
  125.  
  126.     tube = 2.0*(end - start);
  127.  
  128.     return tube;
  129. }
  130.  
  131. void alinhar_chao() {//verificado
  132.     while(wb_robot_step(TIME_STEP) != -1) {
  133.         wb_motor_set_velocity(right_motor, 2);
  134.         wb_motor_set_velocity(left_motor, 2);
  135.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  136.         int left_sensor = 0, right_sensor = 0;
  137.         if(rc.b < 7000 && rc.r < 9000 && rc.g < 8000) {
  138.             right_sensor = 1;
  139.         }
  140.         if(lc.b < 7000 && lc.r < 9000 && lc.g < 8000) {
  141.             left_sensor = 1;
  142.         }
  143.         if(right_sensor && !left_sensor) {
  144.             wb_motor_set_velocity(right_motor, -3);
  145.             wb_motor_set_velocity(left_motor, 1);
  146.             while(wb_robot_step(TIME_STEP) != -1) {
  147.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  148.                 if(abs(rc.b - lc.b) < 150 && abs(rc.g - lc.g) < 150 && abs(rc.r - lc.r) < 150) {
  149.                     wb_motor_set_velocity(right_motor, 0);
  150.                     wb_motor_set_velocity(left_motor, 0);
  151.                     return;
  152.                 }
  153.             }
  154.         }
  155.         if(!right_sensor && left_sensor) {
  156.             wb_motor_set_velocity(right_motor, 1);
  157.             wb_motor_set_velocity(left_motor, -3);
  158.             while(wb_robot_step(TIME_STEP) != -1) {
  159.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  160.                 if(abs(rc.b - lc.b) < 150 && abs(rc.g - lc.g) < 150 && abs(rc.r - lc.r) < 150) {
  161.                     wb_motor_set_velocity(right_motor, 0);
  162.                     wb_motor_set_velocity(left_motor, 0);
  163.                     return;
  164.                 }
  165.             }
  166.         }
  167.         if(right_sensor && left_sensor) {
  168.             wb_motor_set_velocity(right_motor, 0);
  169.             wb_motor_set_velocity(left_motor, 0);
  170.             return;
  171.         }
  172.     }
  173. }
  174.  
  175. void alinhar(rgb min, rgb max) {
  176.     while(wb_robot_step(TIME_STEP) != -1) {
  177.         wb_motor_set_velocity(right_motor, 2);
  178.         wb_motor_set_velocity(left_motor, 2);
  179.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  180.         if(rc.r >= min.r && rc.r <= max.r && rc.b >= min.b && rc.b <= max.b && rc.g >= min.g && rc.g <= max.g) {
  181.             while(wb_robot_step(TIME_STEP) != -1) {
  182.                 wb_motor_set_velocity(left_motor, 2);
  183.                 wb_motor_set_velocity(right_motor, 0);
  184.                 if(lc.r >= min.r && lc.r <= max.r && lc.b >= min.b && lc.b <= max.b && lc.g >= min.g && lc.g <= max.g) {
  185.                     return;
  186.                 }
  187.             }
  188.         }
  189.         if(lc.r >= min.r && lc.r <= max.r && lc.b >= min.b && lc.b <= max.b && lc.g >= min.g && lc.g <= max.g) {
  190.             while(wb_robot_step(TIME_STEP) != -1) {
  191.                 wb_motor_set_velocity(left_motor, 0);
  192.                 wb_motor_set_velocity(right_motor, 2);
  193.                 if(rc.r >= min.r && rc.r <= max.r && rc.b >= min.b && rc.b <= max.b && rc.g >= min.g && rc.g <= max.g) {
  194.                     return;
  195.                 }
  196.             }
  197.         }
  198.     }
  199. }
  200.  
  201. void desviar() {
  202.     rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  203.     if(rc.g < 5800 && rc.b < 5800 && rc.r < 5800) {
  204.         while(wb_robot_step(TIME_STEP) != -1) {
  205.             wb_motor_set_velocity(right_motor, -2);
  206.             wb_motor_set_velocity(left_motor, 0);
  207.             if(rc.g > 5800 && rc.b > 5800 && rc.r > 5800) {
  208.                 return;
  209.             }
  210.         }
  211.     }
  212.     if(lc.g < 5800 && lc.b < 5800 && lc.r < 5800) {
  213.         while(wb_robot_step(TIME_STEP) != -1) {
  214.             wb_motor_set_velocity(right_motor, 0);
  215.             wb_motor_set_velocity(left_motor, -2);
  216.             if(lc.g > 5800 && lc.b > 5800 && lc.r > 5800) {
  217.                 return;
  218.             }
  219.         }
  220.     }
  221.     return;
  222. }
  223.  
  224. void giro_() {
  225.     int j = 0;
  226.     while(wb_robot_step(TIME_STEP) != -1) {
  227.         wb_motor_set_velocity(right_motor, -1);
  228.         wb_motor_set_velocity(left_motor, 2);
  229.         j++;
  230.         if(j == 350) {
  231.             wb_motor_set_velocity(right_motor, 0);
  232.             wb_motor_set_velocity(left_motor, 0);
  233.             break;
  234.         }
  235.     }
  236.     return;
  237. }
  238.  
  239. void _giro() {
  240.     int j = 0;
  241.     while(wb_robot_step(TIME_STEP) != -1) {
  242.         wb_motor_set_velocity(right_motor, 2);
  243.         wb_motor_set_velocity(left_motor, -1);
  244.         j++;
  245.         if(j == 350) {
  246.             wb_motor_set_velocity(right_motor, 0);
  247.             wb_motor_set_velocity(left_motor, 0);
  248.             break;
  249.         }
  250.     }
  251.     return;
  252. }
  253.  
  254. void _giro_() {
  255.     _giro();
  256.     _giro();
  257.     return;
  258. }
  259.  
  260. void vi0() {
  261.     while(wb_robot_step(TIME_STEP) != -1) {
  262.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  263.         if((rc.r < 9000 && rc.g < 8000 && rc.b < 7000) || (lc.r < 9000 && lc.g < 8000 && lc.b < 7000)) {
  264.             break;
  265.         }
  266.         wb_motor_set_velocity(right_motor, 2);
  267.         wb_motor_set_velocity(left_motor, 2);
  268.         if(rc.r >= verde_min.r && rc.r <= verde_max.r && rc.g >= verde_min.g && rc.g <= verde_max.g && rc.b >= verde_min.b && rc.b <= verde_max.b) {
  269.             _giro();
  270.         }
  271.         if(lc.r >= verde_min.r && lc.r <= verde_max.r && lc.g >= verde_min.g && lc.g <= verde_max.g && lc.b >= verde_min.b && lc.b <= verde_max.b) {
  272.             _giro_();
  273.         }
  274.     }
  275.     alinhar_chao();
  276. }
  277.  
  278. void via1() {
  279.     wb_motor_set_velocity(right_motor, -2);
  280.     wb_motor_set_velocity(left_motor, -2);
  281.     giro_();
  282.     while(wb_robot_step(TIME_STEP) != -1) {
  283.         wb_motor_set_velocity(right_motor, 2);
  284.         wb_motor_set_velocity(left_motor, 2);
  285.         double fr = wb_distance_sensor_get_value(frds), fl = wb_distance_sensor_get_value(flds);
  286.         double avg = (fr + fl) / 2;
  287.         if(avg < 150) {
  288.             break;
  289.         }
  290.         rgb lc = getrgbs(left_camera);
  291.         if(lc.r < 3200 && lc.g < 3200 && lc.b < 3200) {
  292.             wb_motor_set_velocity(right_motor, 1);
  293.             int j = 0;
  294.             for(int i = 0; i < 20000000; i++) {
  295.                 j++;
  296.             }
  297.             wb_motor_set_velocity(right_motor, 2);
  298.         }
  299.     }
  300.     giro_();
  301. }
  302.  
  303. void via2() {
  304.     while(wb_robot_step(TIME_STEP) != -1) {
  305.         wb_motor_set_velocity(right_motor, 2);
  306.         wb_motor_set_velocity(left_motor, 2);
  307.         double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(frds);
  308.         if(dsl > 100 || dsr < 100) {
  309.             wb_motor_set_velocity(right_motor, 0);
  310.             wb_motor_set_velocity(left_motor, 0);
  311.             break;
  312.         }
  313.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  314.         rgb md = {(rc.r + lc.r) / 2, (rc.g + lc.g) / 2, (rc.b + lc.b) / 2};
  315.         if(md.r < 5800 && md.g < 5800 && md.b < 5800) {
  316.             return;
  317.         }
  318.     }
  319.     double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(frds), dsb = wb_distance_sensor_get_value(lbds);
  320.     if(dsl > 100) {
  321.         if(dsb < 100 && dsr > 100) {
  322.             return;
  323.         } else if(dsb < 100 & dsr < 100) {
  324.             giro_();
  325.             via2();
  326.         } else {
  327.             wb_motor_set_velocity(right_motor, 2);
  328.             wb_motor_set_velocity(left_motor, 2);
  329.             int j = 0;
  330.             for(int i = 0; i < 20000000; i++) {
  331.                 j++;
  332.             }
  333.             _giro();
  334.             via2();
  335.         }
  336.     }
  337.     if(dsr < 100) {
  338.         giro_();
  339.         via2();
  340.     }
  341. }
  342.  
  343. void vit1() {
  344.     giro_();
  345.     while(wb_robot_step(TIME_STEP) != -1) {
  346.         wb_motor_set_velocity(right_motor, 2);
  347.         wb_motor_set_velocity(left_motor, 2);
  348.         rgb lc = getrgbs(left_camera), rc = getrgbs(right_camera);
  349.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  350.         if(avg.r < 5800 && avg.g < 5800 && avg.b < 5800) {
  351.             break;
  352.         } else {
  353.             desviar();
  354.         }
  355.     }
  356.     rgb mn = {0, 0, 0}, mx = {1700, 1700, 1700};
  357.     alinhar(mn, mx);
  358. }
  359.  
  360. void vit3(rgb cor_min, rgb cor_max) {
  361.     wb_motor_set_velocity(right_motor, 2);
  362.     wb_motor_set_velocity(left_motor, 2);
  363.     int j = 0;
  364.     for(int i = 0; i < 20000000; i++) {
  365.         j++;
  366.     }
  367.     wb_motor_set_velocity(right_motor, 0);
  368.     wb_motor_set_velocity(left_motor, 0);
  369.     while(wb_robot_step(TIME_STEP) != -1) {
  370.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  371.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  372.         if(avg.r >= cor_min.r && avg.r <= cor_max.r && avg.b >= cor_min.b && avg.b <= cor_max.b && avg.g >= cor_min.g && avg.g <= cor_max.g) {
  373.             giro_();
  374.             return;
  375.         } else {
  376.             _giro();
  377.             int deu180 = 0;
  378.             while(wb_robot_step(TIME_STEP) != -1) {
  379.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  380.                 rgb aavg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  381.                 wb_motor_set_velocity(right_motor, 2);
  382.                 wb_motor_set_velocity(left_motor, 2);
  383.                 if(aavg.r >= cor_min.r && aavg.r <= cor_max.r && aavg.b >= cor_min.b && aavg.b <= cor_max.b && aavg.g >= cor_min.g && aavg.g <= cor_max.g) {
  384.                     if(!deu180) {
  385.                         _giro_();
  386.                     }
  387.                     return;
  388.                 }
  389.                 if(aavg.r <= 5800 && aavg.b <= 5800 && aavg.g <= 5800) {
  390.                     _giro_();
  391.                     deu180 = 1;
  392.                 }
  393.             }
  394.         }
  395.     }
  396. }
  397.  
  398. void vit3_1(rgb cor_min, rgb cor_max){
  399.     while(wb_robot_step(TIME_STEP) != -1) {
  400.  
  401.         double ltdsvalue = wb_distance_sensor_get_value(ltds);
  402.         double lbdsvalue = wb_distance_sensor_get_value(lbds);
  403.  
  404.         if (ltdsvalue > 40 && lbdsvalue > 40){
  405.             rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  406.             rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  407.             if(avg.r >= cor_min.r && avg.r <= cor_max.r && avg.b >= cor_min.b && avg.b <= cor_max.b && avg.g >= cor_min.g && avg.g <= cor_max.g){
  408.                 wb_motor_set_velocity(left_motor, 2);
  409.                 wb_motor_set_velocity(right_motor, 2);
  410.             }
  411.             else{
  412.                 _giro_();
  413.             }
  414.         }
  415.         _giro();
  416.  
  417.         double frdsvalue = wb_distance_sensor_get_value(frds);
  418.         double fldsvalue = wb_distance_sensor_get_value(flds);
  419.  
  420.         while (frds > 100 || flds > 100){
  421.             double frdsvalue = wb_distance_sensor_get_value(frds);
  422.             double fldsvalue = wb_distance_sensor_get_value(flds);
  423.             wb_motor_set_velocity(left_motor, 0.08*(flds - 100));
  424.             wb_motor_set_velocity(right_motor, 0.08*(frds - 100));
  425.         }
  426.         wb_motor_set_velocity(left_motor, 0);
  427.         wb_motor_set_velocity(right_motor, 0);
  428.     }
  429. }
  430.  
  431.  
  432.  
  433. int main(int argc, char **argv) {
  434.     //creating our tags and variables
  435.     int pause_counter = 0;
  436.     int left_speed, right_speed;
  437.     wb_robot_init();
  438.    
  439.     //initializing our tags
  440.     frds = wb_robot_get_device("FrontRightDistanceSensor");
  441.     flds = wb_robot_get_device("FrontLeftDistanceSensor");
  442.     ltds = wb_robot_get_device("LeftTopDistanceSensor");
  443.     lbds = wb_robot_get_device("LeftBottomDistanceSensor");
  444.     pos = wb_robot_get_device("EMPPositionSensor");
  445.     right_camera = wb_robot_get_device("RightCamera");
  446.     left_camera = wb_robot_get_device("LeftCamera");
  447.     width = wb_camera_get_width(right_camera);
  448.     height = wb_camera_get_height(right_camera);
  449.     left_motor = wb_robot_get_device("LeftWheel");
  450.     right_motor = wb_robot_get_device("RightWheel");
  451.     empilhadeira = wb_robot_get_device("EMPLinearMotor");
  452.     wb_distance_sensor_enable(frds, TIME_STEP);
  453.     wb_distance_sensor_enable(flds, TIME_STEP);
  454.     wb_distance_sensor_enable(ltds, TIME_STEP);
  455.     wb_distance_sensor_enable(lbds, TIME_STEP);
  456.     wb_position_sensor_enable(pos, TIME_STEP);
  457.     wb_camera_enable(right_camera, TIME_STEP);
  458.     wb_camera_enable(left_camera, TIME_STEP);
  459.     wb_motor_set_position(left_motor, INFINITY);
  460.     wb_motor_set_position(right_motor, INFINITY);
  461.     wb_motor_set_velocity(left_motor, 0.0);
  462.     wb_motor_set_velocity(right_motor, 0.0);
  463.  
  464.     vi0();
  465.     via1();
  466.     via2();
  467.     double l = odometria();
  468.     printf("%lf\n", l);
  469.     /*while (wb_robot_step(TIME_STEP) != -1) {
  470.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  471.         printf("Left values: %d %d %d\n", lc.r, lc.g, lc.b);
  472.         printf("Right values: %d %d %d\n", rc.r, rc.g, rc.b);
  473.     } */
  474.  
  475.     wb_robot_cleanup();
  476.     return 0;
  477. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!