OSEPPcn

# 相扑机器人3--有限状态机

上一节的程序里，我们用while等待一个状态结束，执行一些动作后，进入一个新的状态。while里面就要判断很多情况，代码会很多重复，逻辑也不清晰。即便可以用函数加上合并逻辑来减少代码，但是改变不了结构的缺陷。我们需要需要一种方法来去掉while循环。

# 情景分析

上一节说到，机器人只有3个状态，并且任何时候都处在其中一种状态。不过避开胶带时，分成转弯避开和后退避开，处理上有一些不同。把它分为两个状态，转弯躲避，移动躲避。机器人就有4个状态了。我们可以这样来描述这4个状态：

uml diagram

图中表示了机器人的4个状态，状态改变的条件,和状态改变时的机器人控制。
用一个变量来表示机器人现在的状态，每个状态下面的逻辑都不同，搜索的时候，发现目标，设置机器人前进，然后把状态变量设置为进攻状态。进攻状态发现没有目标了，设置机器人转弯,转为搜索状态……
考虑好每个状态的逻辑后，根据状态变量来选择执行，只执行这个状态的代码。每个状态都负责在条件合适的时候，转移到别的状态，并在转移的时候设置好机器人。这样就不需要while循环了。
先用一个表格来把当前状态，遇到条件，执行动作，进入状态都列清楚。然后跟着表格编写状态的代码。

当前状态 遇到条件 执行动作 进入状态
搜索 左右同时碰胶带 后退 移动躲避
搜索 左或者右碰胶带 选择转弯，右转或者左转 转弯躲避
搜索 发现目标 前进 进攻
移动躲避 离开了胶带 随机转弯 搜索
转弯躲避 离开了胶带 不改变马达 搜索
进攻 左或者右或者同时碰胶带 后退 移动躲避
进攻 丢失目标 随机转弯 搜索

# 流程解析

  1. 开机后记录纸张参考值，并设置初始状态为搜索状态
  2. 读取传感器，并判断是否碰到胶带，是否发现目标，保存到变量。
  3. 根据当前状态变量，执行对应状态的代码，
  4. 当前状态下符合转移条件时，设置合适的机器人动作，并改变状态变量。下一次循环将由新的状态代码来处理。

    在没有循环等待，没有暂停指令的情况下，每秒钟loop函数执行成千上万次。

uml diagram

# 参考程序

如果用数字来表示状态会很容易弄混淆。我们用名字来表示

const int STATE_SEARCHING = 1;      //搜索状态
const int STATE_ATTACKING = 2;      //进攻状态
const int STATE_TURN_AVOIDING = 3;  //转弯躲避状态
const int STATE_MOVE_AVOIDING = 4;  //移动躲避状态
int state;                          //当前状态
1
2
3
4
5

在变量前面添加一个关键字const，表示这个变量是不能修改的，称为常量。注意每个状态的值都要不一样。
再定义一个变量state来表示当前的状态。state是需要改变的，不要加const。
从表格里看，我们要经常设置状态，同时设置马达。前面还提到要有超时控制，需要保存时间。把它写成一个函数。在进入新状态时，顺便保存一下时间。每次转换状态都调用函数实现，这样就不怕忘记设置时间了。但从转弯躲避进入搜索时不需要设置马达，我们再多定义一个重载函数。省略掉设置马达速度参数。
表格里的执行动作部分，有重复的，并且里面都隐含了马达速度，也写成函数。就不怕犯迷糊设置错误的速度，或者同一个行为设置了不相同的速度。

试想，有一天你决定给机器人换上一个强劲的马达，店小二热心地把线帮你接好了。回来发现线接反了。当然你可以把线调换回来，可你懒得找工具了。所以你决定修改代码，把速度的正负符号调换来修正。于是你从头到尾把代码里的速度修改了一遍，上传程序。打开电源，机器人满世界乱跑。你非常肯定你漏了某些地方没修改！可是那些地方呢？要是写成函数，它们就都只出现一次，并且都集中在一起了。

#include <oseppRobot.h>

OseppTBMotor L(12, 11);
OseppTBMotor R(8, 3, LOW);
OseppRangeFinder U(2);

int P = 0;   //开机时记录的纸张感应值作为参考
boolean SR;  //右边传感器是否在胶带上
boolean SL;  //左边传感器是否在胶带上
boolean SU;  //是否发现目标

void readSensor() {
    SR = abs(analogRead(A0) - P) > 300;
    SL = abs(analogRead(A1) - P) > 300;
    SU = U.ping() < 600;  // 600毫米内有东西就判定为发现目标
}

const int STATE_SEARCHING = 1;      //搜索状态
const int STATE_ATTACKING = 2;      //进攻状态
const int STATE_TURN_AVOIDING = 3;  //转弯躲避状态
const int STATE_MOVE_AVOIDING = 4;  //移动躲避状态
int state;                          //当前状态
unsigned long begin;                //当前状态的开始时间

