关于NXC编写的迷宫机器人问题

tyker1

恩~是这样的，第二个程序咯这个是我接触乐高以来的，因为学校里有德国交换生来了，所以学校布置下任务要求我想一个乐高项目来进行演示，于是我选择了迷宫机器人。迷宫是一系列泡沫板拼接起来的，就是最普通的迷宫，里面没有任何记号。我选用的是一个超声传感器用于判断距离，然后每走30cm或者遇到障壁时停下进行一次左右方向的探测，然后根据“十”字岔路先直走然后若不通则退回来时左转（相当于进入十字路时右转）不通则退回直走（此时相当于左转）还不通则退回左转（相当于原路退出这个十字路）丁字路则先左后右然后返回，其他路总是先直走不通时再退回探测。可是当实际行走的时候出现以下问题：

1、机器人的判断频率似乎很难调整好，30cm有些地方可能超声探头刚好伸到岔路（车身还在直路里）而由于超声此时判断有岔路，若选择转弯则会卡在迷宫墙上，或者是直走转弯时可能由于左/右距离不够导致转弯时卡住
2、机器人选用2个伺服电机（B、C）加同样电压（设置为30）同时开关，但是行走时发现总是向左偏离直线，若设置为B=30、C=28，则会向右偏离直线，若选择B=30、C=29则向左偏，如此导致经常卡住或者卡在墙角（L形墙角前左右都认为被挡住了原地180度转弯会卡住，然后就一直在哪里转探头）
3、通过游标卡尺测得的两轮间距D（即车宽）与车轮直径d，计算转弯角度a及每个轮子所需转动角度b间关系的算式：b= (D*a)/d时实际转动的角度总是会多出那么4~5度（是指整辆车，如果是每个轮子的话肯定不止这么几度）

由以上这么多问题导致机器人总是无法正常通过迷宫（有时候需要用手去调整它的位置比如转弯角度太大的时候）能通过也是滑着墙过去的，有时甚至在不该回溯的地方回溯（由于转弯问题和走歪问题）
天朝说要河蟹不可以歪的！
附上程序各部件，希望各位大牛帮忙找找问题（程序上的），由于今天时间仓促，明天我会附上机器人的照片（配尺）希望大家能帮我找找错误，谢谢吖！因为后天下午就要演示了！><学校催的紧吖><

maze_stack.h：

1. 
   
2. #define max_depth 100 //the max depth of the cross roads in the maze(no more than 100)
   
3. int maze[max_depth];
   
4. int head,top;
   
5. bool push(int record)
   
6. {
   
7.   if (top >= max_depth)
   
8.     {
   
9.       return false;
   
10.     }else{
    
11.     if (top < head)
    
12.       {
    
13. top = 0;
    
14.       }
    
15.     maze[top] = record;
    
16.     top++;
    
17.     return true;
    
18.   }
    
19. }
    
20. int pop()
    
21. {
    
22.   int record;
    
23.   if (top < head)
    
24.     {
    
25.       return -1;
    
26.     }else{
    
27.     record = maze[top];
    
28.     top--;
    
29.     return record;
    
30.   }
    
31. }
    

复制代码

maze_motor.h：

1. 
   
2. 
   
3. /*=================================================/
   
4. /                                                                                                                                         /
   
5. /                The Header of maze.c                                                                                      /
   
6. /  Here includes some definations and functions                                                              /
   
7. /                       Made by CAKURA                                                                                      /
   
8. /=================================================*/
   
9. #define NEAR 10
   
10. #define Can_pass 30
    
11. #define h_WheelBase 12.5
    
12. #define d_Wheel 5.5
    
13. #define Turn_Angle(alpha) ((h_WheelBase*alpha)/d_Wheel)
    
14. #define Angle 90
    
15. #define left  2 //2 = 0b10 shows the left way hasn't been explored
    
16. #define right 1 //1 = 0b01 shows the right way hasn't been explored
    
17. #define step_len 30 //this parameter sets how often the robot stop to check the way(20 means stop per 20cm)
    
