请教一下robotC小车寻线

夜冥

小弟初学robotc 想做一个寻线加避障的小车，可是写出来的代码有问题

1.   #pragma config(Sensor, S4,     sonarSensor,         sensorSONAR)
   
2. #pragma config(Sensor, S1,lightSensor,sensorLightActive)
   
3. #pragma config(Sensor, S2,lightSensor,sensorLightActive)
   
4. #pragma platform(NXT)          //智能在NXT中使用
   
5. 
   
6. 
   
7. #define Kproportional 2         // Kp, 比例因子
   
8. #define Kintegral 0.001          // Ki，积分因子
   
9. #define Kderivative 10            // Kd，微分因子
   
10. #define dt 1                   // 取样时间
    
11. task main()
    
12. {
    
13.         for(;;){
    
14.    do
    
15.     {
    
16.       motor[motorA]=100;
    
17.         motor[motorC]=100;
    
18.         
    
19.   }
    
20.     while(SensorValue(sonarSensor)>8);
    
21.     motor[motorB]=0;
    
22.     motor[motorC]=0;
    
23.     wait1Msec(1000);
    
24.    
    
25.     nMotorEncoderTarget[motorA] = 150;
    
26. motor[motorA] = -20;
    
27. wait1Msec(800);
    
28. motor[motorA]=0;
    
29. wait1Msec(1000);
    
30. nMotorEncoder[motorB] = 0;               
    
31. nMotorEncoder[motorC] = 0;  
    
32. // reset the Motor Encoder of Motor C
    
33. nMotorEncoderTarget[motorB] = 300;        // set the  target for Motor Encoder of Motor B to 360
    
34. nMotorEncoderTarget[motorC] = -300;        // set the  target for Motor Encoder of Motor C to 360
    
35. motor[motorB] = 70;                       // motor B is run at a power level of 75
    
36. motor[motorC] = -70;  
    
37. // motor C is run at a power level of 75
    
38.        
    
39. while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle)
    
40. wait1Msec(1000);
    
41. {
    
42.        
    
43. motor[motorA]=25;
    
44. wait1Msec(800);
    
45. motor[motorB] = -50;                       // motor B is run at a power level of 75
    
46. motor[motorC] = 50;
    
47. wait1Msec(700);
    
48. }
    
49. }
    
50. }

复制代码

这个代码能很好的执行，小车检测到前面的障碍物就把它放到旁边
但是下面这个代码加上寻线之后

1. #pragma config(Sensor, S4,     sonarSensor,         sensorSONAR)
   
2. #pragma config(Sensor, S1,lightSensor,sensorLightActive)
   
3. #pragma config(Sensor, S2,lightSensor,sensorLightActive)
   
4. #pragma platform(NXT)          //智能在NXT中使用
   
5. 
   
6. 
   
7. #define Kproportional 2         // Kp, 比例因子
   
8. #define Kintegral 0.001          // Ki，积分因子
   
9. #define Kderivative 10            // Kd，微分因子
   
10. #define dt 1                   // 取样时间
    
11. task main()
    
