API tools faq
paste
Advertisement
cosenza987

Untitled

cosenza987
Oct 12th, 2021
1,006
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 18.75 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. #ifdef _WIN32
  14. #include <Windows.h>
  15. #else
  16. #include <unistd.h>
  17. #endif
  18.  
  19. #define TIME_STEP 32
  20.  
  21. int width, height, tamanhodorobo = 27;
  22. WbDeviceTag right_camera, left_camera, left_motor, right_motor, empilhadeira, frds, flds, ltds, lbds, pos, iu;
  23.  
  24. typedef struct _rgb {
  25.     int r, g, b;
  26. } rgb;
  27.  
  28. rgb verde_min = {1000, 12000, 1000}, verde_max = {10000, 14000, 10000};
  29.  
  30. rgb getrgbs(WbDeviceTag camera) {
  31.     const unsigned char *image = wb_camera_get_image(camera);
  32.     rgb ans = {0, 0, 0};
  33.     for(int i = 0; i < width; i++) {
  34.         for(int j = 0; j < height; j++) {
  35.             ans.r += wb_camera_image_get_red(image, width, i, j);
  36.             ans.g += wb_camera_image_get_green(image, width, i, j);
  37.             ans.b += wb_camera_image_get_blue(image, width, i, j);
  38.         }
  39.     }
  40.     return ans;
  41. }
  42.  
  43. void pegar_tubo(){
  44.  
  45.     double max_pos = wb_motor_get_max_position(empilhadeira);
  46.     double min_pos = wb_motor_get_min_position(empilhadeira);
  47.  
  48.     while(wb_robot_step(TIME_STEP) != -1) {
  49.         wb_motor_set_velocity(left_motor, 2);
  50.         wb_motor_set_velocity(right_motor, 2);
  51.    
  52.         double frdsvalue = wb_distance_sensor_get_value(frds);
  53.         double fldsvalue = wb_distance_sensor_get_value(flds);
  54.         /*
  55.         if(frdsvalue > fldsvalue) {
  56.             double teta = atan((frdsvalue - fldsvalue)/8.8);
  57.  
  58.             //gira o ângulo
  59.         }*/
  60.        
  61.         if(frdsvalue < 145 && fldsvalue < 145) {
  62.             wb_motor_set_velocity(left_motor, 0);
  63.             wb_motor_set_velocity(right_motor, 0);
  64.  
  65.             while(wb_position_sensor_get_value(pos) > min_pos) {
  66.                 wb_motor_set_velocity(empilhadeira, -2);
  67.             }
  68.         }    
  69.         wb_motor_set_velocity(left_motor, 2);
  70.         wb_motor_set_velocity(right_motor, 2);
  71.        
  72.         if(frdsvalue < 50 && fldsvalue < 50){
  73.           wb_motor_set_velocity(left_motor, 0);
  74.           wb_motor_set_velocity(right_motor, 0);
  75.  
  76.           while(wb_position_sensor_get_value(pos) < max_pos) {
  77.               wb_motor_set_velocity(empilhadeira, 2);
  78.           }
  79.           return;
  80.         }
  81.     }
  82. }
  83.  
  84.  
  85. void entregar_tubo(){
  86.     while(wb_robot_step(TIME_STEP) != -1) {
  87.  
  88.         double frdsvalue = wb_distance_sensor_get_value(frds);
  89.         double fldsvalue = wb_distance_sensor_get_value(flds);
  90.  
  91.         while(wb_position_sensor_get_value(pos) > 0) {
  92.             wb_motor_set_velocity(empilhadeira, -2);
  93.         }
  94.        
  95.         wb_motor_set_velocity(left_motor, -2);
  96.         wb_motor_set_velocity(right_motor, -2);
  97.  
  98.         if(frdsvalue > 60 && fldsvalue > 60){
  99.             wb_motor_set_velocity(left_motor, 0);
  100.             wb_motor_set_velocity(right_motor, 0);
  101.  
  102.             while(wb_position_sensor_get_value(pos) < 0.3) {
  103.                 wb_motor_set_velocity(empilhadeira, 2);
  104.             }
  105.             return;
  106.         }
  107.     }
  108. }
  109.  
  110. double odometria() { //verificado
  111.     double tube;
  112.     clock_t start = clock();
  113.  
  114.     while(wb_robot_step(TIME_STEP) != -1) {
  115.         double ltdsvalue = wb_distance_sensor_get_value(ltds);
  116.        
  117.         wb_motor_set_velocity(left_motor, 2);
  118.         wb_motor_set_velocity(right_motor, 2);
  119.  
  120.         if(ltdsvalue < 200) {
  121.             wb_motor_set_velocity(left_motor, 0);  
  122.             wb_motor_set_velocity(right_motor, 0);
  123.  
  124.             break;
  125.         }
  126.  
  127.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  128.  
  129.         if(rc.b < 7000 && rc.r < 9000 && rc.g < 8000) {
  130.             wb_motor_set_velocity(left_motor, 0);  
  131.             wb_motor_set_velocity(right_motor, 0);
  132.  
  133.             clock_t end = clock();
  134.             tube = 2.0*(end - start) + 27;
  135.  
  136.             return tube;
  137.         }
  138.  
  139.     }
  140.  
  141.     clock_t end = clock();
  142.  
  143.     tube = 2.0*(end - start)*3.5/1000;
  144.  
  145.     return tube;
  146. }
  147.  
  148. void alinhar_chao() {//verificado
  149.     while(wb_robot_step(TIME_STEP) != -1) {
  150.         wb_motor_set_velocity(right_motor, 2);
  151.         wb_motor_set_velocity(left_motor, 2);
  152.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  153.         int left_sensor = 0, right_sensor = 0;
  154.         if(rc.b < 7000 && rc.r < 9000 && rc.g < 8000) {
  155.             right_sensor = 1;
  156.         }
  157.         if(lc.b < 7000 && lc.r < 9000 && lc.g < 8000) {
  158.             left_sensor = 1;
  159.         }
  160.         if(right_sensor && !left_sensor) {
  161.             wb_motor_set_velocity(right_motor, 0);
  162.             wb_motor_set_velocity(left_motor, 0.4);
  163.             while(wb_robot_step(TIME_STEP) != -1) {
  164.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  165.                 if(abs(rc.b - lc.b) < 65 && abs(rc.g - lc.g) < 65 && abs(rc.r - lc.r) < 65) {
  166.                     wb_motor_set_velocity(right_motor, -3);
  167.                     wb_motor_set_velocity(left_motor, -3);
  168.                     int j = 0;
  169.                     while(wb_robot_step(TIME_STEP) != -1) {
  170.                         j++;
  171.                         if(j == 50) {
  172.                             break;
  173.                         }
  174.                     }
  175.                     return;
  176.                 }
  177.             }
  178.         }
  179.         if(!right_sensor && left_sensor) {
  180.             wb_motor_set_velocity(right_motor, 0.4);
  181.             wb_motor_set_velocity(left_motor, 0);
  182.             while(wb_robot_step(TIME_STEP) != -1) {
  183.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  184.                 if(abs(rc.b - lc.b) < 65 && abs(rc.g - lc.g) < 65 && abs(rc.r - lc.r) < 65) {
  185.                     wb_motor_set_velocity(right_motor, -3);
  186.                     wb_motor_set_velocity(left_motor, -3);
  187.                     int j = 0;
  188.                     while(wb_robot_step(TIME_STEP) != -1) {
  189.                         j++;
  190.                         if(j == 50) {
  191.                             break;
  192.                         }
  193.                     }
  194.                     return;
  195.                 }
  196.             }
  197.         }
  198.         if(right_sensor && left_sensor) {
  199.             wb_motor_set_velocity(right_motor, -3);
  200.             wb_motor_set_velocity(left_motor, -3);
  201.             int j = 0;
  202.             while(wb_robot_step(TIME_STEP) != -1) {
  203.                 j++;
  204.                 if(j == 50) {
  205.                     break;
  206.                 }
  207.             }
  208.             return;
  209.         }
  210.     }
  211. }
  212.  
  213. void alinhar(rgb min, rgb max) {
  214.     while(wb_robot_step(TIME_STEP) != -1) {
  215.         wb_motor_set_velocity(right_motor, 2);
  216.         wb_motor_set_velocity(left_motor, 2);
  217.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  218.         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) {
  219.             while(wb_robot_step(TIME_STEP) != -1) {
  220.                 wb_motor_set_velocity(left_motor, 2);
  221.                 wb_motor_set_velocity(right_motor, 0);
  222.                 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) {
  223.                     return;
  224.                 }
  225.             }
  226.         }
  227.         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) {
  228.             while(wb_robot_step(TIME_STEP) != -1) {
  229.                 wb_motor_set_velocity(left_motor, 0);
  230.                 wb_motor_set_velocity(right_motor, 2);
  231.                 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) {
  232.                     return;
  233.                 }
  234.             }
  235.         }
  236.     }
  237. }
  238.  
  239. void desviar() {
  240.     rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  241.     if(rc.g < 5800 && rc.b < 5800 && rc.r < 5800) {
  242.         while(wb_robot_step(TIME_STEP) != -1) {
  243.             wb_motor_set_velocity(right_motor, -2);
  244.             wb_motor_set_velocity(left_motor, 0);
  245.             if(rc.g > 5800 && rc.b > 5800 && rc.r > 5800) {
  246.                 return;
  247.             }
  248.         }
  249.     }
  250.     if(lc.g < 5800 && lc.b < 5800 && lc.r < 5800) {
  251.         while(wb_robot_step(TIME_STEP) != -1) {
  252.             wb_motor_set_velocity(right_motor, 0);
  253.             wb_motor_set_velocity(left_motor, -2);
  254.             if(lc.g > 5800 && lc.b > 5800 && lc.r > 5800) {
  255.                 return;
  256.             }
  257.         }
  258.     }
  259.     return;
  260. }
  261.  
  262. void giro_() { //verificado
  263.     wb_motor_set_velocity(right_motor, -0.5);
  264.     wb_motor_set_velocity(left_motor, 0.5);
  265.     wb_inertial_unit_enable(iu, TIME_STEP);
  266.     const double *init = wb_inertial_unit_get_roll_pitch_yaw(iu);
  267.     double k = init[2], prev = k;
  268.     while(wb_robot_step(TIME_STEP) != -1) {
  269.         const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  270.         double cur = val[2];
  271.         if(cur > 0) {
  272.             if(prev < 0 && fabs(k - prev) > 1) {
  273.                 break;
  274.             } else if(prev > 0) {
  275.                 k *= -1.0;
  276.             }
  277.         } else {
  278.             if(prev > 0 && fabs(k - prev) > 1) {
  279.                 break;
  280.             } else if(prev > 0) {
  281.                 k *= -1.0;
  282.             }
  283.         }
  284.         if(fabs(val[2] - k) >= 1.56 && fabs(val[2] - k) <= 1.58) {
  285.             break;
  286.         }
  287.         prev = cur;
  288.     }
  289.     wb_inertial_unit_disable(iu);
  290.     return;
  291. }
  292.  
  293. void _giro() { //verificado
  294.     wb_motor_set_velocity(right_motor, 0.5);
  295.     wb_motor_set_velocity(left_motor, -0.5);
  296.     wb_inertial_unit_enable(iu, TIME_STEP);
  297.     const double *init = wb_inertial_unit_get_roll_pitch_yaw(iu);
  298.     double k = init[2], prev = k;
  299.     while(wb_robot_step(TIME_STEP) != -1) {
  300.         const double *val = wb_inertial_unit_get_roll_pitch_yaw(iu);
  301.         double cur = val[2];
  302.         if(cur > 0) {
  303.             if(prev < 0 && fabs(k - prev) > 1) {
  304.                 break;
  305.             } else if(prev > 0) {
  306.                 k *= -1.0;
  307.             }
  308.         } else {
  309.             if(prev > 0 && fabs(k - prev) > 1) {
  310.                 break;
  311.             } else if(prev > 0) {
  312.                 k *= -1.0;
  313.             }
  314.         }
  315.         if(fabs(val[2] - k) >= 1.56 && fabs(val[2] - k) <= 1.58) {
  316.             break;
  317.         }
  318.         prev = cur;
  319.     }
  320.     wb_inertial_unit_disable(iu);
  321.     return;
  322. }
  323.  
  324. void _giro_() { //verificado
  325.     _giro();
  326.     _giro();
  327.     return;
  328. }
  329.  
  330. void vi0() { //verificado
  331.     while(wb_robot_step(TIME_STEP) != -1) {
  332.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  333.         printf("l %d %d %d\n", lc.r, lc.g, lc.b);
  334.         printf("r %d %d %d\n", rc.r, rc.g, rc.b);
  335.         if((rc.r < 7200 && rc.g < 7200 && rc.b < 7200) || (lc.r < 7200 && lc.g < 7200 && lc.b < 7200)) {
  336.             alinhar_chao();
  337.             giro_();
  338.             vi0();
  339.             return;
  340.         }
  341.         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) {
  342.             _giro();
  343.             vi0();
  344.             return;
  345.         }
  346.         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) {
  347.             _giro_();
  348.             vi0();
  349.             return;
  350.         }
  351.         if((rc.r < 9000 && rc.g < 8000 && rc.b < 7000) || (lc.r < 9000 && lc.g < 8000 && lc.b < 7000)) {
  352.             break;
  353.         }
  354.         wb_motor_set_velocity(right_motor, 2);
  355.         wb_motor_set_velocity(left_motor, 2);
  356.     }
  357.     alinhar_chao();
  358. }
  359.  
  360. void via1() { //verificado
  361.     wb_motor_set_velocity(right_motor, -2);
  362.     wb_motor_set_velocity(left_motor, -2);
  363.     giro_();
  364.     while(wb_robot_step(TIME_STEP) != -1) {
  365.         wb_motor_set_velocity(right_motor, 2);
  366.         wb_motor_set_velocity(left_motor, 2);
  367.         double fr = wb_distance_sensor_get_value(frds), fl = wb_distance_sensor_get_value(flds);
  368.         double avg = (fr + fl) / 2;
  369.         if(avg < 150) {
  370.             break;
  371.         }
  372.         rgb lc = getrgbs(left_camera);
  373.         if(lc.r < 3200 && lc.g < 3200 && lc.b < 3200) {
  374.             wb_motor_set_velocity(right_motor, 1);
  375.             int j = 0;
  376.             for(int i = 0; i < 20000000; i++) {
  377.                 j++;
  378.             }
  379.             wb_motor_set_velocity(right_motor, 2);
  380.         }
  381.     }
  382.     giro_();
  383. }
  384.  
  385. void via2() {
  386.     while(wb_robot_step(TIME_STEP) != -1) {
  387.         wb_motor_set_velocity(right_motor, 2);
  388.         wb_motor_set_velocity(left_motor, 2);
  389.         double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(frds);
  390.         if(dsl > 200 || dsr < 200) {
  391.             wb_motor_set_velocity(right_motor, 0);
  392.             wb_motor_set_velocity(left_motor, 0);
  393.             break;
  394.         }
  395.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  396.         rgb md = {(rc.r + lc.r) / 2, (rc.g + lc.g) / 2, (rc.b + lc.b) / 2};
  397.         if(md.r < 5800 && md.g < 5800 && md.b < 5800) {
  398.             return;
  399.         }
  400.     }
  401.     double dsl = wb_distance_sensor_get_value(ltds), dsr = wb_distance_sensor_get_value(frds), dsb = wb_distance_sensor_get_value(lbds);
  402.     if(dsl > 200) {
  403.         if(dsb < 200 && dsr > 200) {
  404.             return;
  405.         } else if(dsb < 200 & dsr < 200) {
  406.             giro_();
  407.             via2();
  408.         } else {
  409.             wb_motor_set_velocity(right_motor, 2);
  410.             wb_motor_set_velocity(left_motor, 2);
  411.             int j = 0;
  412.             for(int i = 0; i < 20000000; i++) {
  413.                 j++;
  414.             }
  415.             _giro();
  416.             via2();
  417.         }
  418.     }
  419.     if(dsr < 200) {
  420.         giro_();
  421.         via2();
  422.     }
  423. }
  424.  
  425. void vit1() {
  426.     giro_();
  427.     while(wb_robot_step(TIME_STEP) != -1) {
  428.         wb_motor_set_velocity(right_motor, 2);
  429.         wb_motor_set_velocity(left_motor, 2);
  430.         rgb lc = getrgbs(left_camera), rc = getrgbs(right_camera);
  431.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  432.         if(avg.r < 5800 && avg.g < 5800 && avg.b < 5800) {
  433.             break;
  434.         } else {
  435.             desviar();
  436.         }
  437.     }
  438.     rgb mn = {0, 0, 0}, mx = {1700, 1700, 1700};
  439.     alinhar(mn, mx);
  440. }
  441.  
  442. void vit3(rgb cor_min, rgb cor_max) {
  443.     wb_motor_set_velocity(right_motor, 2);
  444.     wb_motor_set_velocity(left_motor, 2);
  445.     int j = 0;
  446.     for(int i = 0; i < 20000000; i++) {
  447.         j++;
  448.     }
  449.     wb_motor_set_velocity(right_motor, 0);
  450.     wb_motor_set_velocity(left_motor, 0);
  451.     while(wb_robot_step(TIME_STEP) != -1) {
  452.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  453.         rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  454.         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) {
  455.             giro_();
  456.             return;
  457.         } else {
  458.             _giro();
  459.             int deu180 = 0;
  460.             while(wb_robot_step(TIME_STEP) != -1) {
  461.                 rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  462.                 rgb aavg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  463.                 wb_motor_set_velocity(right_motor, 2);
  464.                 wb_motor_set_velocity(left_motor, 2);
  465.                 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) {
  466.                     if(!deu180) {
  467.                         _giro_();
  468.                     }
  469.                     return;
  470.                 }
  471.                 if(aavg.r <= 5800 && aavg.b <= 5800 && aavg.g <= 5800) {
  472.                     _giro_();
  473.                     deu180 = 1;
  474.                 }
  475.             }
  476.         }
  477.     }
  478. }
  479.  
  480. void vit3_1(rgb cor_min, rgb cor_max){
  481.     while(wb_robot_step(TIME_STEP) != -1) {
  482.  
  483.         double ltdsvalue = wb_distance_sensor_get_value(ltds);
  484.         double lbdsvalue = wb_distance_sensor_get_value(lbds);
  485.  
  486.         if (ltdsvalue > 40 && lbdsvalue > 40){
  487.             rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  488.             rgb avg = {(lc.r + rc.r) / 2, (lc.g + rc.g) / 2, (lc.b + rc.b) / 2};
  489.             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){
  490.                 wb_motor_set_velocity(left_motor, 2);
  491.                 wb_motor_set_velocity(right_motor, 2);
  492.             }
  493.             else{
  494.                 _giro_();
  495.             }
  496.         }
  497.         _giro();
  498.  
  499.         double frdsvalue = wb_distance_sensor_get_value(frds);
  500.         double fldsvalue = wb_distance_sensor_get_value(flds);
  501.  
  502.         while (frds > 100 || flds > 100){
  503.             double frdsvalue = wb_distance_sensor_get_value(frds);
  504.             double fldsvalue = wb_distance_sensor_get_value(flds);
  505.             wb_motor_set_velocity(left_motor, 0.08*(flds - 100));
  506.             wb_motor_set_velocity(right_motor, 0.08*(frds - 100));
  507.         }
  508.         wb_motor_set_velocity(left_motor, 0);
  509.         wb_motor_set_velocity(right_motor, 0);
  510.     }
  511. }
  512.  
  513.  
  514.  
  515. int main(int argc, char **argv) {
  516.     //creating our tags and variables
  517.     int pause_counter = 0;
  518.     int left_speed, right_speed;
  519.     wb_robot_init();
  520.    
  521.     //initializing our tags
  522.     frds = wb_robot_get_device("FrontRightDistanceSensor");
  523.     flds = wb_robot_get_device("FrontLeftDistanceSensor");
  524.     ltds = wb_robot_get_device("LeftTopDistanceSensor");
  525.     lbds = wb_robot_get_device("LeftBottomDistanceSensor");
  526.     pos = wb_robot_get_device("EMPPositionSensor");
  527.     right_camera = wb_robot_get_device("RightCamera");
  528.     left_camera = wb_robot_get_device("LeftCamera");
  529.     width = wb_camera_get_width(right_camera);
  530.     height = wb_camera_get_height(right_camera);
  531.     left_motor = wb_robot_get_device("LeftWheel");
  532.     right_motor = wb_robot_get_device("RightWheel");
  533.     empilhadeira = wb_robot_get_device("EMPLinearMotor");
  534.     iu = wb_robot_get_device("inertial_unit");
  535.     wb_distance_sensor_enable(frds, TIME_STEP);
  536.     wb_distance_sensor_enable(flds, TIME_STEP);
  537.     wb_distance_sensor_enable(ltds, TIME_STEP);
  538.     wb_distance_sensor_enable(lbds, TIME_STEP);
  539.     wb_position_sensor_enable(pos, TIME_STEP);
  540.     wb_camera_enable(right_camera, TIME_STEP);
  541.     wb_camera_enable(left_camera, TIME_STEP);
  542.     wb_motor_set_position(left_motor, INFINITY);
  543.     wb_motor_set_position(right_motor, INFINITY);
  544.     wb_motor_set_velocity(left_motor, 0.0);
  545.     wb_motor_set_velocity(right_motor, 0.0);
  546.  
  547.     vi0();
  548.     via1();
  549.     via2();
  550.     double l = odometria();
  551.     printf("%lf\n", l);
  552.     /*while (wb_robot_step(TIME_STEP) != -1) {
  553.         rgb rc = getrgbs(right_camera), lc = getrgbs(left_camera);
  554.         printf("Left values: %d %d %d\n", lc.r, lc.g, lc.b);
  555.         printf("Right values: %d %d %d\n", rc.r, rc.g, rc.b);
  556.     } */
  557.  
  558.     wb_robot_cleanup();
  559.     return 0;
  560. }
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!