18. #define tire_len (d_Wheel*PI) //this parameter shows the circumference of the tire
    
19. #define turn_left  8 //8 = 0b1000 tells that we are now exploring the left cross
    
20. #define turn_right 4 //4 = 0b100  tells that we are now exploring the right cross
    
21. #define power_motor_run 30
    
22. #define power_turning_sensor power_motor_run
    
23. #define power_backing_sensor power_motor_run
    
24. #define unexplored 3 // 3  = 0b11 the low 2 digs records the ways that hasn't been explored
    
25. #define turning 12 //   turning = 0b1100 the records the turnning direction we need to turn
    
26. #define T_road 16 //   16 = 0b10000 sets if it is the T cross road
    
27. bool left_ok,right_ok;
    
28. int angle;
    
29. task run_B()
    
30. {
    
31.   RotateMotorEx(OUT_B,power_motor_run,-Turn_Angle(angle),0,false,true);
    
32.   left_ok = true;
    
33. }
    
34. task run_C()
    
35. {
    
36.   RotateMotorEx(OUT_C,power_motor_run,Turn_Angle(angle),0,false,true);
    
37.   right_ok = true;
    
38. }
    
39. 
    
40. bool explore_left()
    
41. {
    
42.   if (!(MotorRotationCount(OUT_A) == 0))
    
43.     {
    
44.       RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
45.     }
    
46.   ResetRotationCount(OUT_A);
    
47.   RotateMotorEx(OUT_A,power_turning_sensor,Angle,0,false,true);
    
48.   if (SensorUS(IN_4) > Can_pass)
    
49.   {
    
50.    RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
51.    return true;
    
52.   }else
    
53.   {
    
54.    RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
55.    return false;
    
56.   }
    
57. }
    
58. bool explore_right()
    
59. {
    
60.   if (!(MotorRotationCount(OUT_A) == 0))
    
61.     {
    
62.       RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
63.     }
    
64.   ResetRotationCount(OUT_A);
    
65.   RotateMotorEx(OUT_A,power_turning_sensor,-Angle,0,false,true);
    
66.   if (SensorUS(IN_4) > Can_pass)
    
67.   {
    
68.    RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
69.    return true;
    
70.   }else
    
71.   {
    
72.    RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
73.    return false;
    
74.   }
    
75. }
    
76. int turn_the_way(int ways)
    
77. {
    
78.   int new_way;
    
79.   switch (ways)
    
80.     {
    
81.     case  1:
    
82.       new_way = 2;
    
83.       break;
    
84.     case  2:
    
85.       new_way = 1;
    
86.       break;
    
87.     default:
    
88.       new_way = ways;
    
89.       break;
    
90.     }
    
91.   return new_way;
    
92. }
    
93. void wait_motor(bool is_back,bool is_left)
    
94. {
    
95.   left_ok = false;
    
96.   right_ok = false;
    
97.   if (is_back)
    
98.   {
    
99.      angle = 180;
    
100.   }else
     
101.   {
     
102.    if (is_left)
     
103.    {
     
104.     angle = Angle;
     
105.    }else
     
106.    {
     
107.     angle = -Angle;
     
108.    }
     
109.   }
     
110.   if (is_left)
     
111.     {
     
112.       start run_C;
     
113.       start run_B;
     
114.     }else
     
115.     {
     
116.       start run_B;
     
117.       start run_C;
     
118.     }
     
119.   while (!(left_ok && right_ok));
     
120.   stop run_B;
     
121.   stop run_C;
     
122. }
     
123. void run_step()
     
124. {
     
125.   int distance;
     
126.   ResetRotationCount(OUT_BC);
     
127.   distance = SensorUS(IN_4);
     
128.   OnFwd(OUT_BC,power_motor_run);
     
129.   while (((distance - SensorUS(IN_4)) < step_len) && (SensorUS(IN_4) > NEAR)) ;
     
130.   Off(OUT_BC);
     
131. }
     
132. int check_ways()
     
