# 相扑机器人4--机器人的自我意识
基于有限状态机方式搭建好程序。现在在传感器和机器人的行动中间，多了一个逻辑上的层次：状态。在每个状态都有明确的界限之后，可以针对单个状态作一番思考。
# 情景分析
现在尝试在程序中加入更多的逻辑。考虑以下这些想法：
- 在移动躲避逻辑中，可能机器人刹不住车已经越过了胶带。如果这样，再判断碰到胶带就不合适。将它修改为忽略胶带，强制反方向移动。
- 如果机器人对着同一个目标持续超过15秒钟，那么应该忽略这个目标，尝试找其他目标。
- 如果机器人对着同一个目标进攻过程碰胶带超过5次，那么应该忽略这个目标（它已经出界了），尝试找其他目标。
- 如果机器人进攻状态持续了5秒钟（没有推动），那么强制后退500毫秒再看情况，需要考虑碰胶带。如果碰胶带，应该强制前进。
- 搜索逻辑中，如果持续了3秒钟（足够转一圈）没有改变，需要强制机器人移动一段距离，改变位置。需要考虑碰胶带。
- 观察机器人运行，如果胶带附近，左右循环碰胶带，最后就会出界。要识别这种情况。改变这种情况。
我们看看怎么来实现这些功能，有限状态机会一直维持当前状态，直到主动转移到其他状态，这个特点很适合将每个状态的代码都写成一个函数，在平常看来，函数就是一直从头到尾一遍一遍地执行（可以认为是在一个函数中，实现一个单一功能的机器人）
如果实现一个想法会导致多出来一些状态，可以在当前状态机中新增一个状态。我们更多的时候都是在状态内实现，假如多出来的状态跟其他部分没有关联（其他状态都不会直接进入这个多出来的子状态），在当前状态内部维护一个小的内部状态机也是很不错的办法，就完全不需要修改其他状态的代码。
- 想法1，在移动躲避逻辑中，如果机器人已经越过了胶带，再判断碰到胶带就不合适。将它修改为忽略胶带，强制反方向移动。在一个状态中，如果不主动转移状态，程序就一直会维持在当前状态。进入躲避逻辑时，马达已经设置了。所以只等待状态持续的时间够了，就转移状态。
//移动躲避状态的逻辑
void avoid_move() {
if (state_timeout(200)) {
turnRandom_enter(STATE_TURN_AVOIDING);
}
}
1
2
3
4
5
6
2
3
4
5
6
只要进入了移动躲避状态，函数就会一直循环运行，直到主动转移状态。
想法2，3都涉及进攻状态，因为目标是真实存在的，假如我们在进攻状态中忽略这个目标，那么应该做什么？转移到搜索状态？这样搜索状态会立刻发现目标，之后再次进入进攻状态。对于在多个状态中都有影响的逻辑。并不适合在状态内完成。或者可以想象成一个虚拟的传感器，感应值代表目标是否已经失效了。
定义全局变量target_begin，记录发现新目标的时间，判断是不是新目标比较麻烦，我们可以记录不存在目标的最后时间来替代，如果不存在目标，就一直更新变量为当前时间，发现目标后不更新了，就等于是新目标的发现时间。
决定目标应该忽略以后，要阻止程序进入进攻状态。许多if(SU)的代码需要改成if(SU && 目标有效)，但最方便的办法就是将目标变量SU设置为false（副作用是后面的程序不知道目标是真不存在还是忽略了）。想法3，进攻过程中碰胶带，只能在进攻状态中才知道，定义全局变量
target_out，在进攻状态中记录碰胶带次数。每次在进攻状态中碰胶带，就将它加1 ，丢失（真实的）目标后应该清零。同想法2一样，如果碰胶带次数过多，就将目标忽略掉。
unsigned long target_begin = 0;
int target_out = 0;
void loop() {
readSensor();
//SU代表目标是否真的存在
if (!SU) {
target_begin = millis();
target_out = 0；
}
if (millis() - target_begin > 15000) SU = false;
if (target_out > 5) SU = false;
//到这里，SU代表:不存在或者忽略了。
......
//进攻中碰胶带时增加target_out计数
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
2
3
4
5
6
7
8
9
10
11
12
13
14
15
读取传感器后，SU是超声波传感器的计算结果，代表目标是否真实存在。设置
SU为false之后，代表是不存在或者忽略了，只对后面的程序起作用。无论如何设置，此处都没有改变状态，如果当前状态是进攻状态，仍会执行进攻状态的代码。但因为没有目标，进攻状态中会转移到其他状态。
- 想法4，如果机器人进攻状态持续了5秒钟，那么强制后退500毫秒，需要考虑碰胶带。如果碰胶带，应该强制前进。可以在进攻状态中维持一个小的状态机。但要注意在离开状态前复位状态机。子状态有正常进攻、强制后退两个状态。在正常（子）状态下，添加超时后进入强制后退（子）状态。并编写新的子状态逻辑。子状态可以跳转到外层的其他状态，但是外层的状态不能进入特定的子状态。进入到母状态后，到底是哪一个子状态，由内部的状态机决定。
void attack() {
const int subSTATE_ATTACKING = 11;
const int subSTATE_BACKWARDING = 12;
//静态变量，相当于仅内部使用的全局变量
static int sub_state = subSTATE_ATTACKING;
if (sub_state == subSTATE_BACKWARDING) {
//进攻状态强制后退子状态逻辑
//在进攻状态--强制后退状子态中,任何出口都要复位状态。
if (SL || SR) {
//后退过程中碰胶带，要前进，进入移动躲避
forward_enter(STATE_MOVE_AVOIDING);
sub_state = subSTATE_ATTACKING; //复位子状态
} else if (!SU) {
//后退过程中丢失目标，随机转弯，进入搜索
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_ATTACKING; //复位子状态
} else if (state_timeout(500)) {
//时间结束，转到正常状态
//enter之后，状态没有改变，但是会重新计时
forward_enter(STATE_ATTACKING);
sub_state = subSTATE_ATTACKING;
}
} else {
//进攻状态正常（子）状态
if (SL || SR) {
backward_enter(STATE_MOVE_AVOIDING);
target_out ++; //进攻状态中碰胶带，碰胶带次数+1
} else if (!SU) {
turnRandom_enter(STATE_SEARCHING);
} else if (state_timeout(5000)) {
//进入强制后退子状态。注意，不改变状态，但重新计时
backward_enter(STATE_ATTACKING);
sub_state = subSTATE_BACKWARDING;
}
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
重要的事情再说一遍：只要进入一个状态，就会一直运行这个状态的代码，直到在这个状态中主动转移状态。
想法5同想法4类似。在状态机中维护一个子状态，有两个状态：正常状态、强制前进状态，在正常搜索状态中为它添加入口，并为新状态添加逻辑。
想法6的情况比较前面的抽象一些。观察机器人运行，在中间区域搜索时长时间都不会碰胶带，如果在胶带附近搜索，两次碰胶带的时间比较短。可以设置一个变量，在搜索状态碰胶带时记录时间，第二次碰胶带的时候计算两次碰胶带的时间，如果比较短，就判断为在胶带附近。考虑传感器会误触发，再增加一个变量进行计数，如果连续多次出现短时间内频繁碰胶带，就认为是在胶带附近。如果其他原因离开搜索状态，要把计数清零。因为机器人只有两个红外反射传感器，判断不了机器人的角度，不知道如何移动。不过我们知道两次碰胶带之间的时间，如果让机器人往左前方，或者右前方强制移动一段时间（少于碰胶带间隔）应该是不会出界的。再增加一个子状态，叫做接近胶带状态。逐个状态添加逻辑。
void search() {
const int subSTATE_SEARCHING = 21; //正常搜索子状态
const int subSTATE_FORWARDING = 22; //强制前进子状态
const int subSTATE_WALKING_AWAY = 23; //接近胶带子状态
static int sub_state = subSTATE_SEARCHING;
if (sub_state == subSTATE_FORWARDING) {
//强制前进子状态逻辑
if (SL || SR) {
backward_enter(STATE_MOVE_AVOIDING);
sub_state = subSTATE_SEARCHING;
} else if (state_timeout(500)) {
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_SEARCHING;
}
} else if (sub_state == subSTATE_WALKING_AWAY) {
//接近胶带子状态逻辑
if (state_timeout(500)) {
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_SEARCHING;
}
} else {
//正常搜索子状态逻辑
if (SL && SR) {
backward_enter(STATE_MOVE_AVOIDING);
} else if (SL || SR) {
static unsigned long ir_turn_begin = 0;
static unsigned int ir_turn_counter = 0;
if (millis() - ir_turn_begin < 1000) {
ir_turn_counter ++;
} else {
ir_turn_counter = 0;
}
if (ir_turn_counter > 3) {
if (SL) {
enter(STATE_SEARCHING, 150, 0);
} else {
enter(STATE_SEARCHING, 0, 150);
}
sub_state = subSTATE_WALKING_AWAY;
ir_turn_counter = 0;
} else {
turnByIR_enter(STATE_TURN_AVOIDING);
ir_turn_begin = millis();
}
} else if (SU) {
forward_enter(STATE_ATTACKING);
} else if (state_timeout(3000)) {
//超时进入强制前进状态
forward_enter(STATE_SEARCHING);
sub_state = subSTATE_FORWARDING;
}
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
# 参考程序
把各个状态的代码合并，组成最后的程序，用下面的代码替换掉上一节程序的loop函数即可。
unsigned long target_begin = 0;
int target_out = 0;
void loop() {
readSensor();
if (!SU) {
target_begin = millis();
target_out = 0;
}
if (millis() - target_begin > 15000) SU = false;
if (target_out > 5) SU = false;
if (state == STATE_SEARCHING) {
search();
} else if (state == STATE_ATTACKING) {
attack();
} else if (state == STATE_MOVE_AVOIDING) {
avoid_move();
} else if (state == STATE_TURN_AVOIDING) {
avoid_turn();
} else {
//不可能有未知的状态，如果有就报警，这是异常状态
if (state_timeout(500)) {
enter(state, 0, 0);
pinMode(13, OUTPUT);
digitalWrite(13, !digitalRead(13));
}
}
}
void attack() {
const int subSTATE_ATTACKING = 11;
const int subSTATE_BACKWARDING = 12;
//静态变量，相当于仅内部使用的全局变量
static int sub_state = subSTATE_ATTACKING;
if (sub_state == subSTATE_BACKWARDING) {
//进攻状态强制后退子状态逻辑
//在进攻状态--强制后退状子态中,任何出口都要复位状态。
if (SL || SR) {
//后退过程中碰胶带，要前进，进入移动躲避
forward_enter(STATE_MOVE_AVOIDING);
sub_state = subSTATE_ATTACKING;
} else if (!SU) {
//后退过程中丢失目标，随机转弯，进入搜索
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_ATTACKING;
} else if (state_timeout(500)) {
//时间结束，转到正常状态
//enter之后，状态没有改变，但是会重新计时
forward_enter(STATE_ATTACKING);
sub_state = subSTATE_ATTACKING;
}
} else {
//进攻状态正常（子）状态
if (SL || SR) {
backward_enter(STATE_MOVE_AVOIDING);
target_out ++; //进攻状态中碰胶带，碰胶带次数+1
} else if (!SU) {
turnRandom_enter(STATE_SEARCHING);
} else if (state_timeout(5000)) {
//进入强制后退子状态。注意，不改变状态，但重新计时
backward_enter(STATE_ATTACKING);
sub_state = subSTATE_BACKWARDING;
}
}
}
void search() {
const int subSTATE_SEARCHING = 21; //正常搜索子状态
const int subSTATE_FORWARDING = 22; //强制前进子状态
const int subSTATE_WALKING_AWAY = 23; //接近胶带子状态
static int sub_state = subSTATE_SEARCHING;
if (sub_state == subSTATE_FORWARDING) {
//强制前进子状态逻辑
if (SL || SR) {
backward_enter(STATE_MOVE_AVOIDING);
sub_state = subSTATE_SEARCHING;
} else if (state_timeout(500)) {
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_SEARCHING;
}
} else if (sub_state == subSTATE_WALKING_AWAY) {
//接近胶带子状态逻辑
if (state_timeout(500)) {
turnRandom_enter(STATE_SEARCHING);
sub_state = subSTATE_SEARCHING;
}
} else {
//正常搜索子状态逻辑
if (SL && SR) {
backward_enter(STATE_MOVE_AVOIDING);
} else if (SL || SR) {
static unsigned long ir_turn_begin = 0;
static unsigned int ir_turn_counter = 0;
if (millis() - ir_turn_begin < 1000) {
ir_turn_counter ++;
} else {
ir_turn_counter = 0;
}
if (ir_turn_counter > 3) {
if (SL) {
enter(STATE_SEARCHING, 150, 0);
} else {
enter(STATE_SEARCHING, 0, 150);
}
sub_state = subSTATE_WALKING_AWAY;
ir_turn_counter = 0;
} else {
turnByIR_enter(STATE_TURN_AVOIDING);
ir_turn_begin = millis();
}
} else if (SU) {
forward_enter(STATE_ATTACKING);
} else if (state_timeout(3000)) {
//超时进入强制前进状态
forward_enter(STATE_SEARCHING);
sub_state = subSTATE_FORWARDING;
}
}
}
void avoid_turn() {
//转弯躲避状态的逻辑
if (!(SL || SR)) {
enter(STATE_SEARCHING);
}
}
void avoid_move() {
//移动躲避状态的逻辑
if (state_timeout(200)) {
turnRandom_enter(STATE_TURN_AVOIDING);
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
# 运行结果
测试看看，我们的想法机器人都实现了没有？
# 程序解读
- 我们进一步把过长的代码写成函数，并且在函数内使用静态变量，维持内部的嵌套状态机。将各个想法逐一在各自的状态中实现。loop函数内的结构就非常简单了。有限状态机的每个状态函数，看起来都是一个功能单一的小机器人（内部状态是小小机器人），通过状态机来变身。
- 在c/c++语言中，在前面的代码是不能调用在后面定义的函数的。在Arduino中，函数的定义、调用顺序是可以颠倒，可以先调用后定义。