如何在NXC语言中使用三眼乐高颜色传感器编写走黑线程序

xmlyszmlmf

谢谢，各位大大了。急！！！

糖伯虎

1. /* Techbricks.nl Line Follower
   
2.    Based on a PID controller, using the NXT 2.0 color sensor
   
3.    NXC firmware 1.28
   
4.    www.techbricks.nl
   
5.    last modified 03/03/2010                                    */
   
6. 
   
7. /* Proportional gain, straight forward reaction of the controller
   
8.    Larger values typically mean faster response since the larger the error,
   
9.    the larger the proportional term compensation.
   
10.    An excessively large proportional gain will lead to process instability and oscillation.*/
    
11. #define Kproportional 6
    
12. 
    
13. /* Integral gain, improves the controller accuracy
    
14.    Larger values imply steady state errors are eliminated more quickly.
    
15.    The trade-off is larger overshoot: any negative error integrated during transient response
    
16.    must be integrated away by positive error before reaching steady state.*/
    
17. #define Kintegral 0.0002
    
18. 
    
19. /* Derivative gain, improves the controller speed
    
20.    Larger values decrease overshoot, but slow down transient response
    
21.    and may lead to instability due to signal noise amplification in the differentiation of the error.*/
    
22. #define Kderivative 100
    
23. 
    
24. // Sample time, determined the reaction rate
    
25. #define dt 25
    
26. 
    
27. // NXT 2.0 Color sensor connected to port 2.
    
28. #define COLORSENSOR SENSOR_2
    
29. 
    
30. task main()
    
31. {
    
32. int error = 0;
    
33. float previous_error = 0;
    
34. float setpoint = 0;
    
35. float actual_position = 0;
    
36. float proportional = 0;
    
37. int integral = 0;
    
38. float derivative = 0;
    
39. float output = 0;
    
40. float left = 0;
    
41. float right = 0;
    
42. 
    
43. // Set the motor speed.
    
44. float speed=50;
    
45. 
    
46. // Set the color sensor light on.
    
47. SetSensorColorFull(IN_2);
    
48. 
    
49. // Read the value from the light sensor at the start position. The sensor must be place above the black line.
    
50. TextOut(1,LCD_LINE1,"Setpoint");
    
51. setpoint = COLORSENSOR;
    
52. NumOut(50,LCD_LINE1,setpoint);
    
53. 
    
54. // never ending loop.
    
55. while (true)
    
56.   {
    
57.    // Read the actual color sensor value.
    
58.    actual_position = COLORSENSOR;
    
59.    TextOut(1,LCD_LINE2,"Actual");
    
60.    NumOut(50,LCD_LINE2,actual_position);
    
61. 
    
62.    // Calculate the error, the differance between the setpoint and actual position.
    
63.    error = setpoint - actual_position;
    
64.    // Play a sound when the sensor is off the line
    
65.    if (error <> 0) PlayTone(TONE_B7, 1);
    
66. 
    
67.    // Proportional term makes a change to the output that is proportional to the current error value.
    
68.    proportional = Kproportional * error;
    
69. 
    
70.    // Integrate, sum of errors
    
71.    integral = integral + error;
    
72. 
    
73.    // Derivative, rate of change of the process error is calculated by determining the slope of the error over time.
    
74.    derivative = (error - previous_error) / dt;
    
75. 
    
76.    // Calculate sum of Proportional, Integral and Derivative.
    
77.    output = proportional + Kintegral * dt * integral + Kderivative * derivative;
    
78. 
    
79.    // save error value for period.
    
80.    previous_error = error;
    
81. 
    
82.    // Controll left motor
    
83.    left = speed - output;
    
84.    
    
85.    // Controll right motor
    
86.    right = speed + output;
    
87. 
    
88.    // Adjust the left and right motor value.
    
89.    if (left >   100) left  =  100;
    
90.    if (left <  -100) left  = -100;
    
91.    if (right >  100) right =  100;
    
92.    if (right < -100) right = -100;
    
93. 
    
94.    if (left < 0 )
    
95.      {
    
96.       OnFwd(OUT_A,-left);
    
97.       TextOut(1,LCD_LINE4,"Left  Rev");
    
98.       NumOut(55,LCD_LINE4,-left);
    
99.      }
    
100.    else
     
101.      {
     
102.       OnRev(OUT_A,left);
     
103.       TextOut(1,LCD_LINE4,"Left  Fwd");
     
104.       NumOut(55,LCD_LINE4,left);
     
105.      }
     
106. 
     
107.    if (right < 0 )
     
108.      {
     
109.        OnFwd(OUT_B,-right);
     
110.        TextOut(1,LCD_LINE5,"Right Rev");
     
111.        NumOut(55,LCD_LINE5,-right);
     
112.       }
     
113.     else
     
114.       {
     
115.        OnRev(OUT_B,right);
     
116.        TextOut(1,LCD_LINE5,"Right Fwd");
     
117.        NumOut(55,LCD_LINE5,right);
     
118.        }
     
119. 
     
120.    // Wait sample time.
     
121.    Wait(dt);
     
122.    }
     
123. }
     

复制代码

糖伯虎

http://bbs.cmnxt.com/forum.php?mod=viewthread&tid=4351

xmlyszmlmf

谢谢，几位大大！！