133. {
     
134.   int ways;
     
135.   ways = 0;
     
136.   if (explore_left())
     
137.     {
     
138.       ways = ways | left;
     
139.     }
     
140.   if (explore_right())
     
141.     {
     
142.       ways = ways | right;
     
143.     }
     
144.   return ways;
     
145. }

复制代码

maze.nxc：

1. 
   
2. #include "NXCDefs.h"
   
3. #include "maze_stack.h"
   
4. #include "maze_motor.h"
   
5. //There's something you must know before reading the Program
   
6. //the formation of data used in the stack is set to like this
   
7. //sample data : 10 ;10 = 0b01010 and the low two digs 10 means there's 2 directions still not be explored
   
8. //and the higher two digs 10 means when backing to this cross,we will choose to turn left(left = 0b10,right = 0b01)
   
9. //and the highest dig 0 means it is not a "T" corss (1 means Yes)
   
10. void init()
    
11. {
    
12.   head = 0;
    
13.   top = 0;
    
14.   maze[top] = 0;
    
15.   
    
16.   //Setup the UltraSonicSensor installed in Socked_4,and start it to work
    
17.   SetSensorLowspeed(IN_4);
    
18.   
    
19.   //Reset the Rotation Count of Motor_A and sets it to fornt
    
20.   RotateMotorEx(OUT_A,power_backing_sensor,-(MotorRotationCount(OUT_A)),0,false,true);
    
21.   ResetRotationCount(OUT_A);
    
22. }
    
23. task main()
    
24. {
    
25.   int ways,temp;
    
26.   bool backing;
    
27.   init();
    
28.   backing = false;
    
29.   while (1)
    
30.     {
    
31.       //we run steply to make sure we won't miss any possible crosses
    
32.       run_step();
    
33.       ways = check_ways();
    
34.       
    
35.       if (!(backing))
    
36. {
    
37.      //if we are not going back
    
38.           if (SensorUS(IN_4) > Can_pass)
    
39.      {
    
40.         // if there's a way in front,we check the ways found by UltraSonic Sensor
    
41.        switch (ways)
    
42.   {
    
43.   case   (left | right) :{      // if left & right is both accessible,we identify it as a "+" cross
    
44.     push(ways-1);
    
45.     run_step();
    
46.     break;
    
47.   }
    
48.   case   (left) :{
    
49.     push((ways-1) | turn_right);
    
50.     run_step();
    
51.     break;
    
52.   }
    
53.   case   (right) :{
    
54.     push((ways) | turn_left);
    
55.     run_step();
    
56.     break;
    
57.   }
    
58.   default  :break;
    
59.   }
    
60.      }else
    
61.      {
    
62.        switch (ways)
    
63.   {
    
64.   case  (left | right) :{
    
65.     push(1 | turn_left | T_road);
    
66.     wait_motor(false,true);
    
67.     run_step();
    
68.     break;
    
69.   }
    
70.   case  (left) :{
    
71.     wait_motor(false,true);
    
72.     break;
    
73.   }
    
74.   case  (right) :{
    
75.     wait_motor(false,false);
    
76.     break;
    
77.   }
    
78.   default :{
    
79.     wait_motor(true,true);
    
80.     backing = true;
    
81.     break;
    
82.   }
    
83.   }
    
84.      }
    
85. }else
    
86. {
    
87.    temp = pop();
    
88.    if (SensorUS(IN_4) > Can_pass)
    
89.      {
    
90.        switch (ways)
    
91.   {
    
92.   case    (left | right) :{
    
93.     switch(temp)
    
94.       {
    
95.       case   (left):{
    
96.         wait_motor(false,true);
    
97.         run_step();
    
98.         push(temp-1);
    
99.         backing = false;
    
100.         break;
     
101.       }
     
102.       case   (right):{
     
103.         run_step();
     
104.         backing = false;
     
105.         push(temp-1);
     
106.         break;
     
107.       }
     
108.       case   0:{
     
109.         wait_motor(false,false);
     
110.         run_step();
     
111.         break;
     
112.       }
     
113.       default :break;
     
114.       }
     
115.     break;
     
116.   }
     
117.   case     (left) :{
     
118.     wait_motor(false,((temp & turning) == turn_left));
     
119.     run_step();
     
120.     if (!((temp & unexplored) == 0)) push(temp-1);
     
121.     backing = ((temp & unexplored) == 0);
     
122.     break;
     
123.   }
     
124.   case     (right) :{
     
125.     if ((temp & T_road) != T_road)
     
126.       {
     
127.         wait_motor(false,((temp & turning) == turn_left));
     
128.       }else{
     
129.       temp = (temp & unexplored) | turn_left;
     
130.     }
     
131.     run_step();
     
132.     if (!((temp & unexplored) == 0)) push(temp-1);
     
133.     backing = ((temp & unexplored) == 0);
     
134.     break;
     
135.   }
     
136.   default    : break;
     
137.   }
     
138.      }else
     
139.      {
     
140.        switch(ways)
     
141.   {
     
142.   case    (left | right):{
     
143.     wait_motor(false,((temp & turning) == turn_left));
     
144.     run_step();
     
145.     break;
     
146.   }
     
147.   case    (left):{
     
148.     wait_motor(false,true);
     
149.     break;
     
150.   }
     
151.   case    (right):{
     
152.     wait_motor(false,false);
     
153.     break;
     
154.   }
     
155.   case    0: StopTask(main);
     
156.   }
     
157.      }
     
158. }
     
159.     }
     
160. }