12. {
    
13.         for(;;){
    
14.    do
    
15.     {
    
16.     int error = 0;
    
17.     float previous_error = 0;
    
18.     float setpoint = 0;
    
19.     float actual_position = 0;
    
20.     float proportional = 0;
    
21.     int integral = 0;
    
22.     float derivative = 0;
    
23.     float output = 0;
    
24.     float left = 0;
    
25.     float right = 0;
    
26. 
    
27.     // 设定马达初始速度
    
28.     float speed=35;
    
29. 
    
30.     // 开传感器
    
31.     //SetSensorColorFull(IN_3);
    
32. 
    
33.     // 设定初始值。
    
34.     setpoint=50;
    
35.     nxtDisplayTextLine(1,"setpoint= %d",setpoint);
    
36.     wait1Msec(20);
    
37. 
    
38.     // 循环
    
39.     while (true)
    
40.     {
    
41.         // 读取传感器当前值
    
42.         actual_position = SensorValue[lightSensor];
    
43.         nxtDisplayTextLine(3,"ac_pos= %d",actual_position);
    
44. 
    
45.         // 计算误差
    
46.         error = setpoint - actual_position;
    
47.         // 偏离航线... 发出声音示警
    
48.         //if (error <> 0) PlayTone(TONE_B7, 1);
    
49. 
    
50.         // 比例部分： 当前误差 x 比例因子
    
51.         proportional = Kproportional * error;
    
52. 
    
53.         // 积分： 误差累积
    
54.         integral = integral + error;
    
55. 
    
56.         // 微分： 误差变化率
    
57.         derivative = (error - previous_error) / dt;
    
58. 
    
59.         // 比例、积分、微分三部分加总
    
60.         output = proportional + Kintegral * dt * integral + Kderivative * derivative;
    
61. 
    
62.         // 保存当前误差，留作下次计算时的前次误差
    
63.         previous_error = error;
    
64. 
    
65.         // 控制左侧马达
    
66.         left = speed - output;
    
67. 
    
68.         // 控制右侧马达
    
69.         right = speed + output;
    
70. 
    
71.         // 判断是否越界
    
72.         if (left >   100) left  =  100;
    
73.         if (left <  -100) left  = -100;
    
74.         if (right >  100) right =  100;
    
75.         if (right < -100) right = -100;
    
76. 
    
77.         motor[motorB]=left;
    
78.         motor[motorC]=right;
    
79.         nxtDisplayTextLine(5,"motorB= %d",left);
    
80.         nxtDisplayTextLine(7,"motorC= %d",right);
    
81. 
    
82.         // 等待取样时间
    
83.         wait1Msec(dt);
    
84.       }
    
85.         
    
86.   }
    
87.     while(SensorValue(sonarSensor)>8);
    
88.     motor[motorB]=0;
    
89.     motor[motorC]=0;
    
90.     wait1Msec(1000);
    
91.    
    
92.     nMotorEncoderTarget[motorA] = 150;
    
93. motor[motorA] = -20;
    
94. wait1Msec(800);
    
95. motor[motorA]=0;
    
96. wait1Msec(1000);
    
97. nMotorEncoder[motorB] = 0;               
    
98. nMotorEncoder[motorC] = 0;  
    
99. // reset the Motor Encoder of Motor C
    
100. nMotorEncoderTarget[motorB] = 300;        // set the  target for Motor Encoder of Motor B to 360
     
101. nMotorEncoderTarget[motorC] = -300;        // set the  target for Motor Encoder of Motor C to 360
     
102. motor[motorB] = 70;                       // motor B is run at a power level of 75
     
103. motor[motorC] = -70;  
     
104. // motor C is run at a power level of 75
     
105.        
     
106. while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle)
     
107. wait1Msec(1000);
     
108. {
     
109.        
     
110. motor[motorA]=25;
     
111. wait1Msec(800);
     
112. motor[motorB] = -50;                       // motor B is run at a power level of 75
     
113. motor[motorC] = 50;
     
114. wait1Msec(700);
     
115. }
     
116. }
     
117. }

复制代码

上面这个代码加上寻线之后，小车只执行寻线的那部分，没办法超声波检测了，求解

suhang

你这么写 又不是 并行运算。。肯定跳不出循环啊

suhang

1. #pragma config(Sensor, S4,     sonarSensor,         sensorSONAR)
   
2. #pragma config(Sensor, S1,lightSensor,sensorLightActive)
   
3. #pragma config(Sensor, S2,lightSensor,sensorLightActive)
   
4. #pragma platform(NXT)          //???NXT???
   
5. 
   
6. 
   
7. #define Kproportional 2         // Kp, ????
   
8. #define Kintegral 0.001          // Ki,????
   
9. #define Kderivative 10            // Kd,????
   
10. #define dt 1                   // ????
    
