MobBob：由 Android 智能手机控制的 DIY Arduino 机器人

必要的工具和机器

	烙铁（通用）
	3D 打印机（通用）

应用和在线服务

Arduino IDE

关于这个项目

我根据网站上的说明制作了这个机器人 Arduino 和 Android 共生的好主意。

MobBob 是一款智能手机控制的机器人。通过利用智能手机的强大功能，MobBob 是一款会走路、会说话的机器人，具有语音识别和计算机视觉功能。

3D 打印零件：https://www.thingiverse.com/thing:715688

Android.apk 文件：https://apkpure.com/mobbob/com.cevinius.MobBob

在代码中，您需要：

- 更新引脚变量以匹配您的构建

- 调整伺服中心、最小值和最大值

- 根据您的髋部伺服系统的安装方式，将“FRONT_JOINT_HIPS”设置为 1 或 -1。我用伺服轴安装在 MobBob 的前面。对于此配置，将此值设置为 1。

代码

• 代码

代码Arduino

/* * ============================================================* MobBob 控制程序 - 软件串行蓝牙版本 * 由 Kevin Chan (aka Cevinius) * ==============================================================* * 本程序允许使用串行命令控制 MobBob。在此版本的代码中，* 命令通过软件串行端口接收，引脚定义在顶部附近的 #define 中。 * 这意味着您可以使用任何 Arduino 兼容板，并将蓝牙卡插入为 * 软件串行设置的引脚。 （与 DFRobot 为 Bluno 板设计的另一个版本相反。） * * 该程序很长，包含 2 个主要组件 - 一个平滑的伺服动画程序和一个串行 * 命令解析器程序。 * * 动画系统 * ================* 动画程序旨在平滑地为伺服关键帧阵列设置动画。代码尽量做到最好，以便于使用。 * * 动画系统只会排队 1 个命令。即一个命令可以运行，*和一个命令可以排队。如果您发送更多命令，它们将覆盖排队的命令。 * * 动画系统默认会等待当前动画完成后再开始下一个。这个 * 意味着如果动画数据以机器人处于基本姿势结束，那么事情会顺利地结合起来。为了*支持这一点，动画系统还有一个功能，动画可以有一个“完成序列”*让机器人回到基本姿势。此功能用于前进/后退动画。 * 这些动画有一个最终序列，可以让机器人回到基本姿势。 * * 动画播放完毕后，动画系统会向串口输出一个响应字符串。 * 这使调用者能够知道他们请求的动画何时播放完毕。这对于用户对动画进行排序很有用 - 在开始另一个之前等待一个动画完成。 * * 动画代码有许多变量可以调整。例如。更新频率、arduino 引脚等 * * 动画数据数组格式也设计为易于手动编辑。 * * 命令解析器 * ==============* 该系统解析通过串行接收到的命令，并处理它们。这些命令包括用于直接*设置伺服位置的命令，以及用于触发预定义动画和行走的命令。 * * 因此，不想担心步行细节的用户可以使用预定义的步行/动画。 * 而且，想要完全控制伺服系统（以动态创建新动画）的用户也可以这样做。 * * 如上所述，这些命令可以从 Arduino 串行监视器交互使用。它们也可以 * 使用蓝牙 LE 发送（当使用 Bluno 时）。手机应用程序将通过蓝牙 LE 将命令发送到 * Bluno。 * * 一般命令：* ----------------- * 就绪/OK 检查： * 状态检查。立即返回响应以检查控制器是否工作。 * * Set Servo: * time - 时间到指定角度补间，0 将立即跳转到角度 * leftHip - 距离中心的微秒。 -ve 臀部向内，+ve 臀部向外 * leftFoot - 从平面开始的微秒。 -ve 脚朝下，+ve 脚朝上 * rightHip - 距中心微秒。 -ve 臀部向内，+ve 臀部向外 * rightFoot - 距平底微秒。 -ve 脚朝下，+ve 脚朝上 * 此命令用于完全控制舵机。您可以在指定的持续时间内将机器人从其当前姿势补间到指定姿势。 * * 停止/重置： * 在当前动画结束后停止机器人。可用于停止设置为无限循环 * 的动画。这也可用于将机器人置于其基本姿势（直立） * * 立即停止： * 立即停止机器人，无需等待完成当前动画。这 * 中断机器人当前的动画。机器人可能处于动画中期 * 并处于不稳定的姿势，因此在使用时要小心。 * * 标准步行命令：* ----------------------- * 前进：，-1 表示连续，0 或无参数与1次相同。 * Backward:, -1 表示连续，0 或无参数与 1 次相同。 * 向左转：，-1 表示连续，0 或无参数与 1 次相同。 * 右转：，-1 表示连续，0 或无参数与 1 次相同。 * * 有趣的动画命令：* ----------------------- * 摇头：，-1 表示连续，0 或无参数与1次相同。 * * Bounce:, -1 表示连续，0 或无参数与 1 次相同。 * * Wobble:, -1 表示连续，0 或无参数与 1 次相同。 * Wobble Left:, -1 表示连续，0 或无参数与 1 次相同。 * Wobble Right:, -1 表示连续，0 或无参数与 1 次相同。 * * Tap Feet：，-1 表示连续，0 或无参数与 1 次相同。 * 点击左脚：，-1 表示连续，0 或无参数与 1 次相同。 * 点击右脚：，-1 表示连续，0 或无参数与 1 次相同。 * * Shake Legs:, -1 表示连续，0 或无参数与 1 次相同。 * Shake Left Leg:, -1 表示连续，0 或无参数与 1 次相同。 * Shake Right Leg:, -1 表示连续，0 或无参数与 1 次相同。 * * 此外，当命令完成 * 执行时，代码将通过串行发送一个响应字符串。 * * 如果命令正常完成，则响应字符串为不带 * 参数的命令代码。例如。当它完成前进时，它将发送响应“”。 * * 如果命令被  中断，则当前动画可能已中途停止。 * 在这种情况下，机器人可能处于奇怪的中途姿势，并且可能没有 * 播放完成动画。为了让用户知道这已经发生，响应字符串将包含 * 参数 -1。例如，如果使用  中途停止步行，则响应字符串将是 *  表示步行已停止，但在中途停止。 * （注意：如果你使用  停止，那将等待当前动画循环完成 * 在停止之前。所以，在这种情况下，动画不会中途停止。） * * 因为响应是在一个动画完成后，命令发送者可以 * 查找响应字符串以确定机器人何时准备好接收新命令。 * 例如如果您使用命令 ，则在所有 3 个步骤 *（和完成动画）完成之前不会发送响应字符串。所以，命令发送者可以在告诉机器人做下一件事情之前等待响应 * 字符串。 */ #include #include //-------------------------------- -------------------------------------------------- // 串行通信速度 - 为您的串行（蓝牙）卡设置此项。//---------------------------------------- -------------------------------------------------- -// 与蓝牙板的串行通信速度。// 有些板默认为 9600。我的板的默认值为 115200。#define SERIAL_SPEED 115200 // 在这些引脚上设置软件串行端口。const int rxPin =11; // 用于接收 dataconst 的引脚 int txPin =12; // 用于发送数据的引脚SoftwareSerial softwareSerial(rxPin, txPin);//--------------------------------- ------------------------------------------------//设置 Arduino 引脚 - 为您的特定机器人设置这些引脚。//------------------------------------ ---------------------------------------------const int SERVO_LEFT_HIP =5;const int SERVO_LEFT_FOOT =2;const int SERVO_RIGHT_HIP =3;const int SERVO_RIGHT_FOOT =4;// 我希望此代码可用于所有 4 伺服 Biped！ （比如 Bob、MobBob）// 我注意到有些版本以不同的方式安装臀部伺服器 // 与我做 MobBob 的方式不同，因此，此设置允许您为任一构建样式配置代码。// 1 表示 MobBob 样式前向臀部（关节向前）// -1 为 Bob 式后向臀部（关节向后）#define FRONT_JOINT_HIPS 1//------------------- -------------------------------------------------- -------------//伺服最大/最小/中心常数 - 为您的特定机器人设置这些。//----------------- -------------------------------------------------- --------------const int LEFT_HIP_CENTRE =1580;const int LEFT_HIP_MIN =LEFT_HIP_CENTRE - 500;const int LEFT_HIP_MAX =LEFT_HIP_CENTRE + 500;const int LEFT_FOOT_CENTRE =14_FOT_CENTRE =14 _const _REFT_00000FT const int LEFT_FOOT_MAX =LEFT_FOOT_CENTRE + 500;const int RIGHT_HIP_CENTRE =1500;const int RIGHT_HIP_MIN =RIGHT_HIP_CENTRE - 500;const int RIGHT_HIP_MAX =RIGHT_HIP5_CENTRE =RIGHT_HIP5_CENTRE 465;const int RIGHT_FOOT_MIN =RIGHT_FOOT_CENTRE - 500;const int RIGHT_FOOT_MAX =RIGHT_FOOT_CENTRE + 500;//------------------------------ ------------------------------------------------//帮助函数以更友好的方式帮助计算关节值。// 如果舵机以不同的方式设置，您可以在此处调整标志。// 在此处更新意味着如果// 舵机的设置不同。//（例如原来 Bob 的臀部舵机是向后到 MobBob 的。）//// （另外，我发现很难记住每个舵机使用的标志，因为它们 // 左/右臀部和左/右脚不同。） //------------------------------------------------ ------------------------------int LeftHipCentre() { return LEFT_HIP_CENTRE; }int LeftHipIn(int millisecs) { return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * 毫秒); }int LeftHipOut(int millisecs) { return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * 毫秒); }int RightHipCentre() { 返回 RIGHT_HIP_CENTRE; }int RightHipIn(int millisecs) { return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * 毫秒); }int RightHipOut(int millisecs) { return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * 毫秒); }int LeftFootFlat() { 返回 LEFT_FOOT_CENTRE; }int LeftFootUp(int millisecs) { return LEFT_FOOT_CENTRE - 毫秒； }int LeftFootDown（int 毫秒）{ 返回 LEFT_FOOT_CENTRE + 毫秒； }int RightFootFlat() { 返回 RIGHT_FOOT_CENTRE; }int RightFootUp(int millisecs) { return RIGHT_FOOT_CENTRE + 毫秒； }int RightFootDown(int millisecs) { return RIGHT_FOOT_CENTRE - 毫秒； }//---------------------------------------------- -----------------------------------// 标准步行步态和其他伺服动画的关键帧动画数据// // 格式为 { , , , ,  }// 毫秒 - 补间到此关键帧位置的时间。例如。 500 表示从 // 机器人在此帧开始时的位置到此帧中指定的位置需要 500 毫秒 // leftHipMicros - 以伺服微秒为单位的左臀部位置。// leftFootMicros - 伺服中左臀部的位置microsecs.// rightHipMicros - 以伺服微秒为单位的左臀部位置。// rightFootMicros - 以伺服微秒为单位的左臀部位置。// // 伺服微值，支持特殊值 -1。如果给出了这个值，它会告诉//动画代码在这个关键帧中忽略这个伺服。即该伺服器将//保持在该关键帧开始时的位置。////此外，动画数据中的第一个元素是特殊的。它是一个元数据元素。// 第一个元素是 { , 0, 0, 0, 0 }，它告诉我们动画中的帧数//。因此，第一个实际关键帧在 animData[1] 中，最后一个关键帧// 在 animData[] 中。 （其中  是 animData[0][0] 中的值。）//---------------------------- -------------------------------------------------- ---// 使访问关键帧数组更易读的常量。const int TWEEN_TIME_VALUE =0;const int LEFT_HIP_VALUE =1;const int LEFT_FOOT_VALUE =2;const int RIGHT_HIP_VALUE =3;const int RIGHT_FOOT_VALUE =4 used;// Constant int LEFT_HIP_VALUE =1在行走步态动画 data.const int FOOT_DELTA =150;const int HIP_DELTA =FRONT_JOINT_HIPS * 120;// 转到默认的直立姿势。由 stopAnim().int standStraightAnim[][5] ={ // 元数据使用。第一个元素是帧数。 { 1, 0, 0, 0, 0 }, // Feet flat, Feet even { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};// 在此之前，让机器人脚平，脚均匀（即standStraightAnim）。int walkForwardAnim[][5] ={ // 元数据。第一个元素是帧数。 { 8, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右脚forward { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 脚平，右脚向前 { 300, LeftHipIn(HIP_DELTA), LeftFootFlatOut(), Right_H RightFootFlat() }, // 向右倾斜，右脚向前 { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // 向右倾斜，双脚均匀 { 300, LeftHipCentre (), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左脚向前{ 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp() , // 脚平，左脚向前 { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() }, // 向左倾斜，左脚向前 { 300, LeftHipOut(HIP_DEL) TA)、LeftFootUp(FOOT_DELTA)、RightHipIn(HIP_DELTA)、RightFootDown(FOOT_DELTA) }};// 在此之前，让机器人平足、平足（即standStraightAnim).int walkBackwardAnim[][5] ={ // 元数据。第一个元素是帧数。 { 8, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，左脚forward { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 双脚平放，左脚向前 { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), Right_HIPH RightFootFlat() }, // 向右倾斜，左脚向前 { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // 向右倾斜，双脚均匀 { 300, LeftHipCentre (), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，右脚向前{ 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp() , // 脚平，右脚向前 { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() }, // 向左倾斜，右脚向前 { 300, LeftHipIn(HIP_DELTA) ), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }};// 完成行走动画将机器人从walkForwardAnim/walkBackwardAnim 的末端带回standStraightAnim.int walkEndAnim[][5] ={ // 元数据.第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 脚平，脚均匀 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};// 在此之前，让机器人达到Feet Flat, Feet Even（即standStraightAnim）。int turnLeftAnim[][5] ={ / / 元数据。第一个元素是帧数。 { 6, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，向左转hip, 右转臀部 { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 脚平，左转臀部，右转臀部 { 300，LeftHipIn(HIP_DELTA), LeftFootFTlat (), RightHipIn(HIP_DELTA), RightFootFlat() }, // 向右倾斜，转左臀部，转右臀部 { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // 向右倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 脚平，脚均匀 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre ), RightFootFlat() }};// 在此之前，让机器人达到 Feet Flat, Feet Even（即 standStraightAnim).int turnRightAnim[][5] ={ // Metadata.第一个元素是帧数。 { 6, 0, 0, 0, 0 }, // 向右倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，向左转hip, 右转臀部 { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // 脚平，左转臀部，右转臀部 { 300, LeftHipIn(HIP_DELTA), LeftFoot (), RightHipIn(HIP_DELTA), RightFootFlat() }, // 向左倾斜，转左臀部，转右臀部 { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 脚平，脚均匀 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre ), RightFootFlat() }};// 摇头动画。左右快速模拟摇头。int shapeHeadAnim[][5] ={ // Metadata.第一个元素是帧数。 { 4, 0, 0, 0, 0 }, // 脚平，向左扭转 { 150, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() }, // 脚平，脚均匀 { 150 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, // 脚平放，向右扭转 { 150, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() }, // 脚flat, Feet even { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// 摆动动画。左右倾斜做一个有趣的 wobble.int wobbleAnim[][5] ={ // Metadata.第一个元素是帧数。 { 4, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 脚平，脚均匀 { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, // 向右倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // flat, Feet even { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// 摆动左动画。向左和向后倾斜。int wobbleLeftAnim[][5] ={ // 元数据。第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 脚平，脚均匀 { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// 摆动右侧动画。向右和向后倾斜。int wobbleRightAnim[][5] ={ // 元数据。第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 向右倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 脚平，脚均匀 { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// 点击脚动画。 Tap both feet.int tapFeetAnim[][5] ={ // 元数据。第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 双脚抬起，脚均匀 { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 脚平，脚均匀 { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// 点击左脚 anim.int tapLeftFootAnim[][5] ={ // Metadata.第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 抬起左脚，双脚均匀 { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootFlat() }, // 双脚平放，双脚均匀 { 500 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// 点击右脚 anim.int tapRightFootAnim[][5] ={ // Metadata.第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 抬起右脚，脚均匀 { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 脚平，脚均匀 { 500 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// 上下弹跳 anim.int BounceAnim[][5] ={ // 元数据。第一个元素是帧数。 { 2, 0, 0, 0, 0 }, // 双脚抬起，双脚均匀 { 500, LeftHipCentre(), LeftFootDown(300), RightHipCentre(), RightFootDown(300) }, // 双脚平放，双脚均匀 { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// 摇腿 Animation.int ShakeLegsAnim[][5] ={ // 元数据。第一个元素是帧数。 { 14, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部在 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，双脚均匀{ 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightHipCentre() , // 向左倾斜，右臀部向外 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootUp(FOOT_D) , RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部在 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，脚均匀{ 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 脚平，脚均匀 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat()倾斜钻机ht, Feet even { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部在 { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), Right , RightFootUp(FOOT_DELTA) }, // 向右倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部向外 { 100, LeftHipOut(HIP_DELTA) ), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，脚均匀 { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) } , Feet even { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// 晃动左腿 Animation.int ShakeLeftLegAnim[][5] ={ // 元数据。第一个元素是帧数。 { 12, 0, 0, 0, 0 }, // 向右倾斜，双脚均匀 { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部在 { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，脚均匀{ 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightHipCentre() , // 向右倾斜，左臀部向外 { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootDown(FOOT_D) , RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部在 { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，脚均匀{ 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部向外{ 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre() otUp(FOOT_DELTA) }, // 向右倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，左臀部在 { 100, LeftHipIn(HIP_DELTA) , LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // 向右倾斜，脚均匀 { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA), //脚均匀 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// 晃动右腿 Animation.int ShakeRightLegAnim[][5] ={ // 元数据。第一个元素是帧数。 { 12, 0, 0, 0, 0 }, // 向左倾斜，双脚均匀 { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部在 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，双脚均匀{ 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightHipCentre() , // 向左倾斜，右臀部向外 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootUp(FOOT_D) , RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部在 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，脚均匀{ 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部向外{ 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA) (FOOT_DELTA) }, // 向左倾斜，双脚均匀 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // 向左倾斜，右臀部在 { 100, LeftHipCentre(), LeftFootUp (FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // 向左倾斜，脚均匀 { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA), Feet flat甚至 { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, };//------------------------ -------------------------------------------------- -------// 用于设置/补间伺服位置的特殊动态动画数据。//---------------------------- -------------------------------------------------- ----// 这些是我们用于 SetServos() 函数的 2 个特殊动画数据。他们有//一个单一的框架。这些将更改这些动画数据中的数据并播放它们以 // 移动伺服器.int setServosAnim1[][5] ={ // 元数据。第一个元素是帧数。 { 1, 0, 0, 0, 0 }, // 向左倾斜，脚均匀 { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};int setServosAnim2[][5] ={ / / 元数据。 First element is number of frames. { 1, 0, 0, 0, 0 }, // Tilt left, Feet even { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};//----------------------------------------------------------------------------------// Servo Variables//----------------------------------------------------------------------------------Servo servoLeftHip;Servo servoLeftFoot;Servo servoRightHip;Servo servoRightFoot;//----------------------------------------------------------------------------------// State variables for playing animations.//----------------------------------------------------------------------------------// Milliseconds between animation updates.const int millisBetweenAnimUpdate =20;// Time when we did the last animation update.long timeAtLastAnimUpdate;// Related to currently playing anim.int (*currAnim)[5]; // Current animation we're playing.int (*finishAnim)[5]; // Animation to play when the currAnim finishes or is stopped.long timeAtStartOfFrame; // millis() at last keyframe - frame we're lerping fromint targetFrame; // Frame we are lerping toint animNumLoops; // Number of times to play the animation. -1 means loop forever.char animCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".// Related to anim queue. I.e. Next anim to play.bool animInProgress; // Whether an animation is playingint (*nextAnim)[5]; // This is the next animation to play once the current one is done. // i.e. It's like a queue of size 1! // If curr is non-looping, we play this at the end of the current anim. // If curr is looping, this starts at the end of the current loop, // replacing curr anim. // If nothing is playing, this starts right away. int (*nextFinishAnim)[5]; // This is the finish animation for the queued animation.int nextAnimNumLoops; // Number of times to play the animation. -1 means loop forever.char nextAnimCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".bool interruptInProgressAnim; // Whether to change anim immediately, interrupting the current one.// Curr servo positionsint currLeftHip;int currLeftFoot;int currRightHip;int currRightFoot;// Servo positions at start of current keyframeint startLeftHip;int startLeftFoot;int startRightHip;int startRightFoot;//-------------------------------------------------------------------------------// Parser Variables//-------------------------------------------------------------------------------// Constant delimiter tag charsconst char START_CHAR ='<';const char END_CHAR ='>';const char SEP_CHAR =',';// Constants and a variable for the parser state.const int PARSER_WAITING =0; // Waiting for '<' to start parsing.const int PARSER_COMMAND =1; // Reading the command string.const int PARSER_PARAM1 =2; // Reading param 1.const int PARSER_PARAM2 =3; // Reading param 2.const int PARSER_PARAM3 =4; // Reading param 3.const int PARSER_PARAM4 =5; // Reading param 3.const int PARSER_PARAM5 =6; // Reading param 3.const int PARSER_EXECUTE =7; // Finished parsing a command, so execute it.// Current parser state.int currParserState =PARSER_WAITING; // String for storing the command. 2 chars for the command and 1 char for '\0'.// We store the command here as we're parsing.char currCmd[3] ="--";// For tracking which letter we are in the command.int currCmdIndex;// Max command length.const int CMD_LENGTH =2;// Current param values. Store them here after we parse them.int currParam1Val;int currParam2Val;int currParam3Val;int currParam4Val;int currParam5Val;// Variable for tracking which digit we're parsing in a param.// We use this to convert the single digits back into a decimal value.int currParamIndex;// Whether the current param is negative.boolean currParamNegative;// Max parameter length. Stop parsing if it exceeds this.const int MAX_PARAM_LENGTH =6;//===============================================================================// Arduino setup() and loop().//===============================================================================void setup() { // Setup the main serial port softwareSerial.begin(SERIAL_SPEED); // Setup the Servos servoLeftHip.attach( SERVO_LEFT_HIP, LEFT_HIP_MIN, LEFT_HIP_MAX); servoLeftFoot.attach( SERVO_LEFT_FOOT, LEFT_FOOT_MIN, LEFT_FOOT_MAX); servoRightHip.attach( SERVO_RIGHT_HIP, RIGHT_HIP_MIN, RIGHT_HIP_MAX); servoRightFoot.attach(SERVO_RIGHT_FOOT, RIGHT_FOOT_MIN, RIGHT_FOOT_MAX); // Set things up for the parser. setup_Parser(); // Set things up for the animation code. setup_Animation();}void loop() { // Update the parser. loop_Parser(); // Update the animation. loop_Animation();}//===============================================================================// Related to the parser//===============================================================================// Sets up the parser stuff. Called in setup(). Should not be called elsewhere.void setup_Parser(){ // Wait for first command. currParserState =PARSER_WAITING; // Print this response to say we've booted and are ready. softwareSerial.println("");}// Loop() for the parser stuff. Called in loop(). Should not be called elsewhere.void loop_Parser(){ //--------------------------------------------------------- // PARSER // // If there is data, parse it and process it. //--------------------------------------------------------- // Read from pin serial port and write it out on USB port. if (softwareSerial.available()> 0) { char c =softwareSerial.read(); // If we're in WAITING state, look for the START_CHAR. if (currParserState ==PARSER_WAITING) { // If it's the START_CHAR, move out of this state... if (c ==START_CHAR) { // Start parsing the command. currParserState =PARSER_COMMAND; // Reset thing ready for parsing currCmdIndex =0; currCmd[0] ='-'; currCmd[1] ='-'; currParam1Val =0; currParam2Val =0; currParam3Val =0; currParam4Val =0; currParam5Val =0; } // Otherwise, stay in this state. } // In the state to look for the command. else if (currParserState ==PARSER_COMMAND) { // Else if it's a separator, parse parameter 1. But make sure it's not // empty, or else it's a parse error. if (c ==SEP_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_PARAM1; currParamIndex =0; currParamNegative =false; } else { currParserState =PARSER_WAITING; } } // Else if it's the end char, there are no parameters, so we're ready to // process. But make sure it's not empty. Otherwise, it's a parse error. else if (c ==END_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_EXECUTE; } else { currParserState =PARSER_WAITING; } } // If we've got too many letters here, we have a parse error, // so abandon and go back to PARSER_WAITING else if ( (currCmdIndex>=CMD_LENGTH) || (c <'A') || (c> 'Z') ) { currParserState =PARSER_WAITING; } // Store the current character. else { currCmd[currCmdIndex] =c; currCmdIndex++; } } // In the state to parse param 1. else if (currParserState ==PARSER_PARAM1) { // Else if it's a separator, parse parameter 1. if (c ==SEP_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_PARAM2; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam1Val =(currParam1Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 2. else if (currParserState ==PARSER_PARAM2) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_PARAM3; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam2Val =(currParam2Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 3. else if (currParserState ==PARSER_PARAM3) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_PARAM4; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam3Val =(currParam3Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 4. else if (currParserState ==PARSER_PARAM4) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_PARAM5; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam4Val =(currParam4Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 5. else if (currParserState ==PARSER_PARAM5) { // If it's the end char, there are no parameters, so we're ready to // process. if (c ==END_CHAR) { if (currParamNegative) { currParam5Val =-1 * currParam5Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam5Val =(currParam5Val * 10) + currDigitVal; currParamIndex++; } } //--------------------------------------------------------- // PARSER CODE HANDLER (Still part of Parser, but section that // processes completed commands) // // If the most recently read char completes a command, // then process the command, and clear the state to // go back to looking for a new command. // // The parsed items are stored in:// currCmd, currParam1Val, currParam2Val, currParam3Val, // currParam4Val, currParam5Val //--------------------------------------------------------- if (currParserState ==PARSER_EXECUTE) { // Ready/OK Check: if ((currCmd[0] =='O') &&(currCmd[1] =='K')) { softwareSerial.println(""); } // Set Servo: // time - time to tween to specified angles // leftHip - microsecs from centre. -ve is hip in, +ve is hip out // leftFoot - microsecs from flat. -ve is foot down, +ve is foot up // rightHip - microsecs from centre. -ve is hip in, +ve is hip out // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up else if ((currCmd[0] =='S') &&(currCmd[1] =='V')) { int tweenTime =currParam1Val; if (currParam1Val <0) { tweenTime =0; } SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV"); } // Stop/Reset:, Stops current anim. Also can be used to put robot into reset position. else if ((currCmd[0] =='S') &&(currCmd[1] =='T')) { StopAnim("ST"); } // Stop Immediate: else if ((currCmd[0] =='S') &&(currCmd[1] =='I')) { StopAnimImmediate("SI"); } // Forward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='F') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW"); } // Backward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW"); } // Turn Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT"); } // Turn Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='R') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT"); } // Shake Head:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='S') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX"); } // Bounce:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX"); } // Wobble:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX"); } // Wobble Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY"); } // Wobble Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ"); } // Tap Feet:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX"); } // Tap Left Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY"); } // Tap Right Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ"); } // Shake Legs:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX"); } // Shake Left Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY"); } // Shake Right Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ"); } //-------------------------------------------------- // Clear the state and wait for the next command! // This must be done! //-------------------------------------------------- currParserState =PARSER_WAITING; } }}//===============================================================================// Related to playing servo animations.//===============================================================================// Call this to play the given animation once. Pass in NULL if there is no finishAnim.void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);}// Call this to loop the given animation. Pass in NULL if there is no finishAnim.void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);}// Call this to play the given animation the specified number of times. // -1 number of times will make it loop forever.// Pass in NULL if there is no finishAnim.void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr){ // Put this in the queue. nextAnim =animToPlay; nextFinishAnim =finishAnim; nextAnimNumLoops =numTimes; // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; }}// Stop after the current animation.void StopAnim(const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Stop immediately and lerp robot to zero position, interrupting // any animation that is in progress.void StopAnimImmediate(const char *completeStr){ // Put this in the queue. interruptInProgressAnim =true; PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,// it'll tween it over a specified time.// For positions, 0 means centered.// For hips, -ve is hip left, +ve is hip right// For feet, -ve is foot down, +ve is foot upvoid SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr){ // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; } // Decide which tween data we use. We don't want to over-write the one that is // in progress. We have and reuse these to keep memory allocation fixed. int (*tweenServoData)[5]; if (currAnim !=setServosAnim1) { tweenServoData =setServosAnim1; } else { tweenServoData =setServosAnim2; } // Set the tween information into the animation data. tweenServoData[1][TWEEN_TIME_VALUE] =tweenTime; tweenServoData[1][LEFT_HIP_VALUE] =LeftHipIn(leftHip); tweenServoData[1][LEFT_FOOT_VALUE] =LeftFootUp(leftFoot); tweenServoData[1][RIGHT_HIP_VALUE] =RightHipIn(rightHip); tweenServoData[1][RIGHT_FOOT_VALUE] =RightFootUp(rightFoot); // Queue this tween to be played next. PlayAnim(tweenServoData, NULL, completeStr);}// Set up variables for animation. This is called in setup(). Should be not called by anywhere else.void setup_Animation(){ // Set the servos to the feet flat, feet even position. currLeftHip =LEFT_HIP_CENTRE; currLeftFoot =LEFT_FOOT_CENTRE; currRightHip =RIGHT_HIP_CENTRE; currRightFoot =RIGHT_FOOT_CENTRE; UpdateServos(); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // No animation is playing yet, and nothing in the queue yet. timeAtLastAnimUpdate =millis(); animInProgress =false; interruptInProgressAnim =false; currAnim =NULL; finishAnim =NULL; nextAnim =NULL; nextFinishAnim =NULL;}// Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.//// NOTE:The way looping animations work is that they basically add themselves back to the queue// when a cycle is done, and if there's nothing already queued up! This way, looping animations// work in a similar way to single-play animations, and fits into the queueing system.void loop_Animation(){ // Get the time at the start of this frame. long currTime =millis(); //-------------------------------------------------------------------------------------- // Decide if we want to perform the animation update. We don't execute this every frame. //-------------------------------------------------------------------------------------- if (timeAtLastAnimUpdate + millisBetweenAnimUpdate> currTime) { // Not yet time to do an anim update, so jump out.返回; } else { // We reset the timer, and then proceed below to handle the current anim update. timeAtLastAnimUpdate =currTime; } //-------------------------------------------------------------------------------------- // Decide if we need to setup and start a new animation. We do if there's no anim // playing or we've been asked to interrupt the anim. //-------------------------------------------------------------------------------------- if ( (nextAnim !=NULL) &&(!animInProgress || interruptInProgressAnim) ) { // If this was an interrupt, we also set the "start" servo positions // to the current ones. This way, the animation system will tween from the // current positions. if (interruptInProgressAnim) { // This is the place to notify someone of an animation finishing after getting interrupted // Print the command string we just finished. -1 parameter indicates it was interrupted. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(",-1>"); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // We've handled any interrupt request, so clear the flag. interruptInProgressAnim =false; } // Store the animation we are now playing. currAnim =nextAnim; finishAnim =nextFinishAnim; animCompleteStr[0] =nextAnimCompleteStr[0]; animCompleteStr[1] =nextAnimCompleteStr[1]; nextAnim =NULL; // Queue is cleared. nextFinishAnim =NULL; nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; // Record the number of times to play the animation. animNumLoops =nextAnimNumLoops; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } //-------------------------------------------------------------------------------------- // If we are currently playing an animation, then update the animation state and the // servo positions. //-------------------------------------------------------------------------------------- if (animInProgress) { // Determine if we need to switch to the next frame. int timeInCurrFrame =currTime - timeAtStartOfFrame; if (timeInCurrFrame> currAnim[targetFrame][TWEEN_TIME_VALUE]) { // Set the servo positions to the targetFrame's values. // We only set this if the value is> 0. -ve values means that // the current target keyframe did not alter that servos position. if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =currAnim[targetFrame][LEFT_HIP_VALUE]; } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =currAnim[targetFrame][LEFT_FOOT_VALUE]; } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =currAnim[targetFrame][RIGHT_HIP_VALUE]; } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =currAnim[targetFrame][RIGHT_FOOT_VALUE]; } UpdateServos(); // These current values are now the start of frame values. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // Now, we try to move to the next frame. // - If there is a next frame, set that as the new target, and proceed. // - If there's no next frame, but it's looping, we re-add this animation // to the queue. // - If there's no next frame, and this is not looping, we stop animating. // (Remember that targetFrame is 1-based since the first element of the animation // data array is metadata) // Increment targetFrame, and reset time in the current frame. targetFrame++; timeAtStartOfFrame =currTime; // If there is no next frame, we stop this current animation. // If it is looping, then we re-queue the current animation if the queue is empty. if (targetFrame> NumOfFrames(currAnim)) { // Stop the current animation. animInProgress =false; // If we're looping forever, and there's no next anim, re-queue the // animation if the queue is empty. if ((animNumLoops <0) &&(nextAnim ==NULL)) { LoopAnim(currAnim, finishAnim, animCompleteStr); } // If we're looping forever, and there is something in the queue, then // finish the animation and proceed. else if ((animNumLoops <0) &&(nextAnim !=NULL)) { if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } else { // We've stopped, so can notify if needed. // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } // If we're looping a limited number of times, and there's no next anim, // re-queue the animation if the queue is empty. else if ((animNumLoops> 1) &&(nextAnim ==NULL)) { PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr); } // In this case, numAnimLoops is 1, this is the last loop through, so // we're done. We play the finishAnim first if needed. else { // If there is a finish animation, switch to that animation. if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } // Otherwise, we're done! We've played the finishAnim if there was one. else { // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } } } // If we're still animating (i.e. the previous check didn't find that // we've finished the current animation), then proceed. if (animInProgress) { // Set the servos per data in the current frame. We only update the servos that have target // microsecond values> 0. This is to support the feature where we leave a servo at its // existing position if an animation data item is -1. float frameTimeFraction =(currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]); if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction); } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =startRightHip + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction); } UpdateServos(); } }}// Move all the servo to the positions set in the curr... variables.// In the code, we update those variables and then call this to set the servos.void UpdateServos(){ servoLeftHip.writeMicroseconds(currLeftHip); servoLeftFoot.writeMicroseconds(currLeftFoot); servoRightHip.writeMicroseconds(currRightHip); servoRightFoot.writeMicroseconds(currRightFoot);}// Return the number of frames in the given animation data.// Have this helper function to avoid the "magic number" reference of animData[0][0].int NumOfFrames(int animData[][5]){ return animData[0][0];}

示意图