复制代码

万分感谢吖！

tyker1

程序用的思想是“深度优先搜索”即回溯法，即不断尝试各条路，只要发现可能便深入发现不对便退回重新选择（将原递归思路改成while循环配上栈的方法）进行探测。

PS：NXT（或者说NXC）允许递归吗~？最大深度是多少~？谁用过NXC来递归~？

“”

我觉得不用这么复杂，不需要用递归。
先测右边
右边有墙：测前面：前面有墙左转，前面没有墙，往前走。
右边没有墙，测右转。

如此循环就可以了

“”

如果不用递归来写，
程序相对简单。
用nxt——g就能完成任务了，因为nxt-g电机的控制比较好，直线性比较好。
转弯由于惯性是会有差错的，而且每块地方的摩擦力不同，水平程度不同，有误差很正常。
如果时间急，是没有方法精确调整这么多的角度的。
建议直接后退靠墙，调整机器位置，有效而且稳定，就是整个过程时间比较长。

糖伯虎

nxc不支持递归的，至少以前是不知道，不知道后面更新后怎么样

ed8841

楼主，我也刚接触乐高机器人是个新手，说一下我的一些想法，也没有验证过，仅供参考。
如果用右转法则解迷宫，我的想法如下：
1、超声波距离传感器面向右侧，探测与墙面的距离，和是否有岔路。
2、触碰传感器面向前方判断路是不是通。
程序可以这样设计：
1、保持距离传感器与右墙面的距离使小车直线行走。
2、如果右侧探测到岔路（距离突然变大），则右转。
3、如果前方的触碰传感器被压下，说明此路不通，则180度掉头。
不知道用以上方法能不能做出迷宫机器人，只是本人的设想而已。

tyker1

回复6楼，理论上可行，不过想想走出迷宫以后机器人的行为·。·绕着迷宫外墙走·。·不过理论上来说除了回形岔路以外基本这样右行规则确实可以走啦~。
4楼的童鞋，有个问题，靠墙以后我怎么转弯·。·~？还有~要么就是按6楼的X行规则，不然没有数据结构的支持NXT-G做不出来吧~？
大家一起讨论啊！这个问题似乎跟电机和尾轮有关，走不了直线啊！！！

突然想到，大家说，我在run_step()过程那里while循环里面监视BC2个电机转过的角度发现某电机转过角度比另一个大就把转的慢的那个电压加高等相同了再调回来是否可行~？

“”

……是转弯后靠墙~~~
不是靠墙后转弯……