紅外線測距器&程式判斷「問題」

jame2105

進入此版主會員、版主你們好~

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

在3PI開始啟動就一直持續
void turn_in_place()當中的set_motors(50, -50);
究竟是哪一部分的程式判斷是有問題要修改呢‥
以下附上自錄問題影片：

==================================================
原作者的網址：http://www.pololu.com/docs/0J26/all
展示影片：

==================================================
以下為原作者提供的程式碼，也是透過實際測試，其結果就是我自錄的影片
懇請相關專業人士協助我解決此問題，謝謝!

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. 
    
13. // The 3pi include file must be at the beginning of any program that
    
14. // uses the Pololu AVR library and 3pi.
    
15. #include <pololu/3pi.h>
    
16. 
    
17. // This include file allows data to be stored in program space.  The
    
18. // ATmegaxx8 has 16x more flash than RAM, so large
    
19. // pieces of static data should be stored in program space.
    
20. #include <avr/pgmspace.h>
    
21. 
    
22. // Introductory messages.  The "PROGMEM" identifier causes the data to
    
23. // go into program space.
    
24. const char welcome_line1[] PROGMEM = " Pololu";
    
25. const char welcome_line2[] PROGMEM = "3\xf7 Robot";
    
26. const char name_line1[] PROGMEM = "Wall";
    
27. const char name_line2[] PROGMEM = "Follower";
    
28. 
    
29. // A couple of simple tunes, stored in program space.
    
30. const char welcome[] PROGMEM = ">g32>>c32";
    
31. const char go[]      PROGMEM = "L16 cdegreg4";
    
32. 
    
33. // Refresh the LCD display every tenth of a second.
    
34. const int display_interval_ms = 100;
    
35. 
    
36. #define MS_ELAPSED_IS(n) (get_ms() % n == 0)
    
37. #define TIME_TO_DISPLAY (MS_ELAPSED_IS(display_interval_ms))
    
38. 
    
39. void initialize()
    
40. {
    
41.         // Set PC5 as an input with internal pull-up disabled
    
42.         DDRC  &= ~(1<< PORTC5);
    
43.         PORTC &= ~(1<< PORTC5);
    
44. 
    
45.         // Play welcome music and display a message
    
46.         print_from_program_space(welcome_line1);
    
47.         lcd_goto_xy(0,1);
    
48.         print_from_program_space(welcome_line2);
    
49.         play_from_program_space(welcome);
    
50.         delay_ms(1000);
    
51. 
    
52.         clear();
    
53.         print_from_program_space(name_line1);
    
54.         lcd_goto_xy(0,1);
    
55.         print_from_program_space(name_line2);
    
56.         delay_ms(1000);
    
57. 
    
58.         // Display battery voltage and wait for button press
    
59.         while(!button_is_pressed(BUTTON_B))
    
60.         {
    
61.                 clear();
    
62.                 print_long(read_battery_millivolts());
    
63.                 print("mV");
    
64.                 lcd_goto_xy(0,1);
    
65.                 print("Press B");
    
66.                 delay_ms(100);
    
67.         }
    
68. 
    
69.         // Always wait for the button to be released so that 3pi doesn't
    
70.         // start moving until your hand is away from it.
    
71.         wait_for_button_release(BUTTON_B);
    
72.         clear();
    
73.         print("Go!");
    
74. 
    
75.         // Play music and wait for it to finish before we start driving.
    
76.         play_from_program_space(go);
    
77.         while(is_playing());
    
78. }
    
79. 
    
80. void back_up()
    
81. {
    
82.         if (TIME_TO_DISPLAY)
    
83.         {
    
84.                 clear();
    
85.                 lcd_goto_xy(0,0);
    
86.                 print("Backing");
    
87.                 lcd_goto_xy(0,1);
    
88.                 print("Up");
    
89.         }
    
90. 
    
91.         // Back up slightly to the left
    
92.         set_motors(-50,-90);
    
93. }
    
94. 
    
95. void turn_in_place() {
    
96.         if (TIME_TO_DISPLAY) {
    
97.                 clear();
    
98.                 lcd_goto_xy(0,0);
    
99.                 print("Front");
    
100.                 lcd_goto_xy(0,1);
     
101.                 print("Obstacle");
     
102.         }
     
103. 
     
104.         // Turn to the right in place
     
105.         set_motors(50, -50);
     
106. }
     
107. 
     
108. int main()
     
109. {
     
110.         // set up the 3pi
     
111.         initialize();
     
112. 
     
113.         int last_proximity    = 0;
     
114.         const int base_speed  = 200;
     
115.         const int set_point   = 100;
     
116. 
     
117.         // This is the "main loop" - it will run forever.
     
118.         while(1)
     
119.         {
     
120.                 // In case it gets stuck: for 1 second every 15 seconds back up
     
121.                 if (get_ms() % 15000 > 14000) {
     
122.                         back_up();
     
123.                         continue;
     
124.                 }
     
125. 
     
126.                 // If something is directly in front turn to the right in place
     
127.                 int front_proximity = analog_read(5);
     
128.                 if (front_proximity > 200) {
     
129.                         turn_in_place();
     
130.                         continue;
     
131.                 }
     
132. 
     
133.                 int proximity = analog_read(7); // 0 (far away) - 650 (close)
     
134.                 int proportional = proximity - set_point;
     
135.                 int derivative = proximity - last_proximity;
     
136. 
     
137.                 // Proportional-Derivative Control Signal
     
138.                 int pd = proportional / 3 + derivative * 20;
     
139. 
     
140.                 int left_set  = base_speed + pd;
     
141.                 int right_set = base_speed - pd;
     
142. 
     
143.                 set_motors(left_set, right_set);
     
144. 
     
145.                 if (TIME_TO_DISPLAY) {
     
146.                         clear();
     
147.                         lcd_goto_xy(0,0);
     
148.                         print_long(proximity);
     
149. 
     
150.                         lcd_goto_xy(5,0);
     
151.                         print_long(pd);
     
152. 
     
153.                         lcd_goto_xy(0,1);
     
154.                         print_long(left_set);
     
155.                         lcd_goto_xy(4,1);
     
156.                         print_long(right_set);
     
157.                 }
     
158. 
     
159.                 last_proximity = proximity; // remember last proximity for derivative
     
160.         }
     
161. 
     
162.         // This part of the code is never reached.  A robot should
     
163.         // never reach the end of its program, or unpredictable behavior
     
164.         // will result as random code starts getting executed.  If you
     
165.         // really want to stop all actions at some point, set your motors
     
166.         // to 0,0 and run the following command to loop forever:
     
167.         //
     
168.         // while(1);
     
169. }

複製代碼