11. task main()
    
12. {
    
13.         for(;;){
    
14.    do
    
15.     {
    
16.     int error = 0;
    
17.     float previous_error = 0;
    
18.     float setpoint = 0;
    
19.     float actual_position = 0;
    
20.     float proportional = 0;
    
21.     int integral = 0;
    
22.     float derivative = 0;
    
23.     float output = 0;
    
24.     float left = 0;
    
25.     float right = 0;
    
26. 
    
27.     // ????????
    
28.     float speed=35;
    
29. 
    
30.     // ????
    
31.     //SetSensorColorFull(IN_3);
    
32. 
    
33.     // ??????
    
34.     setpoint=50;
    
35.     nxtDisplayTextLine(1,"setpoint= %d",setpoint);
    
36.     wait1Msec(20);
    
37. 
    
38.     // ??
    
39. while(1)
    
40. {
    
41.     while(SensorValue(sonarSensor)>8)
    
42.     {
    
43.         // ????????
    
44.         actual_position = SensorValue[lightSensor];
    
45.         nxtDisplayTextLine(3,"ac_pos= %d",actual_position);
    
46. 
    
47.         // ????
    
48.         error = setpoint - actual_position;
    
49.         // ????... ??????
    
50.         //if (error <> 0) PlayTone(TONE_B7, 1);
    
51. 
    
52.         // ????: ???? x ????
    
53.         proportional = Kproportional * error;
    
54. 
    
55.         // ??: ????
    
56.         integral = integral + error;
    
57. 
    
58.         // ??: ?????
    
59.         derivative = (error - previous_error) / dt;
    
60. 
    
61.         // ?????????????
    
62.         output = proportional + Kintegral * dt * integral + Kderivative * derivative;
    
63. 
    
64.         // ??????,????????????
    
65.         previous_error = error;
    
66. 
    
67.         // ??????
    
68.         left = speed - output;
    
69. 
    
70.         // ??????
    
71.         right = speed + output;
    
72. 
    
73.         // ??????
    
74.         if (left >   100) left  =  100;
    
75.         if (left <  -100) left  = -100;
    
76.         if (right >  100) right =  100;
    
77.         if (right < -100) right = -100;
    
78. 
    
79.         motor[motorB]=left;
    
80.         motor[motorC]=right;
    
81.         nxtDisplayTextLine(5,"motorB= %d",left);
    
82.         nxtDisplayTextLine(7,"motorC= %d",right);
    
83. 
    
84.         // ??????
    
85.         wait1Msec(dt);
    
86.       }
    
87.         
    
88.   }
    
89.    
    
90.     motor[motorB]=0;
    
91.     motor[motorC]=0;
    
92.     wait1Msec(1000);
    
93.    
    
94.     nMotorEncoderTarget[motorA] = 150;
    
95. motor[motorA] = -20;
    
96. wait1Msec(800);
    
97. motor[motorA]=0;
    
98. wait1Msec(1000);
    
99. nMotorEncoder[motorB] = 0;               
    
100. nMotorEncoder[motorC] = 0;  
     
101. // reset the Motor Encoder of Motor C
     
102. nMotorEncoderTarget[motorB] = 300;        // set the  target for Motor Encoder of Motor B to 360
     
103. nMotorEncoderTarget[motorC] = -300;        // set the  target for Motor Encoder of Motor C to 360
     
104. motor[motorB] = 70;                       // motor B is run at a power level of 75
     
105. motor[motorC] = -70;  
     
106. // motor C is run at a power level of 75
     
107.         
     
108. while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle)
     
109. wait1Msec(1000);
     
110. {
     
111.         
     
112. motor[motorA]=25;
     
113. wait1Msec(800);
     
114. motor[motorB] = -50;                       // motor B is run at a power level of 75
     
115. motor[motorC] = 50;
     
116. wait1Msec(700);
     
117. }
     
118. }
     
119. }
     
120. }

复制代码