这是浙江大学2022年9月机器人基础实践课程代码。
本仓库在9.8课程结束之前仅作为组内代码托管、协作开发平台。在9.8课程结束之前，请勿随意传播本仓库。
在开发视觉传感器过程中，高斯模糊后机器人眼中的小组成员，黄色部分为机械爪（及海绵）。
最终考核运行得分：110 历史最高得分：145
在如图所示的场地内设计并制造一个机器人完成巡线和物块的抓取、放置工作。
场地全景图。其他小图为场地细节。
由全景图可见，场地的黑线并非闭合，而是在中间有断开，若要完成巡线工作，则要求机器人能够顺利识别障碍物（椅子）并完成避障，再顺利回到线上进行巡线。
此外，场地的白色部分并非全为白色，有些地方灰度较高，起点终点处有红线（但其反光率较高故影响较小）。可能会对巡线尤其是边缘灰度传感器或边缘白标传感器造成影响，导致其误判为T字形黑线。
抓取和放置的台子对称放置，需要经过上坡下坡才能到达，且下坡之后紧接着转弯，若不做速度上的控制，则冲下坡时可能由于速度过大冲出线外。
抓取的位置设有T形黑线，可以通过在车的右边缘安装一个灰度传感器或者白标传感器来识别右侧的黑线。由于前述的场地原因，边缘传感器很可能把场地上比较脏的地方误判为黑线，从而导致过早的转弯。
抓取部分的台子放有六个物块，颜色随机放置，然而其底部有红绿蓝三种颜色的色块，这就对直接向这个台子使用基于视觉或者其他方法的颜色识别造成了干扰。
在最后一个晚上，我们想出了三个策略，其主程序代码以分支的形式储存于仓库中。但最终由于时间关系，其余两个策略来不及做进一步的调试，我们就选择了最稳妥的策略。
本方案预期得分最高可达145分，而我们在测试中也确实曾经达到过这一分数。
在最后一天的傍晚，姚婉婷和杨东申调试出了抓取下排三个物体的机械臂的位姿，为我们实现这部分策略提供了可能。在此之前，我们已经调试出了这个策略所需要的所有函数，于是在最后时间紧张的时候，使用了这个最为稳妥的方法。然而本策略可以进一步改进，详见王开炫个人总结部分。
本方案预期得分最高可达160分，但由于最后时间不足，放弃了这个策略的调试。
本方案预期得分最高可达220分，但由于2min时间限制，几乎无法实现。
车的底部由两个主动轮和一个万向轮组成，用TB6612芯片的驱动板驱动电机，而电机的转速由cruise() 函数根据三个灰度传感器的输入来调整。
右轮前装有一个套件内的白标传感器（与灰度传感器类似），在代码中实例名为 EdgeSensor ，类名为WhiteScaleSensor 。当这个传感器探测到黑线的时候，根据已经被触发的次数，可以判定为是grasping placing round2状态，从而做出对应的响应。
在万向轮前安装套件内的超声波传感器，用于检测障碍物并及时触发避障。使用串口监视器测试可知，当距离超过MAX_DISTANCE时，detect() 调用会获得零返回。且在上坡前可能因为检测到上坡的部分导致提前避障等，因此给超声波传感器设置了able 变量，在合适的时候调用其detect().
本次设计的五自由度机械臂安装于机器人底盘的前方，便于抓取较远处的物块。
由于采用转盘结构存储物块，机械爪只能以倾斜角度抓取，因此机械爪部分在原有机械臂的基础上加装海绵和橡皮筋，海绵使用橡皮筋固定。反复缠绕的橡皮筋在保证了一定容错率的同时，也能够为抓取在中间的物块提供较大的摩擦力（事实上机械臂中间的部分较宽，很难抓紧物块，橡皮筋有弹性，在机械臂合拢的时候能够提供足够的容错率，不至于直接打倒物块无法抓取）。
机械臂的运动控制通过向数字舵机发送指令来实现，通过参数的调整预设了几个位姿参数，储存对应动作的目标状态，不同位置物块的抓取和放置都可以通过相应几个目标状态之间的转换实现，以抓取第二级台阶正中间物块为例，抓取过程机械臂的运动可以简化为：
- 机械臂到达物块上方
forward_up（位姿1） - 开爪
- 下降至
forward_down（位姿2） - 合爪-抬升至
forward_up（位姿1） - 旋转180度到达转盘正上方
back_up（位姿3） - 下降至
back_down（位姿4） - 开爪。
放置过程类似，具体代码实现请见后面部分。
物块存储部分主体由一个转盘组成。转盘有上下两层，中间通过螺柱连接，下层用于托载物块，上层中间镂空成三叶形，用于限位，防止小车运载物块上下坡过程中物块掉落。
为了增加容错率，镂空部分直径略大于物块对角线长度；在此基础上为了增大从转盘上抓取物块的准确度，我们设计了以下几种类型的物块用于粘在下层转盘上：
-
三棱锥形
-
半椭球形上带锥顶
- 半椭球形
- 半椭球形上带半球突起
最终我们选择了半椭球形、中间有球形突起的结构，粘在下层置物板上（见下图），这样**上小下大的结构使得物块底部任一点落在球形突起上，都会因为上层转盘和下层结构的合力矩作用而旋转，最终正落到底层转盘上并保持稳定。**都可以从该机构上滑落，最终物块重心位于圆盘镂空处**，使得物块放置位置在可接受误差范围内精度达到最高。（实际测试过程中发现，偏大的软质镂空上层板可以在放置不准的情况下转动时把物块震落到原定位置）。
此外，为了防止不同“叶片”中的物块相互滑动，在下层转盘**加装6cm高的柱子，柱子也通过3D打印制作，模型见右图。起到分割作用。
转盘原为硬卡纸质，但是为了提高强度，防止被机械臂多次损坏，我们绘制模型采用3D打印方式重制了转盘。由于金工中心3D打印所用材料都带有颜色，且能正常工作的打印机所有材料恰好都为三原色，我们采用蓝色PLA材料打印，为防止转盘的蓝色干扰颜色识别，用记号笔将转盘和突起结构涂成黑色。
此外，在运动过程中，机械臂保持back_down 位姿，覆盖于三个物块之上，起到盖子的作用，可以避免小车在运动过程中由于上下坡的颠簸导致物块从中掉落，同时也使重心降低而不致翻车。
转盘俯拍，下方原应有半椭球状物块，本图展示了物块和底盘的连接方式：胶水连接。
小车在上下坡时的机械臂位姿，防止物块掉落。
在转盘的一侧，使用了助教统一采购的颜色识别传感器，对抓取到的物块进行颜色识别与记忆。在我们的实现中，其detect() 会返回”red” “blue” “green” “unknown” .
右图中转盘边即为颜色传感器。抓取到物体后，转盘转动为下一次抓取腾出空间，同时传感器进行颜色识别。
图中转盘左侧为颜色传感器。
右轮上前部分为颜色传感器。
在机器人的设计中，还加入了CK008 电容式触摸传感器和RGB LED灯。
触摸传感器可以在调试的时候给机器人手动下达指令，方便调试，也能避免一通电小车就开始运动最终从桌面掉落的惨剧发生。在最终的主程序中，也作为启动开关，将舵机等的初始化这一耗时较久的过程移动到比赛计时开始前完成，节省时间。LED灯可以在调试的时候让机器人给予我们十分确定的反馈，进入某个函数时、状态改变时、颜色识别成功时，可以亮起特定颜色的灯光，大大便利了调试过程。
另外，注意到当机械臂抓取右前方物块的时候，由于我们设计的小车底盘较小，车体出现倾斜，几近翻车，因此在车前方伸出两片长板，在抓取、放置的时候，两片长板恰好按在物品放置台上，提供向上弹力，避免翻车。
本项目代码均托管于‣, 其中主程序为robot/robot.ino, 自主编写的库在./library/下.
在为每一个传感器编写cpp库时，我们使用了串口监视器进行调试，这部分调试代码也被保留在仓库中，而这在后面我们排查传感器硬件故障等问题时为我们提供了非常大的帮助，在编写库时，传感器测试代码也可以作为应用场景，帮助我们定义接口并将其实现。
全局变量中的cornerCount 变量用于记录经过T字形路口的次数，当cornerCount 为1时，判定为grasping状态，为2时，判定为placing 状态，在这两个状态之间时，允许超声波传感器返回非0值。
机器人在一开始使用巡线，在部分较脏的场地，使用读秒强制巡线（相当于禁用了其他动作，只做巡线，避免错误的判别），上坡识别到T字黑线后，电机制动刹车，右转后使用严格慢速巡线cruise_slowly_strictly() ，确保停于台前时，位姿基本稳定。（注：经多次调试，发现在转弯前让电机直接制动能够使停止更加稳定，为后面开环控制的精确抓取与放置创造条件。）
转弯并停止于台前后，根据cornerCount的值判定调用block_grabbing() block_placing()函数。在此过程中，由于涉及左转右转等等操作，很可能导致边缘传感器误以为到达下一个T字路口，因此大部分函数中包含了delay() 的调用，确保整个过程在一次loop() 调用内完成，后期调试中使用了边缘白标传感器库中的disable() 函数，使之不致误判。
block_grabbing() : 在运行block_grabbing() 时，机器人重复以下动作：
张手→抬手→手移动至目标物块位置并抓住物块→转盘转动至空位在机器人正后方→讲物块放入空位→若非第一次抓取, 调用颜色传感器进行颜色识别, 若识别结果为红绿蓝之一, LED亮对应的颜色, 将物块在转盘上的位置与该物块颜色建立映射.
block_placing(): 在运行block_placing() 时, 机器人重复以下动作: (伪代码)
if blue block exists:
rotate the plate to the place marked as blue
grab the block
place the block on the topleft place (whose color is blue)
对红色, 绿色的物块采取类似的操作。由于放置物块区域的颜色是固定的，因此只需要识别物块的颜色即可完成准确的放置，此外，如果识别不到三原色，则直接放弃放置动作，节省时间（由于颜色传感器经过测试证明非常可靠，因此识别不到颜色极大概率为抓取时问题）。
在下坡时，由于坡面较陡，机器人容易直接冲出线外，因此在坡上我们使用cruise_down_hill() 函数，这个改进版的巡线参数速度更慢且更加严格，保证机器人一直在线上。
而在避障部分，我们为了机器人能够走得优雅，同时也为了提升其稳定性，并未采取触觉传感器的方案，而是使用超声波传感器，在被障碍物阻挡时提前避障。当超声波传感器看到障碍物时，机器人读秒转弯直行，直行的过程并不读秒，而是直行至灰度传感器看到黑线时，使用巡线使其回到线上。
值得一提的是，在避障到达下一条线时可能出现边缘传感器误判为黑线的情况，在抓取物块时，我们将边缘传感器禁用，在避障结束，强制巡线一段时间后，认为机器人已经在正常巡线状态，此时启用边缘传感器。
机器人回到起点后，round2 置真。但由于我们最终采用的策略是只走一圈，因此对于第二圈的控制并未再做维护，此处不再赘述。
本次项目代码为了方便多人协作，使用git作为版本管理工具，并将代码托管于github （仓库见‣），这对我们保留之前的参数和模型都非常重要。在之前我们对巡线代码做了一次复杂度过高的修改，便使用git回滚至之前的版本，为调试和协作提供了莫大的便利。
本代码中大部分的函数设计都考虑了一般性和抽象化，避免了冗余，如rotate(int angle, Servo* servo) 函数可以用于控制任何一个舵机，对各种传感器库也做了封装。
考虑到项目代码具有小组协作完成的特性，代码的可读性要求较高，基本上大部分代码段都通过合适的变量、函数名选用做了自注释，对于部分比较复杂的函数，也用标准的方式写了注释。
调试过程主要包括模型的修改和参数的调整。参数调整部分不再赘述。在调试过程中，我们遇到的部分问题及对应解决策略如下：
| 机器人在满载情况下难以上坡 | 试图自己制造更大的万向轮（最终弃用）；为上坡部分定制了速度更快的巡线函数。 |
|---|---|
| 机器人避障过程中边缘传感器过于敏感误判黑线 | 调整边缘传感器的阈值；使用disable() 函数在不该触发时禁用边缘传感器 |
| 机器人抓取物块时底盘不稳，几近翻车 | 在车前伸出两个长板，抓取放置时卡住载物台。 |
| 机器人下坡时冲出线外 | 在下坡前控制初速度，为下坡定制一个几乎不加速的cruise_down_hill() |
| 机器人停在台前的位置稳定性差 | 在转弯前先制动，转弯后，使用严格的巡线函数，本部分尽可能减少开环控制比例。 |