//设置状态变量为给定状态，并记录时间
void enter(int newState) {
    state = newState;
    begin = millis();
}

//设置状态变量为给定状态，同时设置两个马达，并记录时间
void enter(int newState, int left_motor, int right_motor) {
    L.forward(left_motor);  //通过负数来后退
    R.forward(right_motor);
    enter(newState);
}

//返回当前状态下持续的时间是否超过了参数给的时间
boolean state_timeout(unsigned long ms) {
    return millis() - begin > ms;
}

//后退，并进入给定的状态
void backward_enter(int newState) {
    enter(newState, -100, -100);
}

//选择性转弯，并进入给定的状态
void turnByIR_enter(int newState) {
    if (SL) {
        enter(newState, 100, -100);
    } else {
        enter(newState, -100, 100);
    }
}

//前进，并进入给定的状态
void forward_enter(int newState) {
    enter(newState, 150, 150);
}

//随机转弯，并进入给定的状态
void turnRandom_enter(int newState) {
    if (random(2)) {
        enter(newState, 100, -100);
    } else {
        enter(newState, -100, 100);
    }
}
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

辛苦地写好了这些函数，如果你朋友跟你一起学习机器人，他刚好有空，没准你可以让他帮你完成。你只需要把函数的定义，要求给他。他就可以补充函数的代码。使用函数是团队中最简单的合作方法。
传感器部分已经写成了函数，调用readSensor之后，变量SL,SR,SU就是现成的。代码是很久之前写的了，可能在写下面代码的时候，你已经忘记了是怎么读取怎么判断的了，但是这不关键，它的结果是正确的。哪些读传感器时判断这呀那的伤脑筋的事情，现在不用再回忆一次。函数能把工程中不同的难点划分开逐个解决，你现在只想逻辑部分就可以了。
接下来实现主要逻辑部分。机器人的行为已经编写成了函数，在主循环loop函数中的代码，就只体现逻辑。

void setup() {
    P = analogRead(A0);
    //开机后随机转弯，进入搜索状态
    turnRandom_enter(STATE_SEARCHING);
}

void loop() {
    readSensor();
    if (state == STATE_SEARCHING) {
        //搜索状态的逻辑
        if (SL && SR) {  //两边碰胶带，后退，下一个状态：移动躲避
            backward_enter(STATE_MOVE_AVOIDING);
        } else if (SL || SR) {  //左边碰胶带，右转，下一个状态：转弯躲避
            turnByIR_enter(STATE_TURN_AVOIDING);
        } else if (SU) {  //发现目标，前进，下一个状态：进攻
            forward_enter(STATE_ATTACKING);
        }
    } else if (state == STATE_MOVE_AVOIDING) {
        //移动躲避状态的逻辑
        if (!(SL || SR)) {  //离开胶带，随机转弯，下一个状态：搜索
            turnRandom_enter(STATE_SEARCHING);
        }
    } else if (state == STATE_TURN_AVOIDING) {
        //转弯躲避状态的逻辑
        if (!(SL || SR)) {  //离开胶带，不改变转弯方向，下一个状态：搜索
            enter(STATE_SEARCHING);
        }
    } else if (state == STATE_ATTACKING) {
        //进攻状态的逻辑
        if (SL || SR) {  //碰到胶带，后退，下一个状态：移动躲避
            backward_enter(STATE_MOVE_AVOIDING);
        } else if (!SU) {  //丢失目标，随机转弯，下一个状态：搜索
            turnRandom_enter(STATE_SEARCHING);
        }
    } else {
        //不可能有未知的状态，如果有就报警，这是异常状态
        if (state_timeout(500)) {
            enter(state, 0, 0);
            pinMode(13, OUTPUT);
            digitalWrite(13, !digitalRead(13));
        }
    }
}
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

# 运行结果

用有限状态机实现的代码和上一节的程序运行结果基本一样。

# 程序解读

将程序分成不同状态，根据状态变量来执行状态代码，我们去掉了while循环，实现了同样的功能。通过函数把代码封装后，程序逻辑也更清晰了一些。一串if-else语句将状态分开，对于每个状态，只需要考虑状态内的逻辑对照表格来写代码。相当于用一个状态变量，把几个程序合在一起，搜索状态时，机器人是搜索机器人，碰胶带时是躲避机器人。在当前状态下，条件合适的时候就变身，成为其他机器人。程序虽然比较长，但是每处都只涉及的局部的逻辑，结构比较清晰。
有限状态机由有限的状态，状态转移的条件，和转移时执行的动作组成。有限状态机主要有Moore状态机，Mealy状态机，我们使用的是Mealy状态机，由当前状态和输入（传感器）来决定下一个状态和输出（马达控制）
辛辛苦苦的把原来的代码变长了那么多，下一节我们看看它是怎么改善上一节中提到得哪些难题的。

# ArduinoIDE操作视频

← 相扑机器人4--机器人的自我意识 →