API tools faq
paste
Advertisement
cosenza987

Untitled

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