Robofun 機器人論壇

標題: 紅外線測距器&程式判斷「問題」 [打印本頁]

作者: jame2105    時間: 2010-12-6 23:44
標題: 紅外線測距器&程式判斷「問題」
進入此版主會員、版主你們好~

本人我在思考與詢問以後,暫且得出
是程式判斷有問題,但是,問題就在這裡了!
程式判斷的部分究竟是哪裡有問題呢‥
這是讓我在當下持續思考的大疑問

在3PI開始啟動就一直持續
void turn_in_place()當中的set_motors(50, -50);
究竟是哪一部分的程式判斷是有問題要修改呢‥
以下附上自錄問題影片:
[youtube]http://www.youtube.com/watch?v=IZ3qloPPlSc[/youtube]
==================================================
原作者的網址:http://www.pololu.com/docs/0J26/all
展示影片:
[youtube]http://www.youtube.com/watch?v=Dx2Dkrl41YI&feature=player_embedded[/youtube]
==================================================
以下為原作者提供的程式碼,也是透過實際測試,其結果就是我自錄的影片
懇請相關專業人士協助我解決此問題,謝謝!
  1. /*
  2. * 3pi-wall-follower - demo code for the Pololu 3pi Robot
  3. *
  4. * If two Sharp distance sensors ( http://www.pololu.com/catalog/product/136 )
  5. * are installed, this code will allow a 3pi to explore its environment by
  6. * following objects it finds on its left side.
  7. *
  8. * http://www.pololu.com
  9. * http://forum.pololu.com
  10. *
  11. */

  12. // The 3pi include file must be at the beginning of any program that
  13. // uses the Pololu AVR library and 3pi.
  14. #include <pololu/3pi.h>

  15. // This include file allows data to be stored in program space.  The
  16. // ATmegaxx8 has 16x more flash than RAM, so large
  17. // pieces of static data should be stored in program space.
  18. #include <avr/pgmspace.h>

  19. // Introductory messages.  The "PROGMEM" identifier causes the data to
  20. // go into program space.
  21. const char welcome_line1[] PROGMEM = " Pololu";
  22. const char welcome_line2[] PROGMEM = "3\xf7 Robot";
  23. const char name_line1[] PROGMEM = "Wall";
  24. const char name_line2[] PROGMEM = "Follower";

  25. // A couple of simple tunes, stored in program space.
  26. const char welcome[] PROGMEM = ">g32>>c32";
  27. const char go[]      PROGMEM = "L16 cdegreg4";

  28. // Refresh the LCD display every tenth of a second.
  29. const int display_interval_ms = 100;

  30. #define MS_ELAPSED_IS(n) (get_ms() % n == 0)
  31. #define TIME_TO_DISPLAY (MS_ELAPSED_IS(display_interval_ms))

  32. void initialize()
  33. {
  34.         // Set PC5 as an input with internal pull-up disabled
  35.         DDRC  &= ~(1<< PORTC5);
  36.         PORTC &= ~(1<< PORTC5);

  37.         // Play welcome music and display a message
  38.         print_from_program_space(welcome_line1);
  39.         lcd_goto_xy(0,1);
  40.         print_from_program_space(welcome_line2);
  41.         play_from_program_space(welcome);
  42.         delay_ms(1000);

  43.         clear();
  44.         print_from_program_space(name_line1);
  45.         lcd_goto_xy(0,1);
  46.         print_from_program_space(name_line2);
  47.         delay_ms(1000);

  48.         // Display battery voltage and wait for button press
  49.         while(!button_is_pressed(BUTTON_B))
  50.         {
  51.                 clear();
  52.                 print_long(read_battery_millivolts());
  53.                 print("mV");
  54.                 lcd_goto_xy(0,1);
  55.                 print("Press B");
  56.                 delay_ms(100);
  57.         }

  58.         // Always wait for the button to be released so that 3pi doesn't
  59.         // start moving until your hand is away from it.
  60.         wait_for_button_release(BUTTON_B);
  61.         clear();
  62.         print("Go!");

  63.         // Play music and wait for it to finish before we start driving.
  64.         play_from_program_space(go);
  65.         while(is_playing());
  66. }

  67. void back_up()
  68. {
  69.         if (TIME_TO_DISPLAY)
  70.         {
  71.                 clear();
  72.                 lcd_goto_xy(0,0);
  73.                 print("Backing");
  74.                 lcd_goto_xy(0,1);
  75.                 print("Up");
  76.         }

  77.         // Back up slightly to the left
  78.         set_motors(-50,-90);
  79. }

  80. void turn_in_place() {
  81.         if (TIME_TO_DISPLAY) {
  82.                 clear();
  83.                 lcd_goto_xy(0,0);
  84.                 print("Front");
  85.                 lcd_goto_xy(0,1);
  86.                 print("Obstacle");
  87.         }

  88.         // Turn to the right in place
  89.         set_motors(50, -50);
  90. }

  91. int main()
  92. {
  93.         // set up the 3pi
  94.         initialize();

  95.         int last_proximity    = 0;
  96.         const int base_speed  = 200;
  97.         const int set_point   = 100;

  98.         // This is the "main loop" - it will run forever.
  99.         while(1)
  100.         {
  101.                 // In case it gets stuck: for 1 second every 15 seconds back up
  102.                 if (get_ms() % 15000 > 14000) {
  103.                         back_up();
  104.                         continue;
  105.                 }

  106.                 // If something is directly in front turn to the right in place
  107.                 int front_proximity = analog_read(5);
  108.                 if (front_proximity > 200) {
  109.                         turn_in_place();
  110.                         continue;
  111.                 }

  112.                 int proximity = analog_read(7); // 0 (far away) - 650 (close)
  113.                 int proportional = proximity - set_point;
  114.                 int derivative = proximity - last_proximity;

  115.                 // Proportional-Derivative Control Signal
  116.                 int pd = proportional / 3 + derivative * 20;

  117.                 int left_set  = base_speed + pd;
  118.                 int right_set = base_speed - pd;

  119.                 set_motors(left_set, right_set);

  120.                 if (TIME_TO_DISPLAY) {
  121.                         clear();
  122.                         lcd_goto_xy(0,0);
  123.                         print_long(proximity);

  124.                         lcd_goto_xy(5,0);
  125.                         print_long(pd);

  126.                         lcd_goto_xy(0,1);
  127.                         print_long(left_set);
  128.                         lcd_goto_xy(4,1);
  129.                         print_long(right_set);
  130.                 }

  131.                 last_proximity = proximity; // remember last proximity for derivative
  132.         }

  133.         // This part of the code is never reached.  A robot should
  134.         // never reach the end of its program, or unpredictable behavior
  135.         // will result as random code starts getting executed.  If you
  136.         // really want to stop all actions at some point, set your motors
  137.         // to 0,0 and run the following command to loop forever:
  138.         //
  139.         // while(1);
  140. }
複製代碼





歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/) Powered by Discuz! X3.2