API tools faq
paste
Advertisement
cosenza987

Untitled

cosenza987
Oct 14th, 2021
783
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 37.14 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 <math.h>
  6. #include <webots/camera.h>
  7. #include <webots/position_sensor.h>
  8. #include <webots/distance_sensor.h>
  9. #include <webots/inertial_unit.h>
  10. #include <webots/motor.h>
  11. #include <webots/robot.h>
  12. #include <webots/utils/system.h>
  13.  
  14. #define TIME_STEP 32
  15.  
  16. int width, height, tamanhodorobo = 27, d180 = 1, color_detected, viuchao = 0;
  17. WbDeviceTag right_camera, left_camera, left_motor, right_motor, empilhadeira, frds, flds, fmds, ltds, lbds, pos, iu, left_pos, right_pos;
  18. double epsilon = 3;
  19.  
  20. typedef struct _rgb {
  21.     int r, g, b;
  22. } rgb;
  23.  
  24. rgb verde_min = {1000, 12000, 1000}, verde_max = {10000, 14000, 10000};
  25.  
  26. rgb getrgbs(WbDeviceTag camera) {
  27.     const unsigned char *image = wb_camera_get_image(camera);
  28.     rgb ans = {0, 0, 0};
  29.     for(int i = 0; i < width; i++) {
  30.         for(int j = 0; j < height; j++) {
  31.             ans.r += wb_camera_image_get_red(image, width, i, j);
  32.             ans.g += wb_camera_image_get_green(image, width, i, j);
  33.             ans.b += wb_camera_image_get_blue(image, width, i, j);
  34.         }
  35.     }
  36.     return ans;
  37. }
  38.  
  39. void pegar_tubo() { //verificado
  40.  
  41.     while(wb_robot_step(TIME_STEP) != -1) {
  42.        
  43.         double frdsvalue = wb_distance_sensor_get_value(frds);
  44.         double fldsvalue = wb_distance_sensor_get_value(flds);
  45.         wb_motor_set_velocity(left_motor, 1);
  46.         wb_motor_set_velocity(right_motor, 1);
  47.        
  48.         if(frdsvalue < 75 && fldsvalue < 75) {
  49.             //printf("oi");
  50.             wb_motor_set_velocity(left_motor, 0);
  51.             wb_motor_set_velocity(right_motor, 0);
  52.             while(wb_robot_step(TIME_STEP) != -1) {
  53.                 double k = wb_position_sensor_get_value(pos);
  54.                 wb_motor_set_velocity(empilhadeira, -0.3);
  55.                 if(k < -0.12) {
  56.                     wb_motor_set_velocity(empilhadeira, 0);
  57.                     break;
  58.                 }
  59.             }
  60.             break;
  61.         }    
  62.     }
  63.     while(wb_robot_step(TIME_STEP) != -1) {
  64.         wb_motor_set_velocity(left_motor, 0.5);
  65.         wb_motor_set_velocity(right_motor, 0.5);
  66.         double frdsvalue = wb_distance_sensor_get_value(frds);
  67.         double fldsvalue = wb_distance_sensor_get_value(flds);
  68.         if(frdsvalue < 20 && fldsvalue < 20){
  69.             wb_motor_set_velocity(left_motor, 0);
  70.             wb_motor_set_velocity(right_motor, 0);
  71.  
  72.             while(wb_robot_step(TIME_STEP) != -1) {
  73.                
  74.                 double k = wb_position_sensor_get_value(pos);
  75.                 //printf("%lf\n", k);
  76.                 wb_motor_set_velocity(empilhadeira, 0.3);
  77.                 if(k > 0.176) {
  78.                     break;
  79.                 }
  80.             }
  81.            
  82.             wb_motor_set_velocity(empilhadeira, 0);
  83.             return;
  84.         }
  85.     }
  86. }
  87.  
  88. void entregar_tubo() { //verificado
  89.    
  90.     while(wb_robot_step(TIME_STEP) != -1) {
  91.         double fldsvalue = wb_distance_sensor_get_value(flds);
  92.         double frdsvalue = wb_distance_sensor_get_value(frds);
  93.         wb_motor_set_velocity(right_motor, 1);
  94.         wb_motor_set_velocity(left_motor, 1);
  95.         if(fldsvalue < 40 && frdsvalue < 40) {
  96.             wb_motor_set_velocity(right_motor, 0);
  97.             wb_motor_set_velocity(left_motor, 0);
  98.             break;
  99.         }
  100.     }
  101.     while(wb_robot_step(TIME_STEP) != -1) {
  102.         double k = wb_position_sensor_get_value(pos);
  103.         wb_motor_set_velocity(empilhadeira, -0.3);
  104.         if(k < 0.014) {
  105.             wb_motor_set_velocity(empilhadeira, 0);
  106.             break;
  107.         }
  108.     }
  109.    
  110.     while(wb_robot_step(TIME_STEP) != -1) {
  111.  
  112.         double frdsvalue = wb_distance_sensor_get_value(frds);
  113.         double fldsvalue = wb_distance_sensor_get_value(flds);
  114.        
  115.         wb_motor_set_velocity(left_motor, -1);
  116.         wb_motor_set_velocity(right_motor, -1);
  117.  
  118.         if(frdsvalue > 200 && fldsvalue > 200){
  119.             wb_motor_set_velocity(left_motor, 0);
  120.             wb_motor_set_velocity(right_motor, 0);
  121.  
  122.             while(wb_robot_step(TIME_STEP) != -1) {
  123.                 double k = wb_position_sensor_get_value(pos);
  124.                 wb_motor_set_velocity(empilhadeira, 0.3);
  125.                 if(k > 0.1) {
  126.                     wb_motor_set_velocity(empilhadeira, 0);
  127.                     break;
  128.                 }  
  129.             }
  130.             int j = 0;
  131.             while(wb_robot_step(TIME_STEP) != -1) {
  132.                 j++;
  133.                 if(j == 20) {
  134.                     break;
  135.                 }
  136.             }
  137.             return;
  138.         }
  139.     }
  140. }
  141.  
  142. double odometria() { //verificado
  143.     double tube;
  144.     double pos_inicial = wb_position_sensor_get_value(left_pos);
  145.  
  146.     clock_t start = clock();
  147.     printf("start %lf\n", pos_inicial);
  148.  
  149.     while(wb_robot_step(TIME_STEP) != -1) {
  150.         double cur = wb_position_sensor_get_value(left_pos);
  151.         double ltdsvalue = wb_distance_sensor_get_value(ltds);
  152.         double lbdsvalue = wb_distance_sensor_get_value(lbds);
  153.         printf("%lf\n", cur);
  154.         wb_motor_set_velocity(left_motor, 2);
  155.         wb_motor_set_velocity(right_motor, 2);
  156.  
  157.         if(ltdsvalue < 200) {
  158.             wb_motor_set_velocity(left_motor, 0);  
  159.             wb_motor_set_velocity(right_motor, 0);
  160.  
  161.             break;
  162.         }
  163.  
  164.         if(lbdsvalue > 150) {
  165.             wb_motor_set_velocity(left_motor, 0);  
  166.             wb_motor_set_velocity(right_motor, 0);
  167.             break;
  168.         }
  169.  
  170.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  171.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  172.  
  173.         if(avg.b < 7000 && avg.r < 9000 && avg.g < 8000) {
  174.             wb_motor_set_velocity(left_motor, 0);  
  175.             wb_motor_set_velocity(right_motor, 0);
  176.  
  177.             clock_t end = clock();
  178.             //tube = 2.0*(end - start) + 27;
  179.             tube = (cur - pos_inicial) * 0.0374 * 100;
  180.             if(tube < 10) {
  181.                 if(!via2()) {
  182.                     exit(0);
  183.                 }
  184.                 return odometria();
  185.             }
  186.             return tube;
  187.         }
  188.  
  189.     }
  190.  
  191.     //clock_t end = clock();
  192.     double end = wb_position_sensor_get_value(left_pos);
  193.  
  194.     tube = ((end - pos_inicial) * 0.0374) * 100;
  195.     if(tube < 10) {
  196.         if(!via2()) {
  197.             exit(0);
  198.         }
  199.         return odometria();
  200.     }
  201.  
  202.     return tube;
  203. }
  204.  
  205. void alinhar_chao() {//verificado
  206.     while(wb_robot_step(TIME_STEP) != -1) {
  207.         wb_motor_set_velocity(right_motor, 2);
  208.         wb_motor_set_velocity(left_motor, 2);
  209.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  210.         int left_sensor = 0, right_sensor = 0;
  211.         if(rc.b < 7000 && rc.r < 9000 && rc.g < 8000) {
  212.             right_sensor = 1;
  213.         }
  214.         if(lc.b < 7000 && lc.r < 9000 && lc.g < 8000) {
  215.             left_sensor = 1;
  216.         }
  217.         if(right_sensor && !left_sensor) {
  218.             wb_motor_set_velocity(right_motor, 0);
  219.             wb_motor_set_velocity(left_motor, 0.4);
  220.             while(wb_robot_step(TIME_STEP) != -1) {
  221.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  222.                 if(abs(rc.b - lc.b) < 200 && abs(rc.g - lc.g) < 200 && abs(rc.r - lc.r) < 200) {
  223.                     wb_motor_set_velocity(right_motor, -3);
  224.                     wb_motor_set_velocity(left_motor, -3);
  225.                     int j = 0;
  226.                     while(wb_robot_step(TIME_STEP) != -1) {
  227.                         j++;
  228.                         if(j == 50) {
  229.                             break;
  230.                         }
  231.                     }
  232.                     return;
  233.                 }
  234.             }
  235.         }
  236.         if(!right_sensor && left_sensor) {
  237.             wb_motor_set_velocity(right_motor, 0.4);
  238.             wb_motor_set_velocity(left_motor, 0);
  239.             while(wb_robot_step(TIME_STEP) != -1) {
  240.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  241.                 if(abs(rc.b - lc.b) < 200 && abs(rc.g - lc.g) < 200 && abs(rc.r - lc.r) < 200) {
  242.                     wb_motor_set_velocity(right_motor, -3);
  243.                     wb_motor_set_velocity(left_motor, -3);
  244.                     int j = 0;
  245.                     while(wb_robot_step(TIME_STEP) != -1) {
  246.                         j++;
  247.                         if(j == 50) {
  248.                             break;
  249.                         }
  250.                     }
  251.                     return;
  252.                 }
  253.             }
  254.         }
  255.         if(right_sensor && left_sensor) {
  256.             wb_motor_set_velocity(right_motor, -3);
  257.             wb_motor_set_velocity(left_motor, -3);
  258.             int j = 0;
  259.             while(wb_robot_step(TIME_STEP) != -1) {
  260.                 j++;
  261.                 if(j == 50) {
  262.                     break;
  263.                 }
  264.             }
  265.             return;
  266.         }
  267.     }
  268. }
  269.  
  270. void alinhar(rgb min, rgb max) { //verificado
  271.     while(wb_robot_step(TIME_STEP) != -1) {
  272.         wb_motor_set_velocity(right_motor, 4);
  273.         wb_motor_set_velocity(left_motor, 4);
  274.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  275.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  276.         //printf("avg %d %d %d\n", avg.r, avg.g, avg.b);
  277.         if(avg.r >= min.r && avg.r <= max.r && avg.b >= min.b && avg.b <= max.b && avg.g >= min.g && avg.g <= max.g) {
  278.             int j = 0;
  279.             while(wb_robot_step(TIME_STEP) != -1) {
  280.                 wb_motor_set_velocity(right_motor, 2);
  281.                 wb_motor_set_velocity(left_motor, 2);
  282.                 j++;
  283.                 if(j == 29) {
  284.                     wb_motor_set_velocity(right_motor, 0);
  285.                     wb_motor_set_velocity(left_motor, 0);
  286.                     break;
  287.                 }
  288.             }
  289.             return;
  290.         }
  291.         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) {
  292.             while(wb_robot_step(TIME_STEP) != -1) {
  293.                 wb_motor_set_velocity(left_motor, 0.5);
  294.                 wb_motor_set_velocity(right_motor, 0);
  295.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  296.                 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) {
  297.                     int j = 0;
  298.                     while(wb_robot_step(TIME_STEP) != -1) {
  299.                         wb_motor_set_velocity(right_motor, 2);
  300.                         wb_motor_set_velocity(left_motor, 2);
  301.                         j++;
  302.                         if(j == 29) {
  303.                             wb_motor_set_velocity(right_motor, 0);
  304.                             wb_motor_set_velocity(left_motor, 0);
  305.                             break;
  306.                         }
  307.                     }
  308.                     return;
  309.                 }
  310.             }
  311.         }
  312.         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) {
  313.             while(wb_robot_step(TIME_STEP) != -1) {
  314.                 wb_motor_set_velocity(left_motor, 0);
  315.                 wb_motor_set_velocity(right_motor, 0.5);
  316.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  317.                 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) {
  318.                     int j = 0;
  319.                     while(wb_robot_step(TIME_STEP) != -1) {
  320.                         wb_motor_set_velocity(right_motor, 2);
  321.                         wb_motor_set_velocity(left_motor, 2);
  322.                         j++;
  323.                         if(j == 29) {
  324.                             wb_motor_set_velocity(right_motor, 0);
  325.                             wb_motor_set_velocity(left_motor, 0);
  326.                             break;
  327.                         }
  328.                     }
  329.                     return;
  330.                 }
  331.             }
  332.         }
  333.     }
  334. }
  335.  
  336. void desviar() {
  337.     rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  338.     if(rc.g < 5800 && rc.b < 5800 && rc.r < 5800) {
  339.         while(wb_robot_step(TIME_STEP) != -1) {
  340.             wb_motor_set_velocity(right_motor, -2);
  341.             wb_motor_set_velocity(left_motor, 0);
  342.             if(rc.g > 5800 && rc.b > 5800 && rc.r > 5800) {
  343.                 return;
  344.             }
  345.         }
  346.     }
  347.     if(lc.g < 5800 && lc.b < 5800 && lc.r < 5800) {
  348.         while(wb_robot_step(TIME_STEP) != -1) {
  349.             wb_motor_set_velocity(right_motor, 0);
  350.             wb_motor_set_velocity(left_motor, -2);
  351.             if(lc.g > 5800 && lc.b > 5800 && lc.r > 5800) {
  352.                 return;
  353.             }
  354.         }
  355.     }
  356.     return;
  357. }
  358.  
  359. void giro_() { //verificado
  360.     wb_motor_set_velocity(right_motor, -0.5);
  361.     wb_motor_set_velocity(left_motor, 0.5);
  362.     wb_inertial_unit_enable(iu, TIME_STEP);
  363.     const double *init = wb_inertial_unit_get_roll_pitch_yaw(iu);
  364.     double k = init[2], prev = k;
  365.     int cnt = 0;
  366.     while(wb_robot_step(TIME_STEP) != -1) {
  367.         const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  368.         double cur = val[2];
  369.         //printf("%lf %lf %lf\n", val[2], k, fabs(val[2] - k));
  370.         if(cur > 0) {
  371.             if(prev < 0 && fabs(k - prev) > 1.3 && cnt > 20) {
  372.                 break;
  373.             } else if(prev > 0) {
  374.                 k *= -1.0;
  375.             }
  376.         } else {
  377.             if(prev > 0 && fabs(k - prev) > 1.3 && cnt > 20) {
  378.                 break;
  379.             } else if(prev < 0) {
  380.                 k *= -1.0;
  381.             }
  382.         }
  383.         if(fabs(val[2] - k) >= 1.56 && fabs(val[2] - k) <= 1.58 && cnt > 20) {
  384.             break;
  385.         }
  386.         prev = cur;
  387.         cnt++;
  388.     }
  389.     wb_inertial_unit_disable(iu);
  390.     wb_motor_set_velocity(right_motor, 0);
  391.     wb_motor_set_velocity(left_motor, 0);
  392.     return;
  393. }
  394.  
  395. void _giro() { //verificado
  396.     wb_motor_set_velocity(right_motor, 0.5);
  397.     wb_motor_set_velocity(left_motor, -0.5);
  398.     wb_inertial_unit_enable(iu, TIME_STEP);
  399.     const double *init = wb_inertial_unit_get_roll_pitch_yaw(iu);
  400.     double k = init[2], prev = k;
  401.     int cnt = 0;
  402.     while(wb_robot_step(TIME_STEP) != -1) {
  403.         const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  404.         double cur = val[2];
  405.         //printf("%lf %lf %lf\n", val[2], k, fabs(val[2] - k));
  406.         if(cur > 0) {
  407.             if(prev < 0 && fabs(k - prev) > 1.3) {
  408.                 break;
  409.             } else if(prev > 0) {
  410.                 k *= -1.0;
  411.             }
  412.         } else {
  413.             if(prev > 0 && fabs(k - prev) > 1.3) {
  414.                 break;
  415.             } else if(prev > 0) {
  416.                 k *= -1.0;
  417.             }
  418.         }
  419.         if(fabs(val[2] - k) >= 1.56 && fabs(val[2] - k) <= 1.58 && cnt > 20) {
  420.             break;
  421.         }
  422.         prev = cur;
  423.         cnt++;
  424.     }
  425.     wb_inertial_unit_disable(iu);
  426.     wb_motor_set_velocity(right_motor, 0);
  427.     wb_motor_set_velocity(left_motor, 0);
  428.     return;
  429. }
  430.  
  431. void _giro_() { //verificado
  432.     giro_();
  433.     giro_();
  434.     return;
  435. }
  436.  
  437. void vi0() { //verificado
  438.     wb_motor_set_position(empilhadeira, INFINITY);
  439.     while(wb_robot_step(TIME_STEP) != -1) {
  440.         wb_motor_set_velocity(right_motor, 6);
  441.         wb_motor_set_velocity(left_motor, 6);
  442.         double dis = wb_distance_sensor_get_value(fmds);
  443.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  444.         printf("%lf\n", dis);
  445.         if(dis > 100) {
  446.             int j = 0;
  447.             while(wb_robot_step(TIME_STEP) != -1) {
  448.                 wb_motor_set_velocity(right_motor, -2);
  449.                 wb_motor_set_velocity(left_motor, -2);
  450.                 j++;
  451.                 if(j == 55) {
  452.                     wb_motor_set_velocity(right_motor, 0);
  453.                     wb_motor_set_velocity(left_motor, 0);
  454.                     break;
  455.                 }
  456.             }
  457.             viuchao = 1;
  458.             giro_();
  459.         }
  460.         if((rc.r < 5000 && rc.g < 5000 && rc.b < 5000) || (lc.r < 5000 && lc.g < 5000 && lc.b < 5000)) {
  461.             int j = 0;
  462.             while(wb_robot_step(TIME_STEP) != -1) {
  463.                 wb_motor_set_velocity(right_motor, -2);
  464.                 wb_motor_set_velocity(left_motor, -2);
  465.                 j++;
  466.                 if(j == 30) {
  467.                     wb_motor_set_velocity(right_motor, 0);
  468.                     wb_motor_set_velocity(left_motor, 0);
  469.                     break;
  470.                 }
  471.             }
  472.             giro_();
  473.         }
  474.         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) || (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)) {
  475.             if(viuchao) {
  476.                 return;
  477.             } else {
  478.                 int j = 0;
  479.                 while(wb_robot_step(TIME_STEP) != -1) {
  480.                     wb_motor_set_velocity(right_motor, -2);
  481.                     wb_motor_set_velocity(left_motor, -2);
  482.                     j++;
  483.                     if(j == 30) {
  484.                         wb_motor_set_velocity(right_motor, 0);
  485.                         wb_motor_set_velocity(left_motor, 0);
  486.                         break;
  487.                     }
  488.                 }
  489.                 giro_();
  490.             }
  491.         }
  492.     }
  493. }
  494.  
  495. void vi0_tubo() { //verificado
  496.     viuchao = 0;
  497.     while(wb_robot_step(TIME_STEP) != -1) {
  498.         wb_motor_set_velocity(right_motor, 3);
  499.         wb_motor_set_velocity(left_motor, 3);
  500.         double dis = wb_distance_sensor_get_value(fmds);
  501.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  502.         printf("right %d %d %d\n", rc.r, rc.g, rc.b);
  503.         printf("left %d %d %d\n", lc.r, lc.g, lc.b);
  504.         if(dis > 100) {
  505.             int j = 0;
  506.             while(wb_robot_step(TIME_STEP) != -1) {
  507.                 wb_motor_set_velocity(right_motor, -2);
  508.                 wb_motor_set_velocity(left_motor, -2);
  509.                 j++;
  510.                 if(j == 55) {
  511.                     wb_motor_set_velocity(right_motor, 0);
  512.                     wb_motor_set_velocity(left_motor, 0);
  513.                     break;
  514.                 }
  515.             }
  516.             viuchao = 1;
  517.             giro_();
  518.         }
  519.         if((rc.r < 5000 && rc.g < 5000 && rc.b < 5000) || (lc.r < 5000 && lc.g < 5000 && lc.b < 5000)) {
  520.             int j = 0;
  521.             while(wb_robot_step(TIME_STEP) != -1) {
  522.                 wb_motor_set_velocity(right_motor, -2);
  523.                 wb_motor_set_velocity(left_motor, -2);
  524.                 j++;
  525.                 if(j == 30) {
  526.                     wb_motor_set_velocity(right_motor, 0);
  527.                     wb_motor_set_velocity(left_motor, 0);
  528.                     break;
  529.                 }
  530.             }
  531.             giro_();
  532.         }
  533.         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) || (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)) {
  534.             if(viuchao) {
  535.                 return;
  536.             } else {
  537.                 int j = 0;
  538.                 while(wb_robot_step(TIME_STEP) != -1) {
  539.                     wb_motor_set_velocity(right_motor, -2);
  540.                     wb_motor_set_velocity(left_motor, -2);
  541.                     j++;
  542.                     if(j == 30) {
  543.                         wb_motor_set_velocity(right_motor, 0);
  544.                         wb_motor_set_velocity(left_motor, 0);
  545.                         break;
  546.                     }
  547.                 }
  548.                 giro_();
  549.             }
  550.         }
  551.     }
  552. }
  553.  
  554. void via1() { //verificado
  555.     //wb_motor_set_velocity(right_motor, -2);
  556.     //wb_motor_set_velocity(left_motor, -2);
  557.     //giro_();
  558.     wb_inertial_unit_enable(iu, TIME_STEP);
  559.     const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  560.     //printf("yaw = %lf\n", val[2]);
  561.     if(val[2] <= -1.5 && val[2] >= -1.6) {
  562.         vi0();
  563.     }
  564.     wb_inertial_unit_disable(iu);
  565.     while(wb_robot_step(TIME_STEP) != -1) {
  566.         wb_motor_set_velocity(right_motor, 6);
  567.         wb_motor_set_velocity(left_motor, 6);
  568.         double fr = wb_distance_sensor_get_value(frds), fl = wb_distance_sensor_get_value(flds);
  569.         double avg = (fr + fl) / 2;
  570.         if(avg < 200) {
  571.             break;
  572.         }
  573.         rgb lc = getrgbs(left_camera);
  574.         if(lc.r < 3200 && lc.g < 3200 && lc.b < 3200) {
  575.             wb_motor_set_velocity(right_motor, 1);
  576.             int j = 0;
  577.             for(int i = 0; i < 20000000; i++) {
  578.                 j++;
  579.             }
  580.             wb_motor_set_velocity(right_motor, 2);
  581.         }
  582.     }
  583.     giro_();
  584. }
  585.  
  586. void via1_tube() { //verificado
  587.     //wb_motor_set_velocity(right_motor, -2);
  588.     //wb_motor_set_velocity(left_motor, -2);
  589.     //giro_();
  590.     wb_inertial_unit_enable(iu, TIME_STEP);
  591.     const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  592.     //printf("yaw = %lf\n", val[2]);
  593.     if(val[2] <= -1.5 && val[2] >= -1.6) {
  594.         vi0();
  595.     }
  596.     wb_inertial_unit_disable(iu);
  597.     while(wb_robot_step(TIME_STEP) != -1) {
  598.         rgb lc = getrgbs(left_camera), rc = getrgbs(right_camera);
  599.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  600.         if(avg.r <= 4000 && avg.g <= 15000 && avg.b <= 4000) {
  601.             wb_motor_set_velocity(right_motor, 0.5);
  602.             wb_motor_set_velocity(left_motor, 0.5);
  603.         } else {
  604.             wb_motor_set_velocity(right_motor, 3);
  605.             wb_motor_set_velocity(left_motor, 3);
  606.         }
  607.         double fr = wb_distance_sensor_get_value(frds), fl = wb_distance_sensor_get_value(flds);
  608.         double avgg = (fr + fl) / 2;
  609.         if(avgg < 250) {
  610.             break;
  611.         }
  612.         if(lc.r < 3200 && lc.g < 3200 && lc.b < 3200) {
  613.             wb_motor_set_velocity(right_motor, 1);
  614.             int j = 0;
  615.             for(int i = 0; i < 20000000; i++) {
  616.                 j++;
  617.             }
  618.             wb_motor_set_velocity(right_motor, 2);
  619.         }
  620.     }
  621.     giro_();
  622. }
  623.  
  624. int via2() { //verificado
  625.     while(wb_robot_step(TIME_STEP) != -1) {
  626.         wb_motor_set_velocity(right_motor, 5);
  627.         wb_motor_set_velocity(left_motor, 5);
  628.         double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(flds);
  629.         if(dsl > 200 || dsr < 200) {
  630.             wb_motor_set_velocity(right_motor, 0);
  631.             wb_motor_set_velocity(left_motor, 0);
  632.             break;
  633.         }
  634.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  635.         rgb md = {(rc.r + lc.r) / 2, (rc.g + lc.g) / 2, (rc.b + lc.b) / 2};
  636.         if(md.r < 9000 && md.g < 8000 && md.b < 7000) {
  637.             wb_motor_set_velocity(right_motor, 0);
  638.             wb_motor_set_velocity(left_motor, 0);
  639.             return 0;
  640.         }
  641.     }
  642.     double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(flds), dsb = wb_distance_sensor_get_value(lbds);
  643.     if(dsl > 200) {
  644.         if(dsb < 200 && dsr > 200) {
  645.             return 1;
  646.         } else if(dsb < 200 && dsr < 200) {
  647.             giro_();
  648.             via2();
  649.         } else {
  650.             int j = 0;
  651.             while(wb_robot_step(TIME_STEP) != -1) {
  652.                 wb_motor_set_velocity(right_motor, 2);
  653.                 wb_motor_set_velocity(left_motor, 2);
  654.                 j++;
  655.                 if(j == 25) {
  656.                     wb_motor_set_velocity(right_motor, 0);
  657.                     wb_motor_set_velocity(left_motor, 0);
  658.                     break;
  659.                 }
  660.             }
  661.             _giro();
  662.             via2();
  663.         }
  664.     }
  665.     if(dsr < 200) {
  666.         giro_();
  667.         via2();
  668.     }
  669.     wb_motor_set_velocity(right_motor, 0);
  670.     wb_motor_set_velocity(left_motor, 0);
  671.     return 1;
  672. }
  673.  
  674. void vit1() { //verificado
  675.     double lbdsvalue = wb_distance_sensor_get_value(lbds);
  676.     if(lbdsvalue < 200) {
  677.         giro_();
  678.     }
  679.     while(wb_robot_step(TIME_STEP) != -1) {
  680.         wb_motor_set_velocity(right_motor, 8);
  681.         wb_motor_set_velocity(left_motor, 8);
  682.         rgb lc = getrgbs(left_camera), rc = getrgbs(right_camera);
  683.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  684.         //printf("avg %d %d %d\n", avg.r, avg.g, avg.b);
  685.         if(avg.r > 10000 && avg.g > 10000 && avg.b > 10000) {
  686.             wb_motor_set_velocity(right_motor, 0);
  687.             wb_motor_set_velocity(left_motor, 0);
  688.             break;
  689.         }
  690.     }
  691.     rgb mn = {0, 0, 0}, mx = {4000, 4000, 4000};
  692.     alinhar(mn, mx);
  693. }
  694.  
  695. void vit3(rgb cor_min, rgb cor_max) { //verificado
  696.     _giro();
  697.     d180 = 1;
  698.     while(wb_robot_step(TIME_STEP) != -1) {
  699.         wb_motor_set_velocity(right_motor, 4);
  700.         wb_motor_set_velocity(left_motor, 4);
  701.         rgb rrc = getrgbs(right_camera), llc = getrgbs(left_camera);
  702.         rgb avg = {(llc.r + rrc.r) / 2, (llc.g + rrc.g) / 2, (llc.b + rrc.b) / 2};
  703.         if(avg.r <= 9000 && avg.g <= 8000 && avg.b <= 7000) {
  704.             wb_motor_set_velocity(right_motor, 0);
  705.             wb_motor_set_velocity(left_motor, 0);
  706.             break;
  707.         }
  708.     }
  709.     while(wb_robot_step(TIME_STEP) != -1) {
  710.         wb_motor_set_velocity(right_motor, 4);
  711.         wb_motor_set_velocity(left_motor, 4);
  712.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  713.         //printf("right %d %d %d\n", rc.r, rc.g, rc.b);
  714.         //printf("left %d %d %d\n", lc.r, lc.g, lc.b);
  715.         //printf("deu 180? %d\n", d180);
  716.         if(!d180) {
  717.             if(lc.r >= cor_min.r && lc.r <= cor_max.r && lc.b >= cor_min.b && lc.b <= cor_max.b && lc.g >= cor_min.g && lc.g <= cor_max.g) {
  718.                 int j = 0;
  719.                 while(wb_robot_step(TIME_STEP) != -1) {
  720.                     wb_motor_set_velocity(right_motor, 2);
  721.                     wb_motor_set_velocity(left_motor, 2);
  722.                     j++;
  723.                     if(j == 15) {
  724.                         wb_motor_set_velocity(right_motor, 0);
  725.                         wb_motor_set_velocity(left_motor, 0);
  726.                         break;
  727.                     }
  728.                 }
  729.                 wb_motor_set_velocity(right_motor, 0);
  730.                 wb_motor_set_velocity(left_motor, 0);
  731.                 return;
  732.             }
  733.             if(lc.r <= 8500 && lc.g <= 7500 && lc.b <= 6500) {
  734.                 int j = 0;
  735.                 wb_motor_set_velocity(right_motor, -2);
  736.                 wb_motor_set_velocity(left_motor, -2);
  737.                 while(wb_robot_step(TIME_STEP) != -1) {
  738.                     j++;
  739.                     if(j == 100) {
  740.                         wb_motor_set_velocity(right_motor, 0);
  741.                         wb_motor_set_velocity(left_motor, 0);
  742.                         break;
  743.                     }
  744.                 }
  745.                 _giro_();
  746.                 j = 0;
  747.                 while(wb_robot_step(TIME_STEP) != -1) {
  748.                     wb_motor_set_velocity(right_motor, -2);
  749.                     wb_motor_set_velocity(left_motor, -2);
  750.                     j++;
  751.                     if(j == 62) {
  752.                         wb_motor_set_velocity(right_motor, 0);
  753.                         wb_motor_set_velocity(left_motor, 0);
  754.                         break;
  755.                     }
  756.                 }
  757.                 d180 = 1;
  758.             }
  759.         } else {
  760.             if(rc.r >= cor_min.r && rc.r <= cor_max.r && rc.b >= cor_min.b && rc.b <= cor_max.b && rc.g >= cor_min.g && rc.g <= cor_max.g) {
  761.                 int j = 0;
  762.                 while(wb_robot_step(TIME_STEP) != -1) {
  763.                     wb_motor_set_velocity(right_motor, 2);
  764.                     wb_motor_set_velocity(left_motor, 2);
  765.                     j++;
  766.                     if(j == 15) {
  767.                         wb_motor_set_velocity(right_motor, 0);
  768.                         wb_motor_set_velocity(left_motor, 0);
  769.                         break;
  770.                     }
  771.                 }
  772.                 wb_motor_set_velocity(right_motor, 0);
  773.                 wb_motor_set_velocity(left_motor, 0);
  774.                 return;
  775.             }
  776.             if(rc.r <= 8500 && rc.g <= 7500 && rc.b <= 6500) {
  777.                 int j = 0;
  778.                 wb_motor_set_velocity(right_motor, -2);
  779.                 wb_motor_set_velocity(left_motor, -2);
  780.                 while(wb_robot_step(TIME_STEP) != -1) {
  781.                     j++;
  782.                     if(j == 100) {
  783.                         wb_motor_set_velocity(right_motor, 0);
  784.                         wb_motor_set_velocity(left_motor, 0);
  785.                         break;
  786.                     }
  787.                 }
  788.                 _giro_();
  789.                 j = 0;
  790.                 while(wb_robot_step(TIME_STEP) != -1) {
  791.                     wb_motor_set_velocity(right_motor, -2);
  792.                     wb_motor_set_velocity(left_motor, -2);
  793.                     j++;
  794.                     if(j == 62) {
  795.                         wb_motor_set_velocity(right_motor, 0);
  796.                         wb_motor_set_velocity(left_motor, 0);
  797.                         break;
  798.                     }
  799.                 }
  800.                 d180 = 0;
  801.             }
  802.         }
  803.     }
  804. }
  805.  
  806. void vit3_1() {
  807.     if(d180) {
  808.         rgb rinit = getrgbs(right_camera);
  809.         while(wb_robot_step(TIME_STEP) != -1) {
  810.             wb_motor_set_velocity(right_motor, 2);
  811.             wb_motor_set_velocity(left_motor, 2);
  812.             rgb rc = getrgbs(right_camera);
  813.             if(abs(rc.r - rinit.r) >= 200 && abs(rc.g - rinit.g) >= 200 && abs(rc.b - rinit.b) >= 200) {
  814.                 wb_motor_set_velocity(right_motor, 0);
  815.                 wb_motor_set_velocity(left_motor, 0);
  816.                 int j = 0;
  817.                 while(wb_robot_step(TIME_STEP) != -1) {
  818.                     wb_motor_set_velocity(right_motor, -2);
  819.                     wb_motor_set_velocity(left_motor, -2);
  820.                     j++;
  821.                     if(j == 100) {
  822.                         wb_motor_set_velocity(right_motor, 0);
  823.                         wb_motor_set_velocity(left_motor, 0);
  824.                         break;
  825.                     }
  826.                 }
  827.                 _giro_();
  828.                 j = 0;
  829.                 while(wb_robot_step(TIME_STEP) != -1) {
  830.                     wb_motor_set_velocity(right_motor, -2);
  831.                     wb_motor_set_velocity(left_motor, -2);
  832.                     j++;
  833.                     if(j == 70) {
  834.                         wb_motor_set_velocity(right_motor, 0);
  835.                         wb_motor_set_velocity(left_motor, 0);
  836.                         break;
  837.                     }
  838.                 }
  839.                 break;
  840.             }
  841.         }
  842.     }
  843.     while(wb_robot_step(TIME_STEP) != -1) {
  844.         double lbdsvalue = wb_distance_sensor_get_value(lbds);
  845.         wb_motor_set_velocity(right_motor, 2);
  846.         wb_motor_set_velocity(left_motor, 2);
  847.         if (lbdsvalue < 300){
  848.             break;
  849.         }
  850.     }
  851.     int j = 0;
  852.     while(wb_robot_step(TIME_STEP) != -1) {
  853.         wb_motor_set_velocity(right_motor, 2);
  854.         wb_motor_set_velocity(left_motor, 2);
  855.         j++;
  856.         if((color_detected == 1 && j == 14) || (color_detected == 2 && j == 8) || (color_detected == 3 && j == 1)) {
  857.             wb_motor_set_velocity(right_motor, 0);
  858.             wb_motor_set_velocity(left_motor, 0);
  859.             break;
  860.         }
  861.     }
  862.     _giro();
  863. }
  864.  
  865. int main(int argc, char **argv) {
  866.     wb_robot_init();
  867.    
  868.     //initializing our tags
  869.     frds = wb_robot_get_device("FrontRightDistanceSensor");
  870.     flds = wb_robot_get_device("FrontLeftDistanceSensor");
  871.     ltds = wb_robot_get_device("LeftTopDistanceSensor");
  872.     lbds = wb_robot_get_device("LeftBottomDistanceSensor");
  873.     fmds = wb_robot_get_device("FrontMiddleDistanceSensor");
  874.     pos = wb_robot_get_device("EMPPositionSensor");
  875.     right_camera = wb_robot_get_device("RightCamera");
  876.     left_camera = wb_robot_get_device("LeftCamera");
  877.     width = wb_camera_get_width(right_camera);
  878.     height = wb_camera_get_height(right_camera);
  879.     left_motor = wb_robot_get_device("LeftWheel");
  880.     right_motor = wb_robot_get_device("RightWheel");
  881.     empilhadeira = wb_robot_get_device("EMPLinearMotor");
  882.     iu = wb_robot_get_device("inertial_unit");
  883.     left_pos = wb_robot_get_device("LWpossensor");
  884.     right_pos = wb_robot_get_device("RWpossensor");
  885.     wb_distance_sensor_enable(frds, TIME_STEP);
  886.     wb_distance_sensor_enable(flds, TIME_STEP);
  887.     wb_distance_sensor_enable(fmds, TIME_STEP);
  888.     wb_distance_sensor_enable(ltds, TIME_STEP);
  889.     wb_distance_sensor_enable(lbds, TIME_STEP);
  890.     wb_position_sensor_enable(pos, TIME_STEP);
  891.     wb_position_sensor_enable(left_pos, TIME_STEP);
  892.     wb_position_sensor_enable(right_pos, TIME_STEP);
  893.     wb_camera_enable(right_camera, TIME_STEP);
  894.     wb_camera_enable(left_camera, TIME_STEP);
  895.     wb_motor_set_position(left_motor, INFINITY);
  896.     wb_motor_set_position(right_motor, INFINITY);
  897.     wb_motor_set_position(empilhadeira, INFINITY);
  898.     wb_motor_set_velocity(left_motor, 0.0);
  899.     wb_motor_set_velocity(right_motor, 0.0);
  900.     wb_motor_set_velocity(empilhadeira, 0.0);  
  901.     while(true) {
  902.         printf("stt vi0\n");
  903.         vi0();
  904.         printf("ff vi0\n");
  905.         via1();
  906.         if(!via2()) {
  907.             break;
  908.         }
  909.         double length = odometria();
  910.         printf("achado %lf\n", length);
  911.         //length = 18;
  912.         rgb mn, mx;
  913.         if(length > 20) { //BLUE
  914.             rgb tmp1 = {0, 0, 10000}, tmp2 = {4000, 4000, 15000};
  915.             mn = tmp1;
  916.             mx = tmp2;
  917.             color_detected = 1;
  918.             printf("BLUE\n");
  919.         } else if(length > 15) { //RED
  920.             rgb tmp1 = {10000, 0, 0}, tmp2 = {15000, 4000, 4000};
  921.             mn = tmp1;
  922.             mx = tmp2;
  923.             color_detected = 2;
  924.             printf("RED\n");
  925.         } else { //YELLOW
  926.             rgb tmp1 = {10000, 10000, 0}, tmp2 = {15000, 15000, 4000};
  927.             mn = tmp1;
  928.             mx = tmp2;
  929.             color_detected = 3;
  930.             printf("YELLOW\n");
  931.         }
  932.         vit1();
  933.         vit3(mn, mx);
  934.         vit3_1();
  935.         pegar_tubo();
  936.         int j = 0;
  937.  
  938.         while(wb_robot_step(TIME_STEP) != -1) {
  939.  
  940.             rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  941.             wb_motor_set_velocity(right_motor, -2);
  942.             wb_motor_set_velocity(left_motor, -2);
  943.  
  944.             if((rc.r > 10000 && rc.r < 15000 && rc.g > 10000 && rc.g < 15000 && rc.b > 10000 && rc.b < 15000) || (lc.r > 10000 && lc.r < 15000 && lc.g > 10000 && lc.g < 15000 && lc.b > 10000 && lc.b < 15000)) {
  945.                 wb_motor_set_velocity(right_motor, 0);
  946.                 wb_motor_set_velocity(left_motor, 0);
  947.                 break;
  948.             }
  949.         }
  950.  
  951.         while(wb_robot_step(TIME_STEP) != -1) {
  952.             wb_motor_set_velocity(right_motor, -2);
  953.             wb_motor_set_velocity(left_motor, -2);
  954.             j++;
  955.             if(j == 15){
  956.                 wb_motor_set_velocity(right_motor, 0);
  957.                 wb_motor_set_velocity(left_motor, 0);
  958.                 break;
  959.             }
  960.         }
  961.         vi0_tubo();
  962.         printf("aodasij\n");
  963.         via1_tube();
  964.  
  965.         while(wb_robot_step(TIME_STEP) != -1) {
  966.             rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  967.             rgb md = {(rc.r + lc.r) / 2, (rc.g + lc.g) / 2, (rc.b + lc.b) / 2};
  968.             if(md.r < 9000 && md.g < 8000 && md.b < 7000) {
  969.                 wb_motor_set_velocity(right_motor, 0);
  970.                 wb_motor_set_velocity(left_motor, 0);
  971.                 break;
  972.             }
  973.             via2();
  974.             double length2 = odometria();
  975.             printf("%lf %lf\n", length, length2);
  976.             if(fabs(length - length2) < 3) {
  977.                 printf("aousdaoisjd\n");
  978.                 int j = 0;
  979.                 while(wb_robot_step(TIME_STEP) != -1) {
  980.                     wb_motor_set_velocity(right_motor, -2);
  981.                     wb_motor_set_velocity(left_motor, -2);
  982.                     j++;
  983.                     double ltdsvalue = wb_distance_sensor_get_value(ltds);
  984.                     if(ltdsvalue < 200 && j > 3) {
  985.                         wb_motor_set_velocity(right_motor, 0);
  986.                         wb_motor_set_velocity(left_motor, 0);
  987.                         break;
  988.                     }
  989.                 }
  990.                 j = 0;
  991.                 while(wb_robot_step(TIME_STEP) != -1) {
  992.                     wb_motor_set_velocity(right_motor, 2);
  993.                     wb_motor_set_velocity(left_motor, 2);
  994.                     j++;
  995.                     if((color_detected == 1 && j == 20) || (color_detected == 2 && j == 9) || (color_detected == 3 && j == 1)) {
  996.                         wb_motor_set_velocity(right_motor, 0);
  997.                         wb_motor_set_velocity(left_motor, 0);
  998.                         break;
  999.                     }
  1000.                 }
  1001.                 _giro();
  1002.                 entregar_tubo();
  1003.                 break;
  1004.             }
  1005.         }
  1006.         _giro_();
  1007.         j = 0;
  1008.         while(wb_robot_step(TIME_STEP) != -1) {
  1009.             wb_motor_set_velocity(right_motor, 6);
  1010.             wb_motor_set_velocity(left_motor, 6);
  1011.             j++;
  1012.             if(j == 200) {
  1013.                 wb_motor_set_velocity(right_motor, 0);
  1014.                 wb_motor_set_velocity(left_motor, 0);
  1015.                 break;
  1016.             }
  1017.         }
  1018.     }
  1019.     wb_robot_cleanup();
  1020.     return 0;
  1021. }
  1022.  
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!