Merge pull request #27 from sogwms/master

v010

LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

README.md

@@ -1,90 +1,56 @@
-## RT-Robot
+# RT-Robot
 
 ## 1、介绍
 
-RT-Robot 是 RT-Thread 的机器人框架，希望能够支持智能小车、机械臂、飞控等各种不同类型的机器人。
+RT-Robot 是 RT-Thread 的机器人框架，希望能够支持智能小车、机械臂、无人机等各种不同类型的机器人。
 
 当前以智能车为主要目标，希望支持两轮差分驱动、四轮差分驱动、麦克纳姆轮驱动、经典 Ackerman (两轮差分，一方向连杆) 的小车底盘。
 
-### 1.1 目录结构
+当前功能特点：
+- 支持两轮差分驱动、四轮差分驱动、麦克纳姆轮驱动的小车底盘
 
-`rt-robot` 软件包目录结构如下所示：
+- 支持增量、位置式 PID
+- 支持单相、AB 相编码器
+- 支持双 PWM、单 PWM 的直流电机驱动，支持驱动舵机
+- 支持 PS2 遥控器
+- 支持 ANO_TC 匿名科创地面站
 
-```
-rt-robot
-│
-├─docs
-│      design.md            // 模块设计文档
-│      api.md               // API 说明文档
-│      samples.md           // 框架应用示例
-│
-├─controller
-│      controller.c         // 抽象控制器组件
-│      controller.h
-│      ps2_controller.c     // PS2 手柄模块
-│      ps2_controller.h
-│
-├─robot
-│      robot.c              // 抽象机器人组件
-│      robot.h
-│      mobile_robot.c       // 小车机器人模块
-│      mobile_robot.h
-│
-├─chassis
-│      chassis.c            // 小车底盘组件
-│      chassis.h
-│
-├─kinematics
-│      kinematics.c         // 小车动力学模型组件
-│      kinematics.h
-│
-├─wheel
-│      wheel.c              // 小车车轮组件
-│      wheel.h
-│
-├─encoder
-│      encoder.c            // 车轮编码器组件
-│      encoder.h
-│
-├─motor
-│      motor.c              // 车轮电机组件
-│      motor.h
-│      dc_motor.c           // 直流电机模块
-│      dc_motor.h
-│
-├─pid
-│      pid.c                // 车轮 PID 组件
-│      pid.h
-│
-├─README.md                 // 软件包使用说明
-│
-└─SConscript                // RT-Thread 默认的构建脚本
-```
+### 1.1 许可证
 
-### 1.2 依赖
+rt-robot 软件包遵循 Apache-2.0 许可，详见 LICENSE 文件。
 
+### 1.2 依赖
 - RT-Thread 3.1.x +
 
 ## 2、获取软件包
 
-使用 `rt-robot` 软件包需要在 BSP 目录下使用 menuconfig 命令打开 Env 配置界面，在 `RT-Thread online packages → system packages` 中选择 RT-Robot 软件包，操作界面如下图所示：
+使用 `rt-robot` 软件包需要在 BSP 目录下使用 menuconfig 命令打开 Env 配置界面，在 `RT-Thread online packages → system packages` 中选择 RT-Robot 软件包，具体路径如下：
 
-[![选中 RT-Robot 软件包](docs/figures/rt-robot-menuconfig.png)](https://github.com/RT-Thread-packages/paho-mqtt/blob/master/docs/figures/select_mqtt_package.png)
+```shell
+RT-Thread online packages
+    system packages  --->
+         [*] RT-Robot: RT-Thread robots development platform  --->
+            Version (latest) --->
+```
 
-选择合适的配置项后，使用 `pkgs --update` 命令下载软件包并添加到工程中即可。
+配置完成后让 RT-Thread 的包管理器自动更新，或者使用 pkgs --update 命令更新包到 BSP 中。
 
 ## 3、使用 rt-robot
 
-- 框架设计说明，请参考 [设计手册](docs/design.md)。
-- 完整的 API 文档，请参考 [API 手册](docs/api.md)。
-- 详细的示例介绍，请参考 [示例文档](docs/samples.md) 。
+- 软件包详细介绍，请参考 [软件包介绍](docs/introduction.md)
 
-## 4、注意事项
+- 如何从零开始使用，请参考 [用户指南](docs/user-guide.md)
+- 框架设计说明，请参考 [设计手册](docs/design.md)
+- 完整的 API 文档，请参考 [API 手册](docs/api.md)
+- 详细的示例介绍，请参考 [示例文档](docs/samples.md)
 
-- 当前框架正在逐步实现，各种类型的小车还在一步步测试，欢迎大家提 PR 一起完善
+## 4、注意事项
 
+- 当前框架正在逐步完善，欢迎大家来提 PR
 
 ## 5、联系方式 & 感谢
 
-- 维护：RT-Thread 开发团队
-- 主页：https://github.com/RT-Thread-packages/rt-robot
+| 维护     | 主页       | 邮箱  | 
+| -------- | ---------- | --- | 
+| Wu Han | http://wuhanstudio.cc   | wuhanstudio@hust.edu.cn |
+| sogwms | https://github.com/sogwms | sogwyms@gmail.com |

chassis/chassis.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ * 2019-08-26     sogwms       Add move api
+ */
 #include "chassis.h"
 
 #define DBG_SECTION_NAME  "chassis"
@@ -20,8 +30,10 @@ chassis_t chassis_create(wheel_t* c_wheels, kinematics_t c_kinematics)
     return new_chassis;
 }
 
-void chassis_destroy(chassis_t chas)
+rt_err_t chassis_destroy(chassis_t chas)
 {
+    RT_ASSERT(chas != RT_NULL);
+
     for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
     {
         LOG_I("Free wheel %d", i);
@@ -30,10 +42,14 @@ void chassis_destroy(chassis_t chas)
     kinematics_destroy(chas->c_kinematics);
 
     rt_free(chas);
+
+    return RT_EOK;
 }
 
 rt_err_t chassis_enable(chassis_t chas)
 {
+    RT_ASSERT(chas != RT_NULL);
+
     for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
     {
         LOG_I("Enabling wheel %d", i);
@@ -45,20 +61,35 @@ rt_err_t chassis_enable(chassis_t chas)
 
 rt_err_t chassis_disable(chassis_t chas)
 {
-    // TODO
+    RT_ASSERT(chas != RT_NULL);
+
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        LOG_I("Disabling wheel %d", i);
+        wheel_disable(chas->c_wheels[i]);
+    }
 
     return RT_EOK;
 }
 
 rt_err_t chassis_reset(chassis_t chas)
 {
-    // TODO
+    RT_ASSERT(chas != RT_NULL);
+
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        LOG_I("Reset wheel %d", i);
+        wheel_reset(chas->c_wheels[i]);
+    }
+    kinematics_reset(chas->c_kinematics);
 
     return RT_EOK;
 }
 
 rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
 {
+    RT_ASSERT(chas != RT_NULL);
+
     rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
     chassis_set_rpm(chas, res_rpm);
 
@@ -67,6 +98,8 @@ rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
 
 rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[])
 {
+    RT_ASSERT(chas != RT_NULL);
+
     // Set new speed
     for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
     {
@@ -74,17 +107,13 @@ rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[])
         wheel_set_rpm(chas->c_wheels[i], target_rpm[i]);
     }
 
-    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
-    {
-        wheel_update(chas->c_wheels[i]);
-    }
-
     return RT_EOK;
 }
 
 rt_err_t chassis_update(chassis_t chas)
 {
-    // TODO
+    RT_ASSERT(chas != RT_NULL);
+
     for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
     {
         wheel_update(chas->c_wheels[i]);
@@ -92,46 +121,68 @@ rt_err_t chassis_update(chassis_t chas)
     return RT_EOK;
 }
 
-rt_err_t chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args)
+rt_err_t chassis_straight(chassis_t chas, float linear_x)
 {
-    // TODO
-    struct velocity target_vel;
-    
-    LOG_D("received command: cmd:%d arg:%d", cmd, *((rt_uint8_t *)args));
+    RT_ASSERT(chas != RT_NULL);
 
-    switch (cmd)
-    {
-    case CHASSIS_FORWARD_ANALOG:
-        target_vel.linear_x = (float)*((rt_uint8_t *)args) / 100.0f;   // m/s
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    case CHASSIS_BACKWARD_ANALOG:
-        target_vel.linear_x = -(float)*((rt_uint8_t *)args) / 100.0f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    case CHASSIS_TURN_LEFT_ANALOG:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = (float)*((rt_uint8_t *)args);
-        break;
-    case CHASSIS_TURN_RIGHT_ANALOG:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = -(float)*((rt_uint8_t *)args);
-        break;
-    case CHASSIS_STOP:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    default:
-        return RT_ERROR;
-        break;
-    }
+    struct velocity target_velocity = {
+        .linear_x = linear_x,
+        .linear_y = 0.0f,
+        .angular_z = 0.0f
+    };
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
+    return chassis_set_rpm(chas, res_rpm);
+}
 
-    chassis_set_velocity(chas, target_vel);
+rt_err_t chassis_move(chassis_t chas, float linear_y)
+{
+    RT_ASSERT(chas != RT_NULL);
 
-    return RT_EOK;
+    struct velocity target_velocity = {
+        .linear_x = 0.0f,
+        .linear_y = linear_y,
+        .angular_z = 0.0f
+    };
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
+    return chassis_set_rpm(chas, res_rpm);
+}
+
+rt_err_t chassis_rotate(chassis_t chas, float angular_z)
+{
+    RT_ASSERT(chas != RT_NULL);
+
+    struct velocity target_velocity = {
+        .linear_x = 0.0f,
+        .linear_y = 0.0f,
+        .angular_z = angular_z
+    };
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
+    return chassis_set_rpm(chas, res_rpm);
+}
+
+rt_err_t chassis_set_velocity_x(chassis_t chas, float linear_x)
+{
+    RT_ASSERT(chas != RT_NULL);
+
+    chas->c_velocity.linear_x = linear_x;
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
+    return chassis_set_rpm(chas, res_rpm);
+}
+
+rt_err_t chassis_set_velocity_y(chassis_t chas, float linear_y)
+{
+    RT_ASSERT(chas != RT_NULL);
+
+    chas->c_velocity.linear_y = linear_y;
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
+    return chassis_set_rpm(chas, res_rpm);
+}
+
+rt_err_t chassis_set_velocity_z(chassis_t chas, float angular_z)
+{
+    RT_ASSERT(chas != RT_NULL);
+    
+    chas->c_velocity.angular_z = angular_z;
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
+    return chassis_set_rpm(chas, res_rpm);
 }

chassis/chassis.h

@@ -1,17 +1,20 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
 #ifndef __CHASSIS_H__
 #define __CHASSIS_H__
 
 #include <kinematics.h>
 #include <wheel.h>
 
-enum chassis_command
-{
-    CHASSIS_FORWARD_ANALOG,
-    CHASSIS_BACKWARD_ANALOG,
-    CHASSIS_TURN_LEFT_ANALOG,
-    CHASSIS_TURN_RIGHT_ANALOG,
-    CHASSIS_STOP,
-};
+#define CHASSIS_VELOCITY_LINEAR_MAXIMUM         0.1f                                                  // m/s
+#define CHASSIS_VELOCITY_ANGULAR_MAXIMUM        5.0f * CHASSIS_VELOCITY_LINEAR_MAXIMUM                // rad/s
 
 typedef struct chassis *chassis_t;
 
@@ -23,7 +26,7 @@ struct chassis
 };
 
 chassis_t   chassis_create(wheel_t* c_wheel, kinematics_t c_kinematics);
-void        chassis_destroy(chassis_t chas);
+rt_err_t    chassis_destroy(chassis_t chas);
 
 rt_err_t    chassis_enable(chassis_t chas);
 rt_err_t    chassis_disable(chassis_t chas);
@@ -34,6 +37,12 @@ rt_err_t    chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
 
 rt_err_t    chassis_update(chassis_t chas);
 
-rt_err_t    chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args);
+rt_err_t    chassis_straight(chassis_t chas, float linear_x);
+rt_err_t    chassis_move(chassis_t chas, float linear_y);
+rt_err_t    chassis_rotate(chassis_t chas, float angular_z);
+
+rt_err_t    chassis_set_velocity_x(chassis_t chas, float linear_x);
+rt_err_t    chassis_set_velocity_y(chassis_t chas, float linear_y);
+rt_err_t    chassis_set_velocity_z(chassis_t chas, float angular_z);
 
 #endif

controller/controller.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "controller.h"
 
 #define DBG_SECTION_NAME  "controller"
@@ -6,17 +16,17 @@
 
 // single intput and single output system
 
-controller_t controller_create(rt_size_t size)
+controller_t controller_create(rt_size_t size, rt_uint16_t sample_time)
 {
-    // TODO
     // Malloc memory and initialize PID
     controller_t new_controller = (controller_t)rt_malloc(size);
     if(new_controller == RT_NULL)
     {
-        LOG_E("Failed to malloc memory for automatic controller\n");
+        LOG_E("Failed to malloc memory for new controller\n");
         return RT_NULL;
     }
 
+    new_controller->sample_time = sample_time;
     new_controller->enable = RT_FALSE;
 
     return new_controller;
@@ -42,14 +52,16 @@ rt_err_t controller_destroy(controller_t controller)
 {
     RT_ASSERT(controller != RT_NULL);
     
+    LOG_D("Free controller");
+
     return controller->destroy(controller);
 }
 
 rt_err_t controller_enable(controller_t controller)
 {
-    // TODO
     RT_ASSERT(controller != RT_NULL);
 
+    LOG_D("Enabling controller");
     controller->enable = RT_TRUE;
 
     return RT_EOK;
@@ -57,9 +69,9 @@ rt_err_t controller_enable(controller_t controller)
 
 rt_err_t controller_disable(controller_t controller)
 {
-    // TODO
     RT_ASSERT(controller != RT_NULL);
 
+    LOG_D("Disabling controller");
     controller->enable = RT_FALSE;
 
     return RT_EOK;
@@ -67,7 +79,6 @@ rt_err_t controller_disable(controller_t controller)
 
 rt_err_t controller_reset(controller_t controller)
 {
-    // TODO
     RT_ASSERT(controller != RT_NULL);
 
     return controller->reset(controller);
@@ -88,3 +99,18 @@ rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_
     controller->sample_time = sample_time;
     return RT_EOK;
 }
+
+rt_err_t controller_set_param(controller_t controller, controller_param_t param)
+{
+    RT_ASSERT(controller != RT_NULL);
+
+    return controller->set_param(controller, param);
+}
+
+rt_err_t controller_get_param(controller_t controller, controller_param_t param)
+{
+    RT_ASSERT(controller != RT_NULL);
+
+    return controller->get_param(controller, param);
+}
+

controller/controller.h

@@ -1,29 +1,60 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __CONTROLLER_H__
 #define __CONTROLLER_H__
 
 #include <rtthread.h>
 
+struct controller_pid_param
+{
+    float kp;
+    float ki;
+    float kd;
+};
+
+struct controller_param
+{
+    union
+    {
+        struct controller_pid_param pid;
+    } data;
+};
+
+typedef struct controller_param *controller_param_t;
+
 struct controller
 {
-    float       target;
-    float       output;
-    rt_uint16_t sample_time;    // unit:ms
-    int         enable;
-
-    rt_err_t    (*update)(void *controller, float current_point);
-    rt_err_t    (*reset)(void *controller);
-    rt_err_t    (*destroy)(void *controller);
+    float           target;
+    float           output;
+    rt_uint16_t     sample_time;    // unit:ms
+    int             enable;
+
+    rt_err_t        (*update)(void *controller, float current_point);
+    rt_err_t        (*reset)(void *controller);
+    rt_err_t        (*destroy)(void *controller);
+    rt_err_t        (*set_param)(void *controller, controller_param_t param);
+    rt_err_t        (*get_param)(void *controller, controller_param_t param);
 };
 
 typedef struct controller *controller_t;
 
-controller_t    controller_create(rt_size_t size);
-rt_err_t        controller_update(controller_t controller, float current_point);
-rt_err_t        controller_destroy(controller_t controller);
-rt_err_t        controller_enable(controller_t controller);
-rt_err_t        controller_disable(controller_t controller);
-rt_err_t        controller_reset(controller_t controller);
-rt_err_t        controller_set_target(controller_t controller, rt_int16_t target);
-rt_err_t        controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
+controller_t        controller_create(rt_size_t size, rt_uint16_t sample_time);
+rt_err_t            controller_update(controller_t controller, float current_point);
+rt_err_t            controller_destroy(controller_t controller);
+rt_err_t            controller_enable(controller_t controller);
+rt_err_t            controller_disable(controller_t controller);
+rt_err_t            controller_reset(controller_t controller);
+rt_err_t            controller_set_target(controller_t controller, rt_int16_t target);
+rt_err_t            controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
+rt_err_t            controller_set_param(controller_t controller, controller_param_t param);
+rt_err_t            controller_get_param(controller_t controller, controller_param_t param);
 
 #endif // __CONTROLLER_H__

controller/inc_pid_controller.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "inc_pid_controller.h"
 
 #define DBG_SECTION_NAME  "inc_pid_controller"
@@ -6,7 +16,13 @@
 
 static rt_err_t inc_pid_controller_reset(void *pid)
 {
-    rt_memset(pid, 0, sizeof(struct inc_pid_controller));
+    inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
+    
+    inc_pid->error = 0.0f;
+    inc_pid->error_l = 0.0f;
+    inc_pid->error_ll = 0.0f;
+    inc_pid->last_out = 0.0f;
+
     return RT_EOK;
 }
 
@@ -16,6 +32,28 @@ static rt_err_t inc_pid_controller_destroy(void *pid)
     return RT_EOK;
 }
 
+static rt_err_t inc_pid_controller_set_param(void *pid, controller_param_t param)
+{
+    inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
+
+    inc_pid->kp = param->data.pid.kp;
+    inc_pid->ki = param->data.pid.ki;
+    inc_pid->kd = param->data.pid.kd;
+
+    return RT_EOK;
+}
+
+static rt_err_t inc_pid_controller_get_param(void *pid, controller_param_t param)
+{
+    inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
+
+    param->data.pid.kp = inc_pid->kp;
+    param->data.pid.ki = inc_pid->ki;
+    param->data.pid.kd = inc_pid->kd;
+
+    return RT_EOK;
+}
+
 static rt_err_t inc_pid_controller_update(void *pid, float current_point)
 {
     inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
@@ -49,20 +87,12 @@ static rt_err_t inc_pid_controller_update(void *pid, float current_point)
 
     inc_pid->controller.output = inc_pid->last_out;
 
-    // rt_kprintf("%d - %d\n", current_point, pid->set_point);
-    // LOG_D("PID current: %d : setpoint %d - P%d I%d D%d - [%d]", current_point, pid->set_point, (int)(pid->p_error + 0.5f), (int)(pid->i_error + 0.5f), (int)(pid->d_error + 0.5f), (int)(pid->out + 0.5f));
-    // LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
-    // LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
-    // LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
-    // LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
-    // LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
-
     return RT_EOK;
 }
 
-inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd)
+inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time)
 {
-    inc_pid_controller_t new_pid = (inc_pid_controller_t)controller_create(sizeof(struct inc_pid_controller));
+    inc_pid_controller_t new_pid = (inc_pid_controller_t)controller_create(sizeof(struct inc_pid_controller), sample_time);
     if(new_pid == RT_NULL)
     {
         return RT_NULL;
@@ -88,7 +118,9 @@ inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd)
     new_pid->controller.reset = inc_pid_controller_reset;
     new_pid->controller.destroy = inc_pid_controller_destroy;
     new_pid->controller.update = inc_pid_controller_update;
-
+    new_pid->controller.set_param = inc_pid_controller_set_param;
+    new_pid->controller.get_param = inc_pid_controller_get_param;
+    
     return new_pid;
 }
 

controller/inc_pid_controller.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __INC_PID_CONTROLLER_H__
 #define __INC_PID_CONTROLLER_H__
 
@@ -8,28 +18,28 @@ typedef struct inc_pid_controller *inc_pid_controller_t;
 
 struct inc_pid_controller
 {
-    struct controller controller;
+    struct controller   controller;
     
-    float     kp;
-    float     ki;
-    float     kd;
+    float               kp;
+    float               ki;
+    float               kd;
 
-    float     minimum;
-    float     maximum;
+    float               minimum;
+    float               maximum;
 
-    float     p_error;
-    float     i_error;
-    float     d_error;
+    float               p_error;
+    float               i_error;
+    float               d_error;
 
-    float     error;
-    float     error_l;
-    float     error_ll;
-    
-    float     last_out;
-    rt_tick_t last_time;
+    float               error;
+    float               error_l;
+    float               error_ll;
+
+    float               last_out;
+    rt_tick_t           last_time;
 };
 
-inc_pid_controller_t    inc_pid_controller_create(float kp, float ki, float kd);
+inc_pid_controller_t    inc_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time);
 rt_err_t                inc_pid_controller_set_kp(inc_pid_controller_t pid, float kp);
 rt_err_t                inc_pid_controller_set_ki(inc_pid_controller_t pid, float ki);
 rt_err_t                inc_pid_controller_set_kd(inc_pid_controller_t pid, float kd);

controller/pos_pid_controller.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "pos_pid_controller.h"
 
 #define DBG_SECTION_NAME  "pos_pid_controller"
@@ -6,7 +16,13 @@
 
 static rt_err_t pos_pid_controller_reset(void *pid)
 {
-    rt_memset(pid, 0, sizeof(struct pos_pid_controller));
+    pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
+
+    pos_pid->error = 0.0f;
+    pos_pid->error_l = 0.0f;
+    pos_pid->integral = 0.0f;
+    pos_pid->last_out = 0.0f;
+
     return RT_EOK;
 }
 
@@ -16,6 +32,28 @@ static rt_err_t pos_pid_controller_destroy(void *pid)
     return RT_EOK;
 }
 
+static rt_err_t pos_pid_controller_set_param(void *pid, controller_param_t param)
+{
+    pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
+
+    pos_pid->kp = param->data.pid.kp;
+    pos_pid->ki = param->data.pid.ki;
+    pos_pid->kd = param->data.pid.kd;
+
+    return RT_EOK;
+}
+
+static rt_err_t pos_pid_controller_get_param(void *pid, controller_param_t param)
+{
+    pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
+
+    param->data.pid.kp = pos_pid->kp;
+    param->data.pid.ki = pos_pid->ki;
+    param->data.pid.kd = pos_pid->kd;
+
+    return RT_EOK;
+}
+
 static rt_err_t pos_pid_controller_update(void *pid, float current_point)
 {
     pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
@@ -29,7 +67,7 @@ static rt_err_t pos_pid_controller_update(void *pid, float current_point)
 
     pos_pid->error = pos_pid->controller.target - current_point;
 
-    pos_pid->integral += pos_pid->error;
+    pos_pid->integral += pos_pid->ki * pos_pid->error;
 
     //Perform integral value capping to avoid internal PID state to blows up
     //when controllertuators saturate:
@@ -40,7 +78,7 @@ static rt_err_t pos_pid_controller_update(void *pid, float current_point)
     }
 
     pos_pid->p_error = pos_pid->kp * pos_pid->error;
-    pos_pid->i_error = pos_pid->ki * pos_pid->integral;
+    pos_pid->i_error = pos_pid->integral;
     pos_pid->d_error = pos_pid->kd * (pos_pid->error - pos_pid->error_l);
 
     pos_pid->last_out = pos_pid->p_error + pos_pid->i_error + pos_pid->d_error;
@@ -57,20 +95,12 @@ static rt_err_t pos_pid_controller_update(void *pid, float current_point)
 
     pos_pid->controller.output = pos_pid->last_out;
 
-    // rt_kprintf("%d - %d\n", current_point, pid->set_point);
-    // LOG_D("PID current: %d : setpoint %d - P%d I%d D%d - [%d]", current_point, pid->set_point, (int)(pid->p_error + 0.5f), (int)(pid->i_error + 0.5f), (int)(pid->d_error + 0.5f), (int)(pid->out + 0.5f));
-    // LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
-    // LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
-    // LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
-    // LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
-    // LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
-
     return RT_EOK;
 }
 
-pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd)
+pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time)
 {
-    pos_pid_controller_t new_pid = (pos_pid_controller_t)controller_create(sizeof(struct pos_pid_controller));
+    pos_pid_controller_t new_pid = (pos_pid_controller_t)controller_create(sizeof(struct pos_pid_controller), sample_time);
     if(new_pid == RT_NULL)
     {
         return RT_NULL;
@@ -82,7 +112,7 @@ pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd)
 
     new_pid->maximum = +1000;
     new_pid->minimum = -1000;
-    new_pid->anti_windup_value = 0.0f;
+    new_pid->anti_windup_value = new_pid->maximum * 0.5f;
 
     new_pid->integral = 0.0f;
     new_pid->p_error = 0.0f;
@@ -97,24 +127,35 @@ pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd)
     new_pid->controller.reset = pos_pid_controller_reset;
     new_pid->controller.destroy = pos_pid_controller_destroy;
     new_pid->controller.update = pos_pid_controller_update;
+    new_pid->controller.set_param = pos_pid_controller_set_param;
+    new_pid->controller.get_param = pos_pid_controller_get_param;
 
     return new_pid;
 }
 
 rt_err_t pos_pid_controller_set_kp(pos_pid_controller_t pid, float kp)
 {
+    RT_ASSERT(pid != RT_NULL);
+
     pid->kp = kp;
+
     return RT_EOK;
 }
 
 rt_err_t pos_pid_controller_set_ki(pos_pid_controller_t pid, float ki)
 {
+    RT_ASSERT(pid != RT_NULL);
+
     pid->ki = ki;
+
     return RT_EOK;
 }
 
 rt_err_t pos_pid_controller_set_kd(pos_pid_controller_t pid, float kd)
 {
+    RT_ASSERT(pid != RT_NULL);
+
     pid->kd = kd;
+    
     return RT_EOK;
 }

controller/pos_pid_controller.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __POS_PID_CONTROLLER_H__
 #define __POS_PID_CONTROLLER_H__
 
@@ -8,29 +18,29 @@ typedef struct pos_pid_controller *pos_pid_controller_t;
 
 struct pos_pid_controller
 {
-    struct controller controller;
+    struct controller   controller;
     
-    float     kp;
-    float     ki;
-    float     kd;
+    float               kp;
+    float               ki;
+    float               kd;
 
-    float     minimum;
-    float     maximum;
-    float     anti_windup_value;
+    float               minimum;
+    float               maximum;
+    float               anti_windup_value;
 
-    float     p_error;
-    float     i_error;
-    float     d_error;
+    float               p_error;
+    float               i_error;
+    float               d_error;
 
-    float     integral;
-    float     error;
-    float     error_l;
-    
-    float     last_out;
-    rt_tick_t last_time;
+    float               integral;
+    float               error;
+    float               error_l;
+
+    float               last_out;
+    rt_tick_t           last_time;
 };
 
-pos_pid_controller_t    pos_pid_controller_create(float kp, float ki, float kd);
+pos_pid_controller_t    pos_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time);
 rt_err_t                pos_pid_controller_set_kp(pos_pid_controller_t pid, float kp);
 rt_err_t                pos_pid_controller_set_ki(pos_pid_controller_t pid, float ki);
 rt_err_t                pos_pid_controller_set_kd(pos_pid_controller_t pid, float kd);

docs/README.md

@@ -10,5 +10,7 @@
 | ------------------------------------------------------------ | -------- |
 | [design.md](./design.md) | 设计说明 |
 | [api.md](./api.md) | API 说明 |
+| [introduction.md](./introduction.md) | 详细介绍 |
 | [samples.md](./samples.md) | 应用示例 |
+| [user-guide.md](./user-guide.md) | 用户指南 |
 

docs/api.md

@@ -1,12 +1,6 @@
 # RT-Robot API 介绍
 
-在各个模块硬件测试通过后，会一步步将稳定的 API 添加到这里，现在大家可以参照[例程](./samples.md)里面提供的方法进行初始化和控制。
-
-
-
-## 控制器 API (controller)
-
-不同手柄按键指令与机器人运动指令间的转换。
+## 协议 API (protocol)
 
 
 
@@ -18,32 +12,241 @@
 
 #### 1 车底盘 API (chassis)
 
+```c
+chassis_t chassis_create(wheel_t* c_wheel, kinematics_t c_kinematics);
+```
+
+创建车底盘
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| c_wheel  | 车轮对象数组 |
+| c_kinematics | 动力学模型对象 |
+| **返回** | **--**             |
+|   |  |
+
+```c
+rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity);
+```
+
+设置小车底盘速度
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| chas  | 车底盘对象 |
+| target_velocity | 预设速度(x,y,w)。x、y速度单位为m/s，w单位为rad/s|
+| **返回** | **--**             |
+|   |  |
 
+```c
+rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
+```
+
+设置小车底盘车轮速度（单位：转/分）
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| chas  | 车底盘 |
+| target_rpm | 预设速度(转每分) |
+| **返回** | **--**             |
+|   |  |
 
 ##### 1.1 车轮 API (wheel)
 
-闭环车轮的运动。
+```c
+wheel_t wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_controller, float radius, rt_uint16_t gear_ratio);
+```
+
+创建车轮
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| w_motor  | 电机对象 |
+| w_encoder | 编码器对象 |
+| w_controller | 控制器对象 |
+| radius | 车轮直径 |
+| gear_ratio | 电机减速比 |
+| **返回** | **--**             |
+|   |  |
+
+```c
+rt_err_t wheel_set_speed(wheel_t whl, double speed);
+```
+
+设定车轮速度
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| whl  | 车轮对象 |
+| speed | 速度（单位：m/s） |
+| **返回** | **--**             |
+|   |  |
+
+```c
+rt_err_t wheel_set_rpm(wheel_t whl, rt_int16_t rpm);
+```
 
+设定车轮速度
 
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| whl  | 车轮对象 |
+| rpm | 速度（单位：rpm |
+| **返回** | **--**             |
+|   |  |
 
 **1.1.1 电机 API (motor)**
 
-电机控制相关。
+**直流电机**
 
-**1.1.2 编码器 API (encoder)**
+```c
+dual_pwm_motor_t dual_pwm_motor_create(char *pwm1, int pwm1_channel, char *pwm2, int pwm2_channel);
+```
 
-获取编码器信息。
+创建双 PWM 驱动方式电机
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| pwm1  | pwm设备名 |
+| pwm1_channel | pwm通道 |
+| pwm2 | pwm设备名 |
+| pwm2_channel | pwm通道 |
+| **返回** | **--**             |
+|   |  |
+
+```c
+single_pwm_motor_t  single_pwm_motor_create(char *pwm, int channel, rt_base_t pin1, rt_base_t pin2);
+```
+
+创建单 PWM 驱动方式电机
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| pwm  | pwm设备名 |
+| channel | pwm通道 |
+| pin1 | 控制引脚1 |
+| pin2 | 控制引脚2 |
+| **返回** | **--**             |
+|   |  |
+
+**舵机**
+
+```c
+servo_t servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max);
+```
+
+创建舵机
 
-**1.1.3 PID API (PID)**
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| pwm  | pwm 设备名 |
+| channel | pwm 通道 |
+|angle|角度范围|
+| pluse_min | 最小脉宽 （单位：ns）；传入 RT_NULL 则设为默认值 |
+| pluse_max | 最大脉宽 （单位：ns）；传入 RT_NULL 则设为默认值|
+| **返回** | **--**             |
+|   |  |
 
-闭环控制。
+```c
+rt_err_t servo_set_angle(servo_t servo, float angle);
+```
 
+设定舵机转动角度
 
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| servo  | pwm 设备名 |
+| angle | 转动角度 （从零至最大值，最大值取决于 create 时传入的 angle 参数）|
+| **返回** | **--**             |
+|   |  |
+
+
+**1.1.2 编码器 API (encoder)**
+
+获取编码器信息。
+
+```c
+ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol);
+```
+
+创建AB相编码器
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| pin_a  | A相引脚 |
+| pin_b | B相引脚 |
+| pulse_revol | 车轮转一圈输出的脉冲数 |
+| **返回** | **--**             |
+|   |  |
+
+```c
+single_phase_encoder_t single_phase_encoder_create(rt_base_t pin, rt_uint16_t pulse_revol);
+```
+
+创建单相编码器
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| pin  | 引脚 |
+| pulse_revol | 车轮转一圈输出的脉冲数 |
+| **返回** | **--**             |
+|   |  |
+
+**1.1.3 控制器 API (controller)**
+
+```c
+inc_pid_controller_t    inc_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time);
+```
+
+创建增量式 PID 控制器
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| kp  | 参数Kp值 |
+| ki | 参数Ki值 |
+| kd | 参数Kd值 |
+| sample_time | 计算周期 |
+| **返回** | **--**             |
+|   |  |
+
+```c
+pos_pid_controller_t    pos_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time);
+```
+
+创建位置式 PID 控制器
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| kp  | 参数Kp值 |
+| ki | 参数Ki值 |
+| kd | 参数Kd值 |
+| sample_time | 计算周期 |
+| **返回** | **--**             |
+|   |  |
 
 ##### 1.2 动力学模型 API (kinematics)
 
-小车速度 (x, y, w) 与各个电机转速 (rpm1, rpm2, rpm3, rpm4) 之间的相互转换。
+```c
+kinematics_t    kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius);
+```
+
+创建动力学模型
+
+|**参数**     | **描述**                 |
+| :------- | :------------------- |
+| k_base  | 类型 |
+| length_x | X轴两轮间距 |
+| length_y | Y轴两轮间距 |
+| wheel_radius | 车轮直径 |
+| **返回** | **--**             |
+|   |  |
 
+_小车速度 (x, y, w) 与各个电机转速 (rpm1, rpm2, rpm3, rpm4) 之间的相互转换。_
 
+```c
+rt_int16_t*     kinematics_get_rpm(struct kinematics kin, struct velocity target_vel);
+```
 
-#### 2 传感器 API (sensor)
+```c
+struct velocity kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm);
+```

docs/design.md

@@ -1,79 +1,50 @@
 # RT-Robot 设计说明
 
-## 顶层框架设计
+## 1 智能车体系结构
 
-这个框架的顶层模块当前有 controller 和 robot，通过 controller 接收和发送指令来控制 robot。
+rt-robot 将智能车平台分为两个部分：车底盘和动力学模型
 
-- robot 是抽象出的机器人模块，为了适配智能小车、机械臂、飞控等各种不同类型的机器人。
-
-- controller 是抽象出的控制器模块，可以适配 PS2 手柄、蓝牙手柄、飞控手柄等不同类型的控制器。
-
-![](./figures/top-level.png)
-
-### 1 控制器模块 (controller) 
-
-控制器对机器人做出控制的流程是一个适配器模式。例如， PS2 手柄控制智能小车的流程：
-
-![](./figures/adapter_mode.png)
-
-- PS2 手柄模块内会用 enum 维护自己的按键值，例如 PS2_BTN_L1_PRESSED 表示 L1 被按下
-
-- 智能小车模块内也会有自己的控制指令，例如 CAR_CMD_FORWARD 表示控制小车向前运动
-
-那么 PS2 手柄的按键值是怎么对应到小车的运动呢？这需要在初始化的时候通过控制器的 bind 函数，将手柄的按键值和小车的运动关联起来，例如将 PS2_BTN_L1_PRESSED 和 CAR_CMD_FORWARD 关联起来，当 PS2 手柄的 L1 按键被按下后，我们就可以知道应当控制小车向前运动。
-
-因此，控制器模块在此处起到一个适配器的作用，将手柄的按键值转换为小车的运动，其实也就是初始化的时候起到定义自己喜欢的按键功能的作用。
-
-### 2 机器人模块 (robot) 
-
-我们下面以智能小车 (mobile robot) 为例，小车模块当前主要分为车底盘 (chassis) 和传感器 (sensor)，例如一个智能小车会包含两轮差分驱动底盘，和一些陀螺仪加速度计等传感器，整体框架图如下图所示。
-
-![](./figures/mobile_robot.png)
-
-#### 2.1 车底盘模块 (chassis)
+### 1.1 车底盘模块 (chassis)
 
 车底盘模块的作用在于定义小车的底盘类型，例如有多少个可以控制的轮子、小车的长宽等物理属性、运动学模型。例如两轮的车底盘搭配两轮差分动力学模型，我们就得到了两轮差分驱动的小车。
 
 车底盘模块下面又包括车轮 (wheel) 模块和动力学模型模块 (kinematics)。
 
-##### 2.1.1 车轮模块 (wheel)
+#### 1.1.1 车轮模块 (wheel)
 
-车轮模块需要实现一个能够闭环控制的车轮，因此需要用到电机、编码器和 PID算法。
+车轮模块需要实现一个能够闭环控制的车轮，因此需要用到电机、编码器和 闭环控制算法。
 
-###### 2.1.1.1 电机模块 (motor)
+##### 1.1.1.1 电机模块 (motor)
 
 电机模块需要为不同电机提供统一的接口，能够实现闭环电机转速控制，包括：
 
 - 直流电机
 - 舵机
-
 - 步进电机
-- CAN电机
+- CAN接口电机
 - 直流无刷电机
 
-###### 2.1.1.2 编码器模块 (encoder)
+##### 1.1.1.2 编码器模块 (encoder)
 
 编码器模块需要为不同编码器提供统一的接口，能够测量得到当前电机的转速、运动方向等，包括：
 
 - 单相编码器
 - AB相编码器
 
-###### 2.1.1.3 PID 模块 (PID)
+##### 1.1.1.3 车轮速度闭环控制器模块 (e.g. PID)
 
-PID模块需要利用当前编码器测得的转速，和目标设定值，实现对电机的闭环控制，包括：
+控制器模块需要利用当前编码器测得的转速，和目标设定值，实现对电机的闭环控制，包括：
 
 - 增量式PID
+- 位置式PID
 
-##### 2.1.2 动力学模型模块 (kinematics)
+#### 1.1.2 动力学模型模块 (kinematics)
 
 动力学模块包含各种小车物理学模型，需要实现小车运动速度 (Vx, Vy, w) 与不同电机转速 (rpm1, rpm2, rpm3, rpm4) 之间的双向切换，包括：
 
 - 两轮差分驱动
-
 - 四轮差分驱动
-
 - 麦克纳姆轮驱动
-
 - Ackerman模型
 
 当前我们的小车坐标系定义如下：
@@ -81,7 +52,3 @@ PID模块需要利用当前编码器测得的转速，和目标设定值，实
 ![](./figures/coordinates.png)
 
 除了全局坐标系外，我们定义了小车的前进方向为 Xr，与前进方向垂直的方向为 Yr，以小车车头逆时针旋转方向为旋转角速度 w 的正方向。
-
-#### 2.2 传感器模块 (sensor)
-
-Coming soon

docs/introduction.md

@@ -0,0 +1,67 @@
+# RT-Robot 详细介绍
+
+**RT-Robot 是 RT-Thread 的机器人框架，希望能够支持智能小车、机械臂、无人机等各种不同类型的机器人。**
+
+## 软件包目录结构
+```shell
+.
+|-- README.md                       // 说明
+|-- SConscript                      // 构建脚本
+|-- chassis                         // 底盘模块
+|-- controller                      // 控制器模块
+|-- docs                            // 文档
+|-- encoder                         // 编码器模块
+|-- kinematics                      // 动力学模型模块
+|-- motor                           // 电机模块
+|-- protocol                        // 协议模块（上位机/遥控器）
+`-- wheel                           // 车轮模块
+```
+
+## 软件包功能特点
+
+rt-robot 当前只支持智能小车，特点如下：
+
+- 支持两轮差分驱动、四轮差分驱动、麦克纳姆轮驱动的小车底盘
+
+- 支持增量、位置式 PID
+- 支持单相、AB 相编码器
+- 支持双 PWM、单 PWM 的直流电机驱动，支持驱动舵机
+- 支持 PS2 遥控器
+- 支持 ANO_TC 匿名科创地面站
+
+## 小车动力学模型
+
+为了控制小车运动，必然要建立运动学模型，rt-robot 当前支持以下模型：
+
+- TWO_WD 两轮差分驱动
+
+- FOUR_WD 四轮差分驱动
+- MECANUM 麦克纳姆轮驱动
+- ACKERMANN 后轮驱动调速，
+前轮通过连杆控制方向
+
+运动型机器人根据其物理学模型又包括 **Holonomic** 和 **Non-Holonomic** 。
+
+**两轮差分驱动小车** 就是典型的Non-Holonomic，因为它的控制自由度(controllable degree of freedom)小于整体自由度(total degrees of freedom)，小车的整体自由度有3个，X,Y坐标的运动以及当前朝向(Orientation)，但是它的控制自由度只有2个：加速度和旋转角度。 **麦克纳姆轮驱动的小车** 则属与于Holonomic，它的控制自由度(controllable degree of freedom)等于整体自由度(total degrees of freedom)因为它可以在平面坐标系内沿任意方向移动。下面我们重点介绍麦克纳姆轮驱动的小车。
+
+### 1 麦克纳姆轮驱动
+
+麦克纳姆(Mecanum)轮驱动的小车属于Holonomic，它的特点在于小车可以保持车头方向不变，就在平面坐标系内沿各个方向自由移动。
+
+我们首先了解一下麦克纳姆轮，其实它主要是在传统的轮子上增加了滚轮，这样传统车轮无法实现的横向移动就可以通过滚轮完成，而滚轮的安装方式有两种：左旋和右旋，如下图所示：
+
+![](./figures/mecanum-wheel.png)
+
+图1.1 麦克纳姆轮不同安装方式
+
+通过合理搭配左旋和右旋的麦克纳姆轮，就可以实现不同的运动组合。 **组装时一定要确保四个轮子上滚轮的转动轴汇聚于底板中心** ，这样控制才不会出现漂移，后面的理论分析也都是基于这一假设的。
+
+![](./figures/mecanum-chassis.png)
+
+图1.2 麦克纳姆轮安装方式
+
+通过速度分解控制四个轮子的转速，就可以实现不同方向的运动。
+
+![](./figures/mecanum-movement.png)
+
+图1.3 麦克纳姆轮驱动小车运动分解

docs/samples.md

@@ -1,11 +1,11 @@
 # RT-Robot 使用示例
 
-使用 rt-robot 框架的初始化流程包括：
+使用 rt-robot 框架的初始化智能车流程包括：
 
-- 1 初始化车轮 (为车轮添加电机、编码器、PID)
-  - 1.1 初始化电机 
-  - 1.2 初始化电编码器
-  - 1.3 初始化 PID 
+- 1 初始化车轮 (为车轮添加电机、编码器、控制器)
+  - 1.1 初始化车轮电机 
+  - 1.2 初始化车轮编码器
+  - 1.3 初始化车轮控制器
   
 - 2 初始化动力学模型
 
@@ -14,79 +14,116 @@
 下面是两轮差分控制的小车初始化示例代码：
 
 ```C
-// 1. Initialize two wheels - left and right
-wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);
-if (c_wheels == RT_NULL)
+// MOTOR
+#define LEFT_FORWARD_PWM            "pwm4"
+#define LEFT_FORWARD_PWM_CHANNEL    3
+#define LEFT_BACKWARD_PWM           "pwm4"
+#define LEFT_BACKWARD_PWM_CHANNEL   4
+
+#define RIGHT_FORWARD_PWM           "pwm2"
+#define RIGHT_FORWARD_PWM_CHANNEL   3
+#define RIGHT_BACKWARD_PWM          "pwm2"
+#define RIGHT_BACKWARD_PWM_CHANNEL  4
+
+// ENCODER
+#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)
+#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
+#define RIGHT_ENCODER_A_PHASE_PIN   36      //
+#define RIGHT_ENCODER_B_PHASE_PIN   8       //
+#define PULSE_PER_REVOL           2000      // Real value 2000
+#define ENCODER_SAMPLE_TIME         50
+
+// CONTROLLER PID
+#define PID_SAMPLE_TIME             50
+#define PID_PARAM_KP                6.6
+#define PID_PARAM_KI                6.5
+#define PID_PARAM_KD                7.6
+
+// WHEEL
+#define WHEEL_RADIUS             0.066
+#define GEAR_RATIO                  36
+
+// CAR
+chassis_t chas;
+
+#define WHEEL_DIST_X                 0
+#define WHEEL_DIST_Y              0.13
+
+// Car Thread
+#define THREAD_PRIORITY             10
+#define THREAD_STACK_SIZE          512
+#define THREAD_TIMESLICE             5
+
+static rt_thread_t tid_car = RT_NULL;
+
+static void command_register_all(void);
+void init_laser_and_gimbal(void);
+
+void car_thread(void *param)
 {
-    LOG_D("Failed to malloc memory for wheels");
-}
-
-// 1.1 Create two motors
-motor_t left_motor  = motor_create(left_motor_init,  left_motor_enable,  left_motor_disable,  left_motor_set_speed,  DC_MOTOR);
-motor_t right_motor = motor_create(right_motor_init, right_motor_enable, right_motor_disable, right_motor_set_speed, DC_MOTOR);
-
-// 1.2 Create two encoders
-encoder_t left_encoder  = encoder_create(LEFT_ENCODER_PIN, PULSE_PER_REVOL);
-encoder_t right_encoder = encoder_create(RIGHT_ENCODER_PIN, PULSE_PER_REVOL);
-
-// 1.3 Create two pid contollers
-pid_control_t left_pid  = pid_create();
-pid_control_t right_pid = pid_create();
-
-// 1.4 Add two wheels
-c_wheels[0] = wheel_create(left_motor,  left_encoder,  left_pid,  WHEEL_RADIUS, GEAR_RATIO);
-c_wheels[1] = wheel_create(right_motor, right_encoder, right_pid, WHEEL_RADIUS, GEAR_RATIO);
-
-// 2. Iinialize Kinematics - Two Wheel Differential Drive
-kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);
-
-// 3. Initialize Chassis
-chas = chassis_create(c_wheels, c_kinematics);
-
-// 4. Set Sample time
-encoder_set_sample_time(chas->c_wheels[0]->w_encoder, SAMPLE_TIME);
-encoder_set_sample_time(chas->c_wheels[1]->w_encoder, SAMPLE_TIME);
-pid_set_sample_time(chas->c_wheels[0]->w_pid, PID_SAMPLE_TIME);
-pid_set_sample_time(chas->c_wheels[1]->w_pid, PID_SAMPLE_TIME);
+    // TODO
 
-// 5. Enable Chassis
-chassis_enable(chas);
+    struct velocity target_velocity;
 
-// Turn left
-target_vel.linear_x = 0;   // m/s
-target_vel.linear_y = 0;    // m/s
-target_vel.angular_z = PI / 4; // rad/s
+    target_velocity.linear_x = 0.00f;
+    target_velocity.linear_y = 0;
+    target_velocity.angular_z = 0;
+    chassis_set_velocity(chas, target_velocity);
 
-chassis_set_velocity(chas, target_vel);
-rt_thread_mdelay(500);
+    // Open loop control
+    // controller_disable(chas->c_wheels[0]->w_controller);
+    // controller_disable(chas->c_wheels[1]->w_controller);
 
-// Turn right
-target_vel.linear_x = 0;   // m/s
-target_vel.linear_y = 0;    // m/s
-target_vel.angular_z = - PI / 4; // rad/s
-
-chassis_set_velocity(chas, target_vel);
-rt_thread_mdelay(500);
-
-// Go straight
-target_vel.linear_x = 0.06;   // m/s
-target_vel.linear_y = 0;    // m/s
-target_vel.angular_z = 0;
-
-chassis_set_velocity(chas, target_vel);
-
-// Stop
-target_vel.linear_x = 0.0;   // m/s
-target_vel.linear_y = 0;    // m/s
-target_vel.angular_z = 0; // rad/s
-
-chassis_set_velocity(chas, target_vel);
-rt_thread_mdelay(500);
+    while (1)
+    {
+        rt_thread_mdelay(50);
+        chassis_update(chas);
+    }
+}
 
-while(1)
+void car_init(void *parameter)
 {
-    chassis_update(chas);
-    rt_thread_mdelay(50);
+    // 1. Initialize two wheels - left and right
+    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);
+    if (c_wheels == RT_NULL)
+    {
+        LOG_D("Failed to malloc memory for wheels");
+    }
+
+    // 1.1 Create two motors
+    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);
+    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);
+
+    // 1.2 Create two encoders
+    ab_phase_encoder_t left_encoder  = ab_phase_encoder_create(LEFT_ENCODER_A_PHASE_PIN, LEFT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL, ENCODER_SAMPLE_TIME);
+    ab_phase_encoder_t right_encoder = ab_phase_encoder_create(RIGHT_ENCODER_A_PHASE_PIN, RIGHT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL, ENCODER_SAMPLE_TIME);
+
+    // 1.3 Create two pid contollers
+    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);
+    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);
+
+    // 1.4 Add two wheels
+    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);
+    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);
+
+    // 2. Iinialize Kinematics - Two Wheel Differential Drive
+    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);
+
+    // 3. Initialize Chassis
+    chas = chassis_create(c_wheels, c_kinematics);
+
+    // 4. Enable Chassis
+    chassis_enable(chas);
+
+    // thread
+    tid_car = rt_thread_create("tcar",
+                              car_thread, RT_NULL,
+                              THREAD_STACK_SIZE,
+                              THREAD_PRIORITY, THREAD_TIMESLICE);
+
+    if (tid_car != RT_NULL)
+    {
+        rt_thread_startup(tid_car);
+    }
 }
-
 ```

docs/user-guide.md

@@ -0,0 +1,132 @@
+# RT-Robot 用户指南
+
+本节主要介绍 rt-robot 框架的基本使用流程，并对重要的 API 和结构体进行说明。(组件思想)
+
+## 准备工作
+
+首先需要下载 rt-robot 软件包，并将软件包加入到项目中。在 BSP 目录下使用 menuconfig 命令打开 env 配置界面，在 RT-Thread online packages → system packages 中选择 RT-Robot 软件包，操作界面如下图所示：
+
+![rt-robot 软件包配置](./figures/rt-robot-menuconfig.png)
+
+```shell
+RT-Thread online packages
+    system packages  --->
+         [*] RT-Robot: RT-Thread robots development platform  --->
+            Version (latest) --->
+```
+
+Version: 配置软件包版本号。
+
+选择完成后，使用 `pkgs --update` 命令下载软件包并更新用户配置。
+
+## 使用流程
+
+使用 rt-robot 框架的初始化智能车流程如下：
+
+- 1 初始化车轮 (为车轮添加电机、编码器、控制器)
+  - 1.1 初始化车轮电机 
+  - 1.2 初始化车轮编码器
+  - 1.3 初始化车轮控制器
+
+- 2 初始化动力学模型
+
+- 3 初始化车底盘 (为底盘添加车轮、动力学模型)
+
+- 4 底盘控制
+
+### 1 初始化车轮
+
+车轮由电机、编码器、控制器三个组件组成。首先需要创建它们，然后再组成车轮。
+
+#### 电机
+
+电机有许多的类型和驱动方式，rt-robot 对此进行了抽象并针对不同类型和驱动方式的电机给出了各自的创建接口。用户调用对应的接口就可以创建出对应的电机对象。相应的创建API请参考 [api.md](./api.md)
+
+#### 编码器
+
+编码器有单相和AB相之分，rt-robot 对此给出了不同的创建接口，用户调用对应的接口就可以创建出对应的编码器对象。相应的创建API请参考 [api.md](./api.md)
+
+#### 控制器
+
+控制器本质上来说是一种算法，用来稳定系统输出（在此为车轮速度）。常见的控制算法为PID，其有增量式和位置式之分，rt-robot 对此有不同的创建接口。用户调用对应的接口就可以创建出对应的控制器对象。相应的创建API请参考 [api.md](./api.md)
+
+#### 创建车轮
+
+每个车轮都是由电机、编码器、控制器三个组件组成。在完成组件创建拿到组件对象后就可以调用如下接口创建车轮了。
+
+_创建接口：_
+```C
+wheel_t     wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_controller, float radius, rt_uint16_t gear_ratio);
+```
+- w_motor 电机对象
+- w_encoder 编码器对象
+- w_controller 控制器对象
+- radius 车轮直径
+- gear_ratio 减速比
+
+此只是创建一个车轮，多个车轮需要多次创建
+
+### 2 初始化动力学模型
+
+"凡系统必有结构，系统结构决定系统功能"。
+虽然小车的物理结构各不相同，但是同样作为小车底板，都能实现相同或相似的运动，
+因此我们对小车的常见运动进行抽象，提供统一的接口，适配不同小车底板。不同车底
+板最主要的区别在于物理模型，例如两轮驱动、四轮驱动等
+。对此 rt-robot 有相应的动力学模型。现抽象出的动力学模型有以下四种类型：
+- TWO_WD 两轮差分驱动
+- FOUR_WD 四轮差分驱动
+- MECANUM 麦克纳姆轮驱动
+- ACKERMANN 后轮驱动调速，
+前轮通过连杆控制方向
+
+_创建接口：_
+```C
+kinematics_t    kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius);
+```
+- k_base 类型
+- length_x X轴两车轮间距（单位: m）
+- length_y Y轴两车轮间距（单位: m）
+- wheel_radius  车轮直径（单位：m）
+
+### 3 初始化车底盘
+
+在创建了足够的车轮和相应的动力学模型后，就可以创建底盘了。底盘的创建比较简单，只要调用 ```chassis_t   chassis_create(wheel_t* c_wheel, kinematics_t c_kinematics);``` 接口即可，其两个参数分别是 车轮对象数组 和 动力学模型对象。
+
+### 4 底盘控制
+
+在以上创建完成后， 我们就拿到了一个车底盘对象。在控制底盘运动前，还需要调用底盘使能接口（```rt_err_t    chassis_enable(chassis_t chas);```）使能车底盘。使能完成后，就可以调用相应接口控制底盘运动了。底盘的**主要控制接口**如下：
+```c
+struct velocity
+{
+    float linear_x;  // m/s
+    float linear_y;  // m/s
+    float angular_z; // rad/s
+};
+
+rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity);
+```
+target_velocity 为底盘的速度，该速度有X、Y、Z三个分量，其分别是底盘的X轴线速度，Y轴线速度，Z轴转速。当前我们的小车坐标系定义如下：
+
+![](./figures/coordinates.png)
+
+X轴正方向为小车车头方向，以小车车头逆时针旋转方向为旋转角速度 w 的正方向。
+
+## 遥控器 / 上位机
+
+在使用遥控器/上位机之前必须先调用接口 '```rt_err_t command_init(chassis_t chas);```' 初始化 command 以便能够处理各种命令。
+
+遥控器和上位机有很多选择，当前 rt-robot 直接支持 PS2 遥控器和 ANO 上位机。在使用对应的遥控器/上位机之前需要先初始化它们。
+
+PS2 初始化接口：
+```c
+void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t di_pin);
+```
+参数 xx_pin 分别为相应的引脚号。
+
+ANO 初始化接口：
+```c
+int ano_init(void *param);
+```
+参数 param 为字符设备名（e.g. "uart1"）
+
+**初始化完成后就可以直接使用了。**

encoder/ab_phase_encoder.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "ab_phase_encoder.h"
 
 #define DBG_SECTION_NAME  "ab_phase_encoder"
@@ -18,7 +28,7 @@ static void encoder_isr(void *args)
 {
     ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)args;
     int val = rt_pin_read(enc_sub->pin_a) * 2 + rt_pin_read(enc_sub->pin_b);
-    // rt_kprintf("val:%d\n", val);
+
     if (AB_PHASE_ENCODER_TABLE[enc_sub->last_value] == val)
     {
         enc_sub->enc.dir = ENCODER_DIR_FORWARD;
@@ -34,8 +44,6 @@ static void encoder_isr(void *args)
 
 static rt_err_t ab_phase_encoder_enable(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
 
     // Attach interrupts
@@ -53,8 +61,6 @@ static rt_err_t ab_phase_encoder_enable(void *enc)
 
 static rt_err_t ab_phase_encoder_disable(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
 
     rt_pin_irq_enable(enc_sub->pin_a, PIN_IRQ_DISABLE);
@@ -65,8 +71,6 @@ static rt_err_t ab_phase_encoder_disable(void *enc)
 
 static rt_err_t ab_phase_encoder_destroy(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     ab_phase_encoder_disable(enc);
     ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
     rt_pin_detach_irq(enc_sub->pin_a);
@@ -77,10 +81,10 @@ static rt_err_t ab_phase_encoder_destroy(void *enc)
     return RT_EOK;
 }
 
-ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol)
+ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol, rt_uint16_t sample_time)
 {
     // Malloc memory and initialize pulse_count and pin
-    ab_phase_encoder_t new_encoder = (ab_phase_encoder_t)encoder_create(sizeof(struct ab_phase_encoder));
+    ab_phase_encoder_t new_encoder = (ab_phase_encoder_t)encoder_create(sizeof(struct ab_phase_encoder), sample_time);
     if(new_encoder == RT_NULL)
     {
         return RT_NULL;

encoder/ab_phase_encoder.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __AB_PHASE_ENCODER_H__
 #define __AB_PHASE_ENCODER_H__
 
@@ -6,6 +16,6 @@
 struct ab_phase_encoder;
 typedef struct ab_phase_encoder *ab_phase_encoder_t;
 
-ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol);
+ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol, rt_uint16_t sample_time);
 
 #endif // __AB_PHASE_ENCODER_H__

encoder/encoder.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include <rtdevice.h>
 #include <encoder.h>
 
@@ -5,7 +15,7 @@
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
-encoder_t encoder_create(rt_size_t size)
+encoder_t encoder_create(rt_size_t size, rt_uint16_t sample_time)
 {
     // Malloc memory and initialize
     encoder_t new_encoder = (encoder_t)rt_malloc(size);
@@ -17,6 +27,7 @@ encoder_t encoder_create(rt_size_t size)
 
     new_encoder->pulse_count = 0;
     new_encoder->last_count = 0;
+    new_encoder->sample_time = sample_time;
 
     return new_encoder;
 }
@@ -57,6 +68,7 @@ rt_err_t encoder_reset(encoder_t enc)
     RT_ASSERT(enc != RT_NULL);
 
     enc->pulse_count = 0;
+    enc->last_count = 0;
 
     return RT_EOK;
 }
@@ -64,7 +76,7 @@ rt_err_t encoder_reset(encoder_t enc)
 rt_int16_t encoder_measure_cps(encoder_t enc)
 {
     RT_ASSERT(enc != RT_NULL);
-    // TODO
+    
     // return count per second
     if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time))
     {
@@ -73,15 +85,6 @@ rt_int16_t encoder_measure_cps(encoder_t enc)
     }
 
     rt_int32_t diff_count = enc->pulse_count - enc->last_count;
-    
-    // if (diff_count >= INT32_MAX)
-    // {
-    //     diff_count = -(enc->pulse_count + enc->last_count);
-    // }
-    // if (diff_count <= INT32_MIN)
-    // {
-    //     diff_count = enc->pulse_count + enc->last_count;
-    // }
 
     enc->cps = diff_count * 1000 / enc->sample_time;
     enc->last_count = enc->pulse_count;

encoder/encoder.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __ENCODER_H__
 #define __ENCODER_H__
 
@@ -26,7 +36,7 @@ struct encoder
     rt_err_t                (*destroy)(void *enc);
 };
 
-encoder_t       encoder_create(rt_size_t size);
+encoder_t       encoder_create(rt_size_t size, rt_uint16_t sample_time);
 rt_err_t        encoder_destroy(encoder_t enc);
 rt_err_t        encoder_enable(encoder_t enc);                                      /* start measurement */
 rt_err_t        encoder_disable(encoder_t enc);                                     /* stop measurement */

encoder/single_phase_encoder.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #include "single_phase_encoder.h"
 
 #define DBG_SECTION_NAME  "single_phase_encoder"
@@ -14,13 +24,10 @@ static void encoder_isr(void *args)
 {
     rt_int32_t* pulse_count = (rt_int32_t*)args;
     (*pulse_count)++;
-    // LOG_D("Count %d", *pulse_count);
 }
 
 static rt_err_t single_phase_encoder_enable(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
 
     // Attach interrupts
@@ -33,8 +40,6 @@ static rt_err_t single_phase_encoder_enable(void *enc)
 
 static rt_err_t single_phase_encoder_disable(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
 
     return rt_pin_irq_enable(enc_sub->pin, PIN_IRQ_DISABLE);;
@@ -42,8 +47,6 @@ static rt_err_t single_phase_encoder_disable(void *enc)
 
 static rt_err_t single_phase_encoder_destroy(void *enc)
 {
-    RT_ASSERT(enc != RT_NULL);
-
     single_phase_encoder_disable(enc);
     single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
     rt_pin_detach_irq(enc_sub->pin);
@@ -53,10 +56,10 @@ static rt_err_t single_phase_encoder_destroy(void *enc)
     return RT_EOK;
 }
 
-single_phase_encoder_t single_phase_encoder_create(rt_base_t pin,rt_uint16_t pulse_revol)
+single_phase_encoder_t single_phase_encoder_create(rt_base_t pin, rt_uint16_t pulse_revol, rt_uint16_t sample_time)
 {
     // Malloc memory and initialize pulse_count and pin
-    single_phase_encoder_t new_encoder = (single_phase_encoder_t)encoder_create(sizeof(struct single_phase_encoder));
+    single_phase_encoder_t new_encoder = (single_phase_encoder_t)encoder_create(sizeof(struct single_phase_encoder), sample_time);
     if(new_encoder == RT_NULL)
     {
         return RT_NULL;

encoder/single_phase_encoder.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #ifndef __SINGLE_PHASE_ENCODER_H__
 #define __SINGLE_PHASE_ENCODER_H__
 
@@ -6,6 +16,6 @@
 struct single_phase_encoder;
 typedef struct single_phase_encoder *single_phase_encoder_t;
 
-single_phase_encoder_t single_phase_encoder_create(rt_base_t pin, rt_uint16_t pulse_revol);
+single_phase_encoder_t single_phase_encoder_create(rt_base_t pin, rt_uint16_t pulse_revol, rt_uint16_t sample_time);
 
 #endif // __SINGLE_PHASE_ENCODER_H__

kinematics/kinematics.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #include "kinematics.h"
 
 #define DBG_SECTION_NAME  "kinematics"
@@ -38,16 +48,18 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y,
     return new_kinematics;
 }
 
-void kinematics_destroy(kinematics_t kinematics)
+void kinematics_destroy(kinematics_t kin)
 {
+    RT_ASSERT(kin != RT_NULL);
     LOG_I("Free Kinematics");
-    rt_free(kinematics);
+    rt_free(kin);
 }
 
 rt_err_t kinematics_reset(kinematics_t kin)
 {
     // TODO
-
+    RT_ASSERT(kin != RT_NULL);
+    LOG_I("Reset kinematics");
     return RT_EOK;
 }
 
@@ -101,11 +113,6 @@ rt_int16_t* kinematics_get_rpm(struct kinematics kin, struct velocity target_vel
     // rear-right motor
     cal_rpm.motor4 = x_rpm - y_rpm + tan_rpm;
 
-    // rt_kprintf("cal_rpm[1]: %d\n", cal_rpm.motor1);
-    // rt_kprintf("cal_rpm[2]: %d\n", cal_rpm.motor2);
-    // rt_kprintf("cal_rpm[3]: %d\n", cal_rpm.motor3);
-    // rt_kprintf("cal_rpm[4]: %d\n", cal_rpm.motor4);
-
     if(kin.k_base == TWO_WD)
     {
         res_rpm[0] = cal_rpm.motor3;

kinematics/kinematics.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #ifndef __KINEMATICS_H__
 #define __KINEMATICS_H__
 

motor/dual_pwm_motor.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "dual_pwm_motor.h"
 
 #define DBG_SECTION_NAME "dual_pwm_motor"
@@ -15,8 +25,6 @@ struct dual_pwm_motor
 
 static rt_err_t dual_pwm_motor_enable(void *mot)
 {
-    RT_ASSERT(mot != RT_NULL);
-
     dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;
 
     rt_pwm_enable(mot_sub->pwm1_dev, mot_sub->pwm1_channel);
@@ -27,8 +35,6 @@ static rt_err_t dual_pwm_motor_enable(void *mot)
 
 static rt_err_t dual_pwm_motor_disable(void *mot)
 {
-    RT_ASSERT(mot != RT_NULL);
-
     dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;
     
     rt_pwm_disable(mot_sub->pwm1_dev, mot_sub->pwm1_channel);
@@ -37,10 +43,13 @@ static rt_err_t dual_pwm_motor_disable(void *mot)
     return RT_EOK;
 }
 
-static rt_err_t dual_pwm_motor_set_speed(void *mot, rt_int16_t thousands)
+static rt_err_t dual_pwm_motor_reset(void *mot)
 {
-    RT_ASSERT(mot != RT_NULL);
+    return RT_EOK;
+}
 
+static rt_err_t dual_pwm_motor_set_speed(void *mot, rt_int16_t thousands)
+{
     dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;
 
     if (thousands == 0)
@@ -98,6 +107,7 @@ dual_pwm_motor_t dual_pwm_motor_create(char *pwm1, int pwm1_channel, char *pwm2,
     new_motor->pwm2_channel = pwm2_channel;
     new_motor->mot.enable = dual_pwm_motor_enable;
     new_motor->mot.disable = dual_pwm_motor_disable;
+    new_motor->mot.reset = dual_pwm_motor_reset;
     new_motor->mot.set_speed = dual_pwm_motor_set_speed;
 
     return new_motor;

motor/dual_pwm_motor.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __DUAL_PWM_MOTOR_H__
 #define __DUAL_PWM_MOTOR_H__
 

motor/motor.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "motor.h"
 
 #define DBG_SECTION_NAME  "motor"
@@ -9,7 +19,7 @@ motor_t motor_create(rt_size_t size)
     motor_t new_motor = (motor_t)rt_malloc(size);
     if (new_motor == RT_NULL)
     {
-        LOG_E("Falied to allocate memory for motor");
+        LOG_E("Falied to allocate memory for new motor\n");
         return RT_NULL;
     }
 
@@ -43,7 +53,6 @@ rt_err_t motor_run(motor_t mot, rt_int16_t thousands)
     RT_ASSERT(mot != RT_NULL);
 
     // Set speed (pwm) to desired value
-    // LOG_D("Set motor speed %d pwm", pwm);
     mot->set_speed(mot, thousands);
 
     return RT_EOK;
@@ -73,3 +82,10 @@ rt_err_t motor_destroy(motor_t mot)
 
     return RT_EOK;
 }
+
+rt_err_t motor_reset(motor_t mot)
+{
+    RT_ASSERT(mot != RT_NULL);
+
+    return mot->reset(mot);
+}

motor/motor.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __MOTOR_H__
 #define __MOTOR_H__
 
@@ -13,11 +23,13 @@ struct motor
 {
     rt_err_t    (*enable)(void *mot);
     rt_err_t    (*disable)(void *mot);
+    rt_err_t    (*reset)(void *mot);
     rt_err_t    (*set_speed)(void *mot, rt_int16_t thousands);
 };
 
 motor_t  motor_create(rt_size_t size);
 rt_err_t motor_destroy(motor_t mot);
+rt_err_t motor_reset(motor_t mot);
 rt_err_t motor_enable(motor_t mot);
 rt_err_t motor_disable(motor_t mot);
 rt_err_t motor_run(motor_t motor, rt_int16_t thousands);

motor/servo.c

@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
+#include "servo.h"
+
+#define DBG_SECTION_NAME    "servo"
+#define DBG_LEVEL           DBG_LOG
+#include <rtdbg.h>
+
+/**
+ * @brief create a servo object
+ * @param pwm pwm device name
+ * @param channel pwm channel
+ * @param angle servo angle range
+ * @param pluse_min the minimum of pwm pluse width that servo can receive   (unit:ns
+ * @param pluse_max the maximum of pwm pluse width that servo can receive   (unit:ns
+ * @return the pointer of structure servo if success
+ */
+servo_t servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max)
+{
+    // Malloc memory for new servo
+    servo_t new_servo = (servo_t) rt_malloc(sizeof(struct servo));
+    if(new_servo == RT_NULL)
+    {
+        LOG_E("Falied to allocate memory for new servo");
+        return RT_NULL;
+    }
+
+    new_servo->pwmdev = (struct rt_device_pwm *)rt_device_find(pwm);
+    if (new_servo->pwmdev == RT_NULL)
+    {
+        LOG_E("Falied to find device on %s", pwm);
+        servo_destroy(new_servo);
+        return RT_NULL;
+    }
+    new_servo->channel = channel;
+    new_servo->angle_maximum = angle;
+    if (pluse_max == RT_NULL || pluse_min == RT_NULL)
+    {
+        new_servo->pluse_maximum = SERVO_DEFAULT_PULSE_MAX;
+        new_servo->pluse_minimum = SERVO_DEFAULT_PULSE_MIN;
+    }
+    else
+    {
+        new_servo->pluse_maximum = pluse_max;
+        new_servo->pluse_minimum = pluse_min;
+    }
+
+    return new_servo;
+}
+
+rt_err_t servo_destroy(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    LOG_D("Free servo");
+    rt_free(servo);
+
+    return RT_EOK;
+}
+
+rt_err_t servo_enable(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    LOG_D("Enabling servo");
+
+    return rt_pwm_enable(servo->pwmdev, servo->channel);
+}
+
+rt_err_t servo_disable(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    LOG_D("Disabling servo");
+
+    return rt_pwm_disable(servo->pwmdev, servo->channel);
+}
+
+rt_err_t servo_reset(servo_t servo)
+{
+    return RT_EOK;
+}
+
+rt_err_t servo_set_angle(servo_t servo, float angle)
+{
+    RT_ASSERT(servo != RT_NULL);
+    
+    rt_uint32_t set_point = servo->pluse_minimum + (servo->pluse_maximum - servo->pluse_minimum) * angle / servo->angle_maximum;
+    
+    return rt_pwm_set(servo->pwmdev, servo->channel, SERVO_PERIOD, set_point);
+}

motor/servo.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
+#ifndef __SERVO_H__
+#define __SERVO_H__
+
+#include <rtthread.h>
+#include <rtdevice.h>
+
+// 20ms; 1.5ms; +-1ms
+#define SERVO_PERIOD                    20000000
+#define SERVO_DEFAULT_PULSE_MAX         2500000
+#define SERVO_DEFAULT_PULSE_MIN         500000
+
+typedef struct servo *servo_t;
+
+struct servo
+{
+    struct rt_device_pwm    *pwmdev;
+    int                     channel;
+    float                   angle_maximum;
+    rt_uint32_t             pluse_maximum;
+    rt_uint32_t             pluse_minimum;
+};
+
+servo_t  servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max);
+rt_err_t servo_destroy(servo_t servo);
+rt_err_t servo_enable(servo_t servo);
+rt_err_t servo_disable(servo_t servo);
+rt_err_t servo_reset(servo_t servo);
+rt_err_t servo_set_angle(servo_t servo, float angle);
+
+#endif //  __SERVO_H__

motor/single_pwm_motor.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "single_pwm_motor.h"
 
 #define DBG_SECTION_NAME  "single_pwm_motor"
@@ -15,8 +25,6 @@ struct single_pwm_motor
 
 static rt_err_t single_pwm_motor_enable(void *mot)
 {
-    RT_ASSERT(mot != RT_NULL);
-
     single_pwm_motor_t mot_sub = (single_pwm_motor_t)mot;
     
     rt_pwm_enable(mot_sub->pwm_dev, mot_sub->channel);
@@ -26,8 +34,6 @@ static rt_err_t single_pwm_motor_enable(void *mot)
 
 static rt_err_t single_pwm_motor_disable(void *mot)
 {
-    RT_ASSERT(mot != RT_NULL);
-
     single_pwm_motor_t mot_sub = (single_pwm_motor_t)mot;
 
     rt_pwm_disable(mot_sub->pwm_dev, mot_sub->channel);
@@ -35,10 +41,13 @@ static rt_err_t single_pwm_motor_disable(void *mot)
     return RT_EOK;
 }
 
+static rt_err_t single_pwm_motor_reset(void *mot)
+{
+    return RT_EOK;
+}
+
 static rt_err_t single_pwm_motor_set_speed(void *mot, rt_int16_t thousands)
 {
-    RT_ASSERT(mot != RT_NULL);
-    
     single_pwm_motor_t mot_sub = (single_pwm_motor_t)mot;
 
     if (thousands == 0)
@@ -93,6 +102,7 @@ single_pwm_motor_t single_pwm_motor_create(char *pwm, int channel, rt_base_t pin
     new_motor->pin2 = pin2;
     new_motor->mot.enable = single_pwm_motor_enable;
     new_motor->mot.disable = single_pwm_motor_disable;
+    new_motor->mot.reset = single_pwm_motor_reset;
     new_motor->mot.set_speed = single_pwm_motor_set_speed;
 
     rt_pin_mode(new_motor->pin1, PIN_MODE_OUTPUT);

motor/single_pwm_motor.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __SINGLE_PWM_MOTOR_H__
 #define __SINGLE_PWM_MOTOR_H__
 

protocol/ano.c

@@ -0,0 +1,809 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
+#include "ano.h"
+
+#define DBG_SECTION_NAME  "ano"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+#define BYTE0(dwTemp) (*((uint8_t *)(&dwTemp)))
+#define BYTE1(dwTemp) (*((uint8_t *)(&dwTemp) + 1))
+#define BYTE2(dwTemp) (*((uint8_t *)(&dwTemp) + 2))
+#define BYTE3(dwTemp) (*((uint8_t *)(&dwTemp) + 3))
+
+#define PID_PARAM_FACTOR       1000.0f
+
+// Thread
+#define THREAD_STACK_SIZE      512
+#define THREAD_PRIORITY        ((RT_THREAD_PRIORITY_MAX / 3) + 2)
+#define THREAD_TICK            10
+
+static rt_thread_t tid_ano = RT_NULL;
+static rt_device_t dev_ano = RT_NULL;
+static rt_sem_t rx_sem = RT_NULL;
+
+static rt_err_t ano_sender_send(rt_uint16_t cmd, void *param, rt_uint16_t size)
+{
+    switch (cmd)
+    {
+    case COMMAND_SEND_PID:
+        if (size == 3 * sizeof(struct cmd_pid))
+        {
+            struct cmd_pid *pid_info = (struct cmd_pid *)param;
+            int group = (int)((pid_info[0].id + pid_info[1].id + pid_info[2].id) / 9) + 1;
+
+            if (group > 6)
+            {
+                group = 6;
+            }
+            if (group < 1)
+            {
+                group = 1;
+            }
+            ano_send_pid(group, pid_info[0].kp, pid_info[0].ki, pid_info[0].kd,
+                pid_info[1].kp, pid_info[1].ki, pid_info[1].kd,
+                pid_info[2].kp, pid_info[2].ki, pid_info[2].kd);
+        }
+        else
+        {
+            LOG_D("You need send three groups pid paramter at once when use COMMAND_SEND_PID");
+            return RT_ERROR;
+        }
+        
+        break;
+    case COMMAND_SEND_SENSOR:
+        if (size == sizeof(struct cmd_sensor))
+        {
+            struct cmd_sensor *sensor_info = (struct cmd_sensor *)param;
+            ano_send_senser(sensor_info->acc_x, sensor_info->acc_y, sensor_info->acc_z, 
+                sensor_info->gyro_x, sensor_info->gyro_y, sensor_info->gyro_z, 
+                sensor_info->mag_x, sensor_info->mag_y, sensor_info->mag_z, 0);
+        }
+        else
+        {
+            return RT_ERROR;
+        }
+        
+        break;
+    case COMMAND_SEND_RPY:
+        if (size == sizeof(struct cmd_rpy))
+        {
+            struct cmd_rpy *rpy_info = (struct cmd_rpy *)param;
+            ano_send_status(rpy_info->roll, rpy_info->pitch, rpy_info->yaw, 0,0,0);
+        }
+        else
+        {
+            return RT_ERROR;
+        }
+        
+        break;
+    default: return RT_ERROR;
+    }
+
+    return RT_EOK;
+}
+
+static struct command_sender ano_sender = {
+    .name = "ano",
+    .send = ano_sender_send
+};
+
+static int _send_data(uint8_t *buffer, uint8_t length)
+{
+    if (dev_ano != RT_NULL)
+    {
+        return rt_device_write(dev_ano, 0, buffer, length);
+    }
+
+    return RT_ERROR;
+}
+
+static void _get_pid_param(uint8_t *buffer, float *kpid)
+{
+    kpid[0] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 4) << 8) | *(buffer + 5)));
+    kpid[1] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 6) << 8) | *(buffer + 7)));
+    kpid[2] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 8) << 8) | *(buffer + 9)));
+    kpid[3] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 10) << 8) | *(buffer + 11)));
+    kpid[4] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 12) << 8) | *(buffer + 13)));
+    kpid[5] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 14) << 8) | *(buffer + 15)));
+    kpid[6] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 16) << 8) | *(buffer + 17)));
+    kpid[7] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 18) << 8) | *(buffer + 19)));
+    kpid[8] = (float)((1/PID_PARAM_FACTOR) * ((int16_t)(*(buffer + 20) << 8) | *(buffer + 21)));
+}
+
+static void ano_send_check(uint8_t head, uint8_t check_sum)
+{
+    uint8_t data_to_send[7];
+
+    data_to_send[0] = 0xAA;
+    data_to_send[1] = 0xAA;
+    data_to_send[2] = 0xEF;
+    data_to_send[3] = 2;
+    data_to_send[4] = head;
+    data_to_send[5] = check_sum;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < 6; i++)
+        sum += data_to_send[i];
+    data_to_send[6] = sum;
+
+    _send_data(data_to_send, 7);
+}
+
+static void ano_parse_frame(uint8_t *buffer, uint8_t length)
+{
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < (length - 1); i++)
+        sum += *(buffer + i);
+    if (!(sum == *(buffer + length - 1)))
+        return;
+    if (!(*(buffer) == 0xAA && *(buffer + 1) == 0xAF))
+        return;
+
+    if (*(buffer + 2) == 0X01)
+    {
+        if (*(buffer + 4) == 0X01)
+        {
+            // acc calibrate
+        }
+        else if (*(buffer + 4) == 0X02)
+        {
+            // gyro calibrate
+        }
+        else if (*(buffer + 4) == 0X04)
+        {
+            // mag calibrate
+        }
+    }
+    else if (*(buffer + 2) == 0X02)
+    {
+        if (*(buffer + 4) == 0X01)
+        {
+            struct cmd_pid pid[4];
+            if (RT_EOK == command_handle(COMMAND_GET_WHEELS_PID, pid, 4*sizeof(struct cmd_pid)))
+            {
+                ano_send_pid(1, 
+                    pid[0].kp, pid[0].ki, pid[0].kd,
+                    pid[1].kp, pid[1].ki, pid[1].kd,
+                    pid[2].kp, pid[2].ki, pid[2].kd);
+                ano_send_pid(2, 
+                    pid[3].kp, pid[3].ki, pid[3].kd,
+                    0.0f,0.0f,0.0f,0.0f,0.0f,0.0f);
+            }
+        }
+        else if (*(buffer + 4) == 0XA0)
+        {
+            // request version info
+        }
+        else if (*(buffer + 4) == 0XA1)
+        {
+            command_handle(COMMAND_SET_DEFAULT_PID, RT_NULL, 0);
+        }
+    }
+    else if (*(buffer + 2) == 0X10) //PID1
+    {
+        struct cmd_pid pid;
+        float kpid[9];
+        _get_pid_param(buffer, kpid);
+
+        pid.id = PID_ID_WHEEL_0;
+        pid.kp = kpid[0];
+        pid.ki = kpid[1];
+        pid.kd = kpid[2];
+        command_handle(COMMAND_SET_WHEEL0_PID, &pid, sizeof(struct cmd_pid));
+        pid.id = PID_ID_WHEEL_1;
+        pid.kp = kpid[3];
+        pid.ki = kpid[4];
+        pid.kd = kpid[5];
+        command_handle(COMMAND_SET_WHEEL1_PID, &pid, sizeof(struct cmd_pid));
+        pid.id = PID_ID_WHEEL_2;
+        pid.kp = kpid[6];
+        pid.ki = kpid[7];
+        pid.kd = kpid[8];
+        command_handle(COMMAND_SET_WHEEL2_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+    else if (*(buffer + 2) == 0X11) //PID2
+    {
+        struct cmd_pid pid;
+        float kpid[9];
+        _get_pid_param(buffer, kpid);
+        
+        pid.id = PID_ID_WHEEL_3;
+        pid.kp = kpid[0];
+        pid.ki = kpid[1];
+        pid.kd = kpid[2];
+        command_handle(COMMAND_SET_WHEEL3_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 5;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 6;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+    else if (*(buffer + 2) == 0X12) //PID3
+    {
+        // struct cmd_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
+        
+        // pid.id = 7;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 8;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 9;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+    else if (*(buffer + 2) == 0X13) //PID4
+    {
+        // struct cmd_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
+        
+        // pid.id = 10;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 11;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 12;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+    else if (*(buffer + 2) == 0X14) //PID5
+    {
+        // struct cmd_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
+        
+        // pid.id = 13;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 14;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 15;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+    else if (*(buffer + 2) == 0X15) //PID6
+    {
+        // struct cmd_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
+        
+        // pid.id = 16;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 17;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+        // pid.id = 18;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_pid));
+
+        ano_send_check(*(buffer + 2), sum);
+    }
+}
+
+static int ano_receive_byte(uint8_t data)
+{    
+    static uint8_t RxBuffer[50];
+    static uint8_t _data_len = 0, _data_cnt = 0;
+    static uint8_t state = 0;
+
+    if (state == 0 && data == 0xAA)
+    {
+        state = 1;
+        RxBuffer[0] = data;
+    }
+    else if (state == 1 && data == 0xAF)
+    {
+        state = 2;
+        RxBuffer[1] = data;
+    }
+    else if (state == 2 && data < 0XF1)
+    {
+        state = 3;
+        RxBuffer[2] = data;
+    }
+    else if (state == 3 && data < 50)
+    {
+        state = 4;
+        RxBuffer[3] = data;
+        _data_len = data;
+        _data_cnt = 0;
+    }
+    else if (state == 4 && _data_len > 0)
+    {
+        _data_len--;
+        RxBuffer[4 + _data_cnt++] = data;
+        if (_data_len == 0)
+            state = 5;
+    }
+    else if (state == 5)
+    {
+        state = 0;
+        RxBuffer[4 + _data_cnt] = data;
+        ano_parse_frame(RxBuffer, _data_cnt + 5);
+        return 1;
+    }
+    else
+        state = 0;
+    
+    return 0;
+}
+
+int ano_send_version(uint8_t hardware_type, uint16_t hardware_ver, uint16_t software_ver, uint16_t protocol_ver, uint16_t bootloader_ver)
+{
+    uint8_t data_to_send[14];
+    uint8_t _cnt = 0;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x00;
+    data_to_send[_cnt++] = 0;
+
+    data_to_send[_cnt++] = hardware_type;
+    data_to_send[_cnt++] = BYTE1(hardware_ver);
+    data_to_send[_cnt++] = BYTE0(hardware_ver);
+    data_to_send[_cnt++] = BYTE1(software_ver);
+    data_to_send[_cnt++] = BYTE0(software_ver);
+    data_to_send[_cnt++] = BYTE1(protocol_ver);
+    data_to_send[_cnt++] = BYTE0(protocol_ver);
+    data_to_send[_cnt++] = BYTE1(bootloader_ver);
+    data_to_send[_cnt++] = BYTE0(bootloader_ver);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_status(float angle_rol, float angle_pit, float angle_yaw, int32_t alt, uint8_t fly_model, uint8_t armed)
+{
+    uint8_t data_to_send[17];
+    uint8_t _cnt = 0;
+    volatile int16_t _temp;
+    volatile int32_t _temp2 = alt;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x01;
+    data_to_send[_cnt++] = 0;
+
+    _temp = (int)(angle_rol * 100);
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = (int)(angle_pit * 100);
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = (int)(angle_yaw * 100);
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+
+    data_to_send[_cnt++] = BYTE3(_temp2);
+    data_to_send[_cnt++] = BYTE2(_temp2);
+    data_to_send[_cnt++] = BYTE1(_temp2);
+    data_to_send[_cnt++] = BYTE0(_temp2);
+
+    data_to_send[_cnt++] = fly_model;
+
+    data_to_send[_cnt++] = armed;
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_senser(int16_t a_x, int16_t a_y, int16_t a_z, int16_t g_x, int16_t g_y, int16_t g_z, int16_t m_x, int16_t m_y, int16_t m_z, int32_t bar)
+{
+    uint8_t data_to_send[23];
+    uint8_t _cnt = 0;
+    volatile int16_t _temp;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x02;
+    data_to_send[_cnt++] = 0;
+
+    _temp = a_x;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = a_y;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = a_z;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+
+    _temp = g_x;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = g_y;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = g_z;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+
+    _temp = m_x;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = m_y;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = m_z;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_rcdata(uint16_t thr, uint16_t yaw, uint16_t rol, uint16_t pit, uint16_t aux1, uint16_t aux2, uint16_t aux3, uint16_t aux4, uint16_t aux5, uint16_t aux6)
+{
+    uint8_t data_to_send[25];
+    uint8_t _cnt = 0;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x03;
+    data_to_send[_cnt++] = 0;
+    data_to_send[_cnt++] = BYTE1(thr);
+    data_to_send[_cnt++] = BYTE0(thr);
+    data_to_send[_cnt++] = BYTE1(yaw);
+    data_to_send[_cnt++] = BYTE0(yaw);
+    data_to_send[_cnt++] = BYTE1(rol);
+    data_to_send[_cnt++] = BYTE0(rol);
+    data_to_send[_cnt++] = BYTE1(pit);
+    data_to_send[_cnt++] = BYTE0(pit);
+    data_to_send[_cnt++] = BYTE1(aux1);
+    data_to_send[_cnt++] = BYTE0(aux1);
+    data_to_send[_cnt++] = BYTE1(aux2);
+    data_to_send[_cnt++] = BYTE0(aux2);
+    data_to_send[_cnt++] = BYTE1(aux3);
+    data_to_send[_cnt++] = BYTE0(aux3);
+    data_to_send[_cnt++] = BYTE1(aux4);
+    data_to_send[_cnt++] = BYTE0(aux4);
+    data_to_send[_cnt++] = BYTE1(aux5);
+    data_to_send[_cnt++] = BYTE0(aux5);
+    data_to_send[_cnt++] = BYTE1(aux6);
+    data_to_send[_cnt++] = BYTE0(aux6);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_power(uint16_t votage, uint16_t current)
+{
+    uint8_t data_to_send[9];
+    uint8_t _cnt = 0;
+    uint16_t temp;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x05;
+    data_to_send[_cnt++] = 0;
+
+    temp = votage;
+    data_to_send[_cnt++] = BYTE1(temp);
+    data_to_send[_cnt++] = BYTE0(temp);
+    temp = current;
+    data_to_send[_cnt++] = BYTE1(temp);
+    data_to_send[_cnt++] = BYTE0(temp);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_motorpwm(uint16_t m_1, uint16_t m_2, uint16_t m_3, uint16_t m_4, uint16_t m_5, uint16_t m_6, uint16_t m_7, uint16_t m_8)
+{
+    uint8_t data_to_send[21];
+    uint8_t _cnt = 0;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x06;
+    data_to_send[_cnt++] = 0;
+
+    data_to_send[_cnt++] = BYTE1(m_1);
+    data_to_send[_cnt++] = BYTE0(m_1);
+    data_to_send[_cnt++] = BYTE1(m_2);
+    data_to_send[_cnt++] = BYTE0(m_2);
+    data_to_send[_cnt++] = BYTE1(m_3);
+    data_to_send[_cnt++] = BYTE0(m_3);
+    data_to_send[_cnt++] = BYTE1(m_4);
+    data_to_send[_cnt++] = BYTE0(m_4);
+    data_to_send[_cnt++] = BYTE1(m_5);
+    data_to_send[_cnt++] = BYTE0(m_5);
+    data_to_send[_cnt++] = BYTE1(m_6);
+    data_to_send[_cnt++] = BYTE0(m_6);
+    data_to_send[_cnt++] = BYTE1(m_7);
+    data_to_send[_cnt++] = BYTE0(m_7);
+    data_to_send[_cnt++] = BYTE1(m_8);
+    data_to_send[_cnt++] = BYTE0(m_8);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_pid(uint8_t group, float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+    uint8_t data_to_send[23];
+    uint8_t _cnt = 0;
+    volatile int16_t _temp;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0x10 + group - 1;
+    data_to_send[_cnt++] = 0;
+
+    _temp = k1_p * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k1_i * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k1_d * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k2_p * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k2_i * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k2_d * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k3_p * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k3_i * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+    _temp = k3_d * PID_PARAM_FACTOR;
+    data_to_send[_cnt++] = BYTE1(_temp);
+    data_to_send[_cnt++] = BYTE0(_temp);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+int ano_send_user_data(uint8_t number, float d0, float d1, float d2, float d3, float d4, float d5, int16_t d6, int16_t d7, int16_t d8)
+{
+    uint8_t data_to_send[35];
+    uint8_t _cnt = 0;
+
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xAA;
+    data_to_send[_cnt++] = 0xF0 + number;
+    data_to_send[_cnt++] = 0;
+
+    data_to_send[_cnt++] = BYTE3(d0);
+    data_to_send[_cnt++] = BYTE2(d0);
+    data_to_send[_cnt++] = BYTE1(d0);
+    data_to_send[_cnt++] = BYTE0(d0);
+
+    data_to_send[_cnt++] = BYTE3(d1);
+    data_to_send[_cnt++] = BYTE2(d1);
+    data_to_send[_cnt++] = BYTE1(d1);
+    data_to_send[_cnt++] = BYTE0(d1);
+
+    data_to_send[_cnt++] = BYTE3(d2);
+    data_to_send[_cnt++] = BYTE2(d2);
+    data_to_send[_cnt++] = BYTE1(d2);
+    data_to_send[_cnt++] = BYTE0(d2);
+
+    data_to_send[_cnt++] = BYTE3(d3);
+    data_to_send[_cnt++] = BYTE2(d3);
+    data_to_send[_cnt++] = BYTE1(d3);
+    data_to_send[_cnt++] = BYTE0(d3);
+
+    data_to_send[_cnt++] = BYTE3(d4);
+    data_to_send[_cnt++] = BYTE2(d4);
+    data_to_send[_cnt++] = BYTE1(d4);
+    data_to_send[_cnt++] = BYTE0(d4);
+
+    data_to_send[_cnt++] = BYTE3(d5);
+    data_to_send[_cnt++] = BYTE2(d5);
+    data_to_send[_cnt++] = BYTE1(d5);
+    data_to_send[_cnt++] = BYTE0(d5);
+
+    data_to_send[_cnt++] = BYTE1(d6);
+    data_to_send[_cnt++] = BYTE0(d6);
+    data_to_send[_cnt++] = BYTE1(d7);
+    data_to_send[_cnt++] = BYTE0(d7);
+    data_to_send[_cnt++] = BYTE1(d8);
+    data_to_send[_cnt++] = BYTE0(d8);
+
+    data_to_send[3] = _cnt - 4;
+
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < _cnt; i++)
+        sum += data_to_send[i];
+
+    data_to_send[_cnt++] = sum;
+
+    return _send_data(data_to_send, _cnt);
+}
+
+static uint8_t ano_getbyte(void)
+{
+    uint8_t tmp;
+    
+    while (rt_device_read(dev_ano, -1, &tmp, 1) != 1)
+        rt_sem_take(rx_sem, RT_WAITING_FOREVER);
+
+    return tmp;
+}
+
+static rt_err_t ano_rx_ind(rt_device_t dev, rt_size_t size)
+{
+    rt_sem_release(rx_sem);
+
+    return RT_EOK;
+}
+
+int ano_set_device(const char *device_name)
+{
+    rt_device_t dev = RT_NULL;
+
+    dev = rt_device_find(device_name);
+    if (dev == RT_NULL)
+    {
+        LOG_E("Can not find device: %s\n", device_name);
+        return RT_ERROR;
+    }
+
+    /* check whether it's a same device */
+    if (dev == dev_ano) return RT_ERROR;
+    /* open this device and set the new device in finsh shell */
+    if (rt_device_open(dev, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX) == RT_EOK)
+    {
+        if (dev_ano != RT_NULL)
+        {
+            /* close old finsh device */
+            rt_device_close(dev_ano);
+            rt_device_set_rx_indicate(dev_ano, RT_NULL);
+        }
+
+        dev_ano = dev;
+        rt_device_set_rx_indicate(dev_ano, ano_rx_ind);
+    }
+
+    return RT_EOK;
+}
+
+command_sender_t ano_get_sender(void)
+{
+    return &ano_sender;
+}
+
+static void ano_thread_entry(void *param)
+{
+    while(1)
+    {
+        ano_receive_byte(ano_getbyte());
+    }
+}
+
+int ano_init(void *param)
+{
+    if (ano_set_device((char *)param) != RT_EOK)
+    {
+        LOG_E("Failed to find device");
+        return RT_ERROR;
+    }
+
+    rx_sem = rt_sem_create("anoRx", 0, RT_IPC_FLAG_FIFO);
+    if (rx_sem == RT_NULL)
+    {
+        LOG_E("Failed to create sem\n");
+        return RT_ERROR;
+    }
+
+    tid_ano = rt_thread_create("ano", ano_thread_entry, RT_NULL, THREAD_STACK_SIZE, THREAD_PRIORITY, THREAD_TICK);
+    if (tid_ano == RT_NULL)
+    {
+        LOG_E("Failed to create thread\n");
+        return RT_ERROR;
+    }
+
+    rt_thread_startup(tid_ano);
+
+    LOG_D("ano thread start");
+
+    return RT_EOK;
+}

protocol/ano.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
+#ifndef __ANO_H__
+#define	__ANO_H__
+
+#include <rtthread.h>
+#include "command.h"
+
+int ano_send_version(uint8_t hardware_type, uint16_t hardware_ver, uint16_t software_ver, uint16_t protocol_ver, uint16_t bootloader_ver);
+int ano_send_status(float angle_rol, float angle_pit, float angle_yaw, int32_t alt, uint8_t fly_model, uint8_t armed);
+int ano_send_senser(int16_t a_x, int16_t a_y, int16_t a_z, int16_t g_x, int16_t g_y, int16_t g_z, int16_t m_x, int16_t m_y, int16_t m_z, int32_t bar);
+int ano_send_rcdata(uint16_t thr, uint16_t yaw, uint16_t rol, uint16_t pit, uint16_t aux1, uint16_t aux2, uint16_t aux3, uint16_t aux4, uint16_t aux5, uint16_t aux6);
+int ano_send_power(uint16_t votage, uint16_t current);
+int ano_send_motorpwm(uint16_t m_1, uint16_t m_2, uint16_t m_3, uint16_t m_4, uint16_t m_5, uint16_t m_6, uint16_t m_7, uint16_t m_8);
+int ano_send_pid(uint8_t group, float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d);
+int ano_send_user_data(uint8_t number, float d0, float d1, float d2, float d3, float d4, float d5, int16_t d6, int16_t d7, int16_t d8);
+int ano_set_device(const char *device_name);
+int ano_init(void *param);
+
+command_sender_t ano_get_sender(void);
+
+#endif

protocol/command.c

@@ -1,58 +1,294 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include <command.h>
 
 #define DBG_SECTION_NAME  "command"
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
-#define TABLE_MAX_SIZE      32
+// Thread
+#define THREAD_PRIORITY             ((RT_THREAD_PRIORITY_MAX / 3) + 1)
+#define THREAD_STACK_SIZE           512
+#define THREAD_TIMESLICE            10
+static rt_thread_t cmd_tid = RT_NULL;
+
+//
+struct command_info
+{
+    rt_uint16_t         cmd;
+};
 
-static struct command command_table[TABLE_MAX_SIZE];
-static uint8_t command_table_size = 0;
+typedef struct command_info *command_info_t;
 
-rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param))
+// Message Queue
+struct command_msg
 {
-    if (command_table_size > TABLE_MAX_SIZE - 1)
+    struct command_info info;
+    void                *param;
+    rt_uint16_t         size;
+};
+#define MAX_MSGS                    32
+static rt_mq_t cmd_mq = RT_NULL;
+
+// Target
+chassis_t g_chas;
+
+static rt_err_t command_handle_get(struct command_msg *msg)
+{
+    switch (msg->info.cmd)
     {
-        LOG_E("Command table voerflow");
-        return RT_ERROR;
+    case COMMAND_GET_WHEEL0_PID:
+    case COMMAND_GET_WHEEL1_PID:
+    case COMMAND_GET_WHEEL2_PID:
+    case COMMAND_GET_WHEEL3_PID:
+        if (msg->size == sizeof(struct cmd_pid))
+        {
+            struct cmd_pid *ppid = msg->param;
+            struct controller_param parameter;
+
+            controller_get_param(g_chas->c_wheels[msg->info.cmd - COMMAND_GET_WHEEL0_PID]->w_controller, &parameter);
+            ppid->kp = parameter.data.pid.kp;
+            ppid->ki = parameter.data.pid.ki;
+            ppid->kd = parameter.data.pid.kd;
+        }
+        else
+        {
+            return RT_ERROR;
+        }
+        break;
+    case COMMAND_GET_WHEELS_PID:
+        if (msg->size >= 2 * sizeof(struct cmd_pid))
+        {
+            struct cmd_pid *ppid = msg->param;
+            struct controller_param parameter;
+
+            for (int i = 0; (i < g_chas->c_kinematics->total_wheels) && (i < (msg->size / sizeof(struct cmd_pid))); i++)
+            {
+                controller_get_param(g_chas->c_wheels[i]->w_controller, &parameter);
+                ppid[i].id = COMMAND_GET_WHEEL0_PID + i;
+                ppid[i].kp = parameter.data.pid.kp;
+                ppid[i].ki = parameter.data.pid.ki;
+                ppid[i].kd = parameter.data.pid.kd;
+            }
+        }
+        else
+        {
+            return RT_ERROR;
+        }
+        break;
+    
+    default: return RT_ERROR;
     }
-    command_table[command_table_size].robot_cmd = cmd;
-    command_table[command_table_size].handler = handler;
-    command_table_size += 1;
 
     return RT_EOK;
 }
 
-rt_err_t command_unregister(rt_int8_t cmd)
+rt_err_t command_handle(rt_uint16_t cmd, void *param, rt_uint16_t size)
 {
-    // TODO
+    struct command_msg msg = {
+        .param = param,
+        .size = size,
+        .info = {
+            .cmd = cmd,
+        }
+    };
 
-    return RT_EOK;
+    if (cmd > COMMAND_GET_START && cmd < COMMAND_GET_END)
+    {
+        return command_handle_get(&msg);
+    }
+
+    return rt_mq_send(cmd_mq, &msg, sizeof(struct command_msg));
 }
 
-rt_err_t command_handle(rt_int8_t cmd, void *param)
+rt_err_t command_send(command_sender_t sender, rt_uint16_t cmd, void *param, rt_uint16_t size)
 {
-    // look-up table and call callback
-    for (uint16_t i = 0; i < command_table_size; i++)
+    if (sender->send != RT_NULL)
     {
-        if (command_table[i].robot_cmd == cmd)
-        {
-            command_table[i].handler(command_table[i].robot_cmd, param);
-            return RT_EOK;
-        }
+        return sender->send(cmd, param, size);
     }
 
     return RT_ERROR;
 }
 
-rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len)
+static void command_thread_entry(void *param)
 {
-    if (sender->send != RT_NULL)
+    struct command_msg msg;
+
+    while (1)
     {
-        sender->send(cmd, param, len);
+        rt_mq_recv(cmd_mq, &msg, sizeof(struct command_msg), RT_WAITING_FOREVER);
+
+        if (g_chas == RT_NULL)
+        {
+            LOG_D("The target is NULL");
+            continue;
+        }
+
+        switch (msg.info.cmd)
+        {
+        case COMMAND_SET_WHEELS_PID:
+            if (msg.size >= 2 * sizeof(struct cmd_pid))
+            {
+                struct cmd_pid *ppid = msg.param;
+                struct controller_param parameter;
+                for (int i = 0; i < g_chas->c_kinematics->total_wheels; i++)
+                {
+                    parameter.data.pid.kp = ppid[i].kp;
+                    parameter.data.pid.ki = ppid[i].ki;
+                    parameter.data.pid.kd = ppid[i].kd;
+                    controller_set_param(g_chas->c_wheels[ppid[i].id]->w_controller, &parameter);
+                }
+            }
+            break;
+        case COMMAND_SET_WHEEL0_PID:
+        case COMMAND_SET_WHEEL1_PID:
+        case COMMAND_SET_WHEEL2_PID:
+        case COMMAND_SET_WHEEL3_PID:
+            if (msg.size == sizeof(struct cmd_pid))
+            {
+                if (g_chas->c_kinematics->total_wheels > msg.info.cmd - COMMAND_SET_WHEEL0_PID)
+                {
+                    struct cmd_pid *ppid = msg.param;
+                    struct controller_param parameter = {
+                        .data.pid.kp = ppid->kp,
+                        .data.pid.ki = ppid->ki,
+                        .data.pid.kd = ppid->kd
+                    };
+                    controller_set_param(g_chas->c_wheels[msg.info.cmd - COMMAND_SET_WHEEL0_PID]->w_controller, &parameter);
+                }
+            }
+            break;
+        case COMMAND_SET_CHASSIS_STOP:
+            chassis_move(g_chas, 0.0f);
+            break;
+        case COMMAND_SET_CHASSIS_FORWARD:
+            chassis_straight(g_chas, CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_BACKWARD:
+            chassis_straight(g_chas, -CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_ROTATE_LEFT:
+            chassis_rotate(g_chas, CHASSIS_VELOCITY_ANGULAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_ROTATE_RIGHT:
+            chassis_rotate(g_chas, -CHASSIS_VELOCITY_ANGULAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_MOVE_LEFT:
+            chassis_move(g_chas, CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_MOVE_RIGHT:
+            chassis_move(g_chas, -CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
+            break;
+        case COMMAND_SET_CHASSIS_FORWARD_WITH_PARAM:
+        case COMMAND_SET_CHASSIS_BACKWARD_WITH_PARAM:
+        case COMMAND_SET_CHASSIS_ROTATE_LEFT_WITH_PARAM:
+        case COMMAND_SET_CHASSIS_ROTATE_RIGHT_WITH_PARAM:
+        case COMMAND_SET_CHASSIS_MOVE_LEFT_WITH_PARAM:
+        case COMMAND_SET_CHASSIS_MOVE_RIGHT_WITH_PARAM:
+            if (msg.size == sizeof(struct cmd_velocity))
+            {
+                struct cmd_velocity *target_velocity = (struct cmd_velocity *)msg.param;
+                rt_err_t (*p_fun)(chassis_t chas, float data);
+                float tmp;
+                if (msg.info.cmd == COMMAND_SET_CHASSIS_FORWARD_WITH_PARAM)
+                {
+                    tmp = target_velocity->data.linear_x;
+                    p_fun = chassis_straight;
+                }
+                else if (msg.info.cmd == COMMAND_SET_CHASSIS_BACKWARD_WITH_PARAM)
+                {
+                    tmp = -target_velocity->data.linear_x;
+                    p_fun = chassis_straight;
+                }
+                else if (msg.info.cmd == COMMAND_SET_CHASSIS_ROTATE_LEFT_WITH_PARAM)
+                {
+                    tmp = target_velocity->data.angular_z;
+                    p_fun = chassis_rotate;
+                }
+                else if (msg.info.cmd == COMMAND_SET_CHASSIS_ROTATE_RIGHT_WITH_PARAM)
+                {
+                    tmp = -target_velocity->data.angular_z;
+                    p_fun = chassis_rotate;
+                }
+                else if (msg.info.cmd == COMMAND_SET_CHASSIS_MOVE_LEFT_WITH_PARAM)
+                {
+                    tmp = target_velocity->data.linear_y;
+                    p_fun = chassis_move;
+                }
+                else
+                {
+                    tmp = -target_velocity->data.linear_y;
+                    p_fun = chassis_move;
+                }
+                
+                p_fun(g_chas, tmp);
+            }
+            break;
+        case COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X:
+        case COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y:
+        case COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z:
+            if (msg.size == sizeof(struct cmd_velocity))
+            {
+                struct cmd_velocity *target_velocity = (struct cmd_velocity *)msg.param;
+                rt_err_t (*p_fun)(chassis_t chas, float data);
+                float tmp;
+                if (msg.info.cmd == COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X)
+                {
+                    tmp = target_velocity->data.linear_x;
+                    p_fun = chassis_set_velocity_x;
+                }
+                else if (msg.info.cmd == COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y)
+                {
+                    tmp = target_velocity->data.linear_y;
+                    p_fun = chassis_set_velocity_y;
+                }
+                else
+                {
+                    tmp = target_velocity->data.angular_z;
+                    p_fun = chassis_set_velocity_z;
+                }
+
+                p_fun(g_chas, tmp);
+            }
+            break;
+        default:
+            break;
+        }
     }
-    
-    return RT_EOK;
 }
 
+rt_err_t command_init(chassis_t chas)
+{
+    cmd_mq = rt_mq_create("command", sizeof(struct command_msg), MAX_MSGS, RT_IPC_FLAG_FIFO);
+    if (cmd_mq == RT_NULL)
+    {
+        LOG_E("Failed to creat meassage queue");
+        return RT_ERROR;
+    }
+
+    cmd_tid = rt_thread_create("command",
+                              command_thread_entry, RT_NULL,
+                              THREAD_STACK_SIZE,
+                              THREAD_PRIORITY, THREAD_TIMESLICE);
+
+    if (cmd_tid == RT_NULL)
+    {
+        LOG_E("Failed to creat thread");
+        return RT_ERROR;
+    }
+
+    g_chas = chas;
 
+    rt_thread_startup(cmd_tid);
+
+    return RT_EOK;
+}

protocol/command.h

@@ -1,36 +1,117 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __COMMAND_H__
 #define __COMMAND_H__
 
 #include <rtthread.h>
+#include <chassis.h>
 
-// command
-#define COMMAND_NONE                    0
-#define COMMAND_CAR_STOP                1
-#define COMMAND_CAR_FORWARD             2
-#define COMMAND_CAR_BACKWARD            3
-#define COMMAND_CAR_TURNLEFT            4
-#define COMMAND_CAR_TURNRIGHT           5
-#define COMMAND_GET_CAR_SPEED           6
+/* Command */
+// down-link
+#define COMMAND_NONE                                    (rt_uint16_t)(0X0000)
+#define COMMAND_SET_CHASSIS_STOP                        (rt_uint16_t)(0X0001)
+#define COMMAND_SET_CHASSIS_FORWARD                     (rt_uint16_t)(0X0002)
+#define COMMAND_SET_CHASSIS_BACKWARD                    (rt_uint16_t)(0X0003)
+#define COMMAND_SET_CHASSIS_ROTATE_LEFT                 (rt_uint16_t)(0X0004)
+#define COMMAND_SET_CHASSIS_ROTATE_RIGHT                (rt_uint16_t)(0X0005)
+#define COMMAND_SET_CHASSIS_MOVE_LEFT                   (rt_uint16_t)(0X0006)
+#define COMMAND_SET_CHASSIS_MOVE_RIGHT                  (rt_uint16_t)(0X0007)
 
-#define COMMAND_CONTROLLER_VIBRATE      100
+#define COMMAND_SET_CHASSIS_FORWARD_WITH_PARAM          (rt_uint16_t)(0X1002)
+#define COMMAND_SET_CHASSIS_BACKWARD_WITH_PARAM         (rt_uint16_t)(0X1003)
+#define COMMAND_SET_CHASSIS_ROTATE_LEFT_WITH_PARAM      (rt_uint16_t)(0X1004)
+#define COMMAND_SET_CHASSIS_ROTATE_RIGHT_WITH_PARAM     (rt_uint16_t)(0X1005)
+#define COMMAND_SET_CHASSIS_MOVE_LEFT_WITH_PARAM        (rt_uint16_t)(0X1006)
+#define COMMAND_SET_CHASSIS_MOVE_RIGHT_WITH_PARAM       (rt_uint16_t)(0X1007)
+#define COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X           (rt_uint16_t)(0x1008)
+#define COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y           (rt_uint16_t)(0x1009)
+#define COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z          (rt_uint16_t)(0x100A)
 
-typedef struct command_sender *command_sender_t;
+#define COMMAND_SET_WHEEL0_PID                          (rt_uint16_t)(0x2000)
+#define COMMAND_SET_WHEEL1_PID                          (rt_uint16_t)(0x2001)
+#define COMMAND_SET_WHEEL2_PID                          (rt_uint16_t)(0x2002)
+#define COMMAND_SET_WHEEL3_PID                          (rt_uint16_t)(0x2003)
+#define COMMAND_SET_WHEELS_PID                          (rt_uint16_t)(0x2004)
+#define COMMAND_SET_DEFAULT_PID                         (rt_uint16_t)(0x3000)
 
-struct command_sender
+// ! Must keep all COMMAND_GET_XXX value is between COMMAND_GET_START and COMMAND_GET_END
+#define COMMAND_GET_START                               (rt_uint16_t)(0x5000)
+#define COMMAND_GET_WHEEL0_PID                          (rt_uint16_t)(0x5001)
+#define COMMAND_GET_WHEEL1_PID                          (rt_uint16_t)(0x5002)
+#define COMMAND_GET_WHEEL2_PID                          (rt_uint16_t)(0x5003)
+#define COMMAND_GET_WHEEL3_PID                          (rt_uint16_t)(0x5004)
+#define COMMAND_GET_WHEELS_PID                          (rt_uint16_t)(0x5005)
+#define COMMAND_GET_END                                 (rt_uint16_t)(0x8FFF)
+
+// up-link
+#define COMMAND_RC_VIBRATE                              (rt_uint16_t)(0x9000)
+#define COMMAND_SEND_PID                                (rt_uint16_t)(0xA000)
+#define COMMAND_SEND_SENSOR                             (rt_uint16_t)(0xA001)
+#define COMMAND_SEND_RPY                                (rt_uint16_t)(0xA002)
+
+// PID ID
+#define PID_ID_WHEEL_0                          1
+#define PID_ID_WHEEL_1                          2
+#define PID_ID_WHEEL_2                          3
+#define PID_ID_WHEEL_3                          4
+
+struct cmd_pid
 {
-    char *name;
-    void (*send)(rt_int8_t cmd, void *param, rt_uint16_t len);
+    int   id;       // range: 1 ~ max
+    float kp;
+    float ki;
+    float kd;
+};
+
+struct cmd_sensor
+{
+    int32_t acc_x;
+    int32_t acc_y;
+    int32_t acc_z;
+    int32_t gyro_x;
+    int32_t gyro_y;
+    int32_t gyro_z;
+    int32_t mag_x;
+    int32_t mag_y;
+    int32_t mag_z;
 };
 
-struct command
+struct cmd_rpy
 {
-    rt_int8_t   robot_cmd;
-    rt_err_t    (*handler)(rt_int8_t cmd, void *param);
+    float roll;
+    float pitch;
+    float yaw;
 };
 
-rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param));
-rt_err_t command_unregister(rt_int8_t cmd);
-rt_err_t command_handle(rt_int8_t cmd, void *param);
-rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len);
+struct cmd_velocity
+{
+    union
+    {
+        float linear_x;     // m/s
+        float linear_y;
+        float angular_z;
+        float common;
+    } data; 
+};
+
+struct command_sender
+{
+    char *name;
+    rt_err_t (*send)(rt_uint16_t cmd, void *param, rt_uint16_t size);
+};
+
+typedef struct command_sender *command_sender_t;
+
+rt_err_t command_handle(rt_uint16_t cmd, void *param, rt_uint16_t size);
+rt_err_t command_send(command_sender_t sender, rt_uint16_t cmd, void *param, rt_uint16_t size);
+rt_err_t command_init(chassis_t chas);
 
 #endif

protocol/ps2.c

@@ -1,6 +1,12 @@
-/*********************************************************
-Change From YFRobot. www.yfrobot.com
-**********************************************************/
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
 
 #include "ps2.h"
 #include "ps2_table.h"
@@ -9,9 +15,9 @@ Change From YFRobot. www.yfrobot.com
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
-#define THREAD_DELAY_TIME           50
+#define THREAD_DELAY_TIME           30
 
-#define THREAD_PRIORITY             16
+#define THREAD_PRIORITY             ((RT_THREAD_PRIORITY_MAX / 3) + 3)
 #define THREAD_STACK_SIZE           1024
 #define THREAD_TIMESLICE            10
 
@@ -109,7 +115,7 @@ int ps2_scan(ps2_ctrl_data_t pt)
     temp[4] = 0;
 
     transfer(temp, temp, 9);
-    
+
     pt->button = temp[3] | (temp[4] << 8);
     pt->right_stick_x = temp[5];
     pt->right_stick_y = temp[6];
@@ -137,9 +143,36 @@ int ps2_read_light(void)
     return light_mode;
 }
 
+static rt_err_t ps2_sender_send(rt_uint16_t cmd, void *param, rt_uint16_t size)
+{
+    // TODO
+
+    if (cmd == COMMAND_RC_VIBRATE)
+    {
+
+    }
+    else
+    {
+        return RT_ERROR;
+    }
+
+    return RT_EOK;
+}
+
+static struct command_sender ps2_sender = {
+    .name = "ano",
+    .send = ps2_sender_send
+};
+
+command_sender_t ps2_get_sender(void)
+{
+    return &ps2_sender;
+}
+
 static void ps2_thread_entry(void *param)
 {
     struct ps2_ctrl_data ctrl_data;
+    struct cmd_velocity target_velocity;
 
     while (1)
     {
@@ -152,7 +185,7 @@ static void ps2_thread_entry(void *param)
             {
                 if (table[i].standard_cmd != COMMAND_NONE)
                 {
-                    command_handle(table[i].standard_cmd, RT_NULL);
+                    command_handle(table[i].standard_cmd, RT_NULL, 0);
                 }
             }
         }
@@ -160,21 +193,31 @@ static void ps2_thread_entry(void *param)
         // rocker
         if (ps2_read_light() == PS2_RED_MODE)
         {
-            if (table[PS2_ROCKER_LX].standard_cmd != COMMAND_NONE)
-            {
-                command_handle(table[PS2_ROCKER_LX].standard_cmd, &ctrl_data.left_stick_x);
-            }
-            if (table[PS2_ROCKER_LY].standard_cmd != COMMAND_NONE)
-            {
-                command_handle(table[PS2_ROCKER_LY].standard_cmd, &ctrl_data.left_stick_y);
-            }
-            if (table[PS2_ROCKER_RX].standard_cmd != COMMAND_NONE)
-            {
-                command_handle(table[PS2_ROCKER_RX].standard_cmd, &ctrl_data.right_stick_x);
-            }
-            if (table[PS2_ROCKER_RY].standard_cmd != COMMAND_NONE)
+            uint8_t value[4] = {
+                ctrl_data.left_stick_x, 
+                ctrl_data.right_stick_x,
+                ctrl_data.left_stick_y,
+                ctrl_data.right_stick_y};
+
+            rt_int16_t cmd[4] = {
+                table[PS2_ROCKER_LX].standard_cmd,
+                table[PS2_ROCKER_LY].standard_cmd,
+                table[PS2_ROCKER_RX].standard_cmd,
+                table[PS2_ROCKER_RY].standard_cmd};
+
+            for (int i = 0; i < 4; i++)
             {
-                command_handle(table[PS2_ROCKER_RY].standard_cmd, &ctrl_data.right_stick_y);
+                if (cmd[i] != COMMAND_NONE)
+                {
+                    float tmp = CHASSIS_VELOCITY_LINEAR_MAXIMUM;
+                    if (cmd[i] == COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z)
+                    {
+                        tmp = CHASSIS_VELOCITY_ANGULAR_MAXIMUM;
+                    }
+                    
+                    target_velocity.data.common = tmp * ((0x80 - (int)(i/2)) - value[i]) / 128;
+                    command_handle(cmd[i], &target_velocity, sizeof(struct cmd_velocity));
+                }
             }
         }
     }
@@ -195,6 +238,8 @@ void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t d
     hal_cs_high();
     hal_clk_high();
 
+    // cmd_info.target = target;
+
     tid_ps2 = rt_thread_create("ps2",
                           ps2_thread_entry, RT_NULL,
                           THREAD_STACK_SIZE,

protocol/ps2.h

@@ -1,9 +1,19 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #ifndef __PS2_H__
 #define __PS2_H__
 
 #include <rtthread.h>
 #include <rtdevice.h>
-#include <command.h>
+#include "command.h"
 
 // COMMAND
 #define PS2_CMD_VIBRATE     1
@@ -13,7 +23,7 @@
 #define PS2_GREEN_MODE      0x41
 #define PS2_RED_MODE        0x73
 
-// KEY
+// BUTTON
 #define PS2_BTN_SELECT      (1 << 0)
 #define PS2_BTN_L3          (1 << 1)
 #define PS2_BTN_R3          (1 << 2)
@@ -51,4 +61,7 @@ void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t d
 int  ps2_scan(ps2_ctrl_data_t pt);
 int  ps2_read_light(void);
 
+command_sender_t ps2_get_sender(void);
+rt_err_t ps2_set_target(void *target);
+
 #endif

protocol/ps2_table.h

@@ -1,28 +1,38 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-08-26     sogwms       The first version
+ */
+
 #include "ps2.h"
 
 #define PS2_TABLE_SIZE      20
 #define PS2_DEFAULT_TABLE                \
 {                                        \
-    {PS2_BTN_SELECT   ,COMMAND_CAR_STOP},    \
+    {PS2_BTN_SELECT   ,COMMAND_SET_CHASSIS_STOP},    \
     {PS2_BTN_L3       ,COMMAND_NONE},    \
     {PS2_BTN_R3       ,COMMAND_NONE},    \
     {PS2_BTN_START    ,COMMAND_NONE},    \
-    {PS2_BTN_UP       ,COMMAND_CAR_FORWARD},    \
-    {PS2_BTN_RIGHT    ,COMMAND_CAR_TURNRIGHT},    \
-    {PS2_BTN_DOWN     ,COMMAND_CAR_BACKWARD},    \
-    {PS2_BTN_LEFT     ,COMMAND_CAR_TURNLEFT},    \
+    {PS2_BTN_UP       ,COMMAND_SET_CHASSIS_FORWARD},    \
+    {PS2_BTN_RIGHT    ,COMMAND_SET_CHASSIS_ROTATE_RIGHT},    \
+    {PS2_BTN_DOWN     ,COMMAND_SET_CHASSIS_BACKWARD},    \
+    {PS2_BTN_LEFT     ,COMMAND_SET_CHASSIS_ROTATE_LEFT},    \
     {PS2_BTN_L2       ,COMMAND_NONE},    \
     {PS2_BTN_R2       ,COMMAND_NONE},    \
-    {PS2_BTN_L1       ,COMMAND_NONE},    \
-    {PS2_BTN_R1       ,COMMAND_NONE},    \
+    {PS2_BTN_L1       ,COMMAND_SET_CHASSIS_MOVE_LEFT},    \
+    {PS2_BTN_R1       ,COMMAND_SET_CHASSIS_MOVE_RIGHT},    \
     {PS2_BTN_TRIANGLE ,COMMAND_NONE},    \
     {PS2_BTN_CICLE    ,COMMAND_NONE},    \
     {PS2_BTN_FORK     ,COMMAND_NONE},    \
     {PS2_BTN_SQUARE   ,COMMAND_NONE},    \
-    {PS2_ROCKER_LX    ,COMMAND_NONE},    \
-    {PS2_ROCKER_LY    ,COMMAND_NONE},    \
-    {PS2_ROCKER_RX    ,COMMAND_NONE},    \
-    {PS2_ROCKER_RY    ,COMMAND_NONE}    \
+    {PS2_ROCKER_LX    ,COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z},    \
+    {PS2_ROCKER_LY    ,COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X},    \
+    {PS2_ROCKER_RX    ,COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y},    \
+    {PS2_ROCKER_RY    ,COMMAND_NONE}     \
 }
 
 struct ps2_table

robot/Sconscript

@@ -1,8 +0,0 @@
-from building import *
-
-cwd   = GetCurrentDir()
-src	  = Glob('*.c')
-PATH  = [cwd]
-group = DefineGroup('robots', src, depend = ['PKG_USING_ROBOTS'], CPPPATH = PATH)
-
-Return('group')

robot/mobile_robot.h

@@ -1,6 +0,0 @@
-#ifndef __MOBILE_ROBOT__H
-#define __MOBILE_ROBOT__H
-
-#include "robot.h"
-
-#endif // __MOBILE_ROBOT__H

robot/robot.c

@@ -1 +0,0 @@
-#include "robot.h"

robot/robot.h

@@ -1,26 +0,0 @@
-#ifndef __ROBOT_H__
-#define __ROBOT_H__
-
-#include <rtthread.h>
-#include <chassis.h>
-
-typedef struct robot *robot_t;
-
-enum robot_type 
-{
-    MOBILE_ROBOT,
-    ARM_ROBOT,
-    AERIAL_ROBOT
-};
-
-struct robot
-{
-    enum    robot_type    type;
-
-    int     (*init)(void);
-    int     (*enable)(void);
-    int     (*disable)(void);
-    int     (*execute)(rt_int8_t cmd, void *args);
-};
-
-#endif

wheel/wheel.c

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #include "wheel.h"
 
 #define DBG_SECTION_NAME  "wheel"
@@ -10,7 +20,7 @@ wheel_t wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_contro
     wheel_t new_wheel = (wheel_t) rt_malloc(sizeof(struct wheel));
     if(new_wheel == RT_NULL)
     {
-        LOG_E("Falied to allocate memory for dc wheel");
+        LOG_E("Falied to allocate memory for new wheel");
         return RT_NULL;
     }
 
@@ -24,20 +34,25 @@ wheel_t wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_contro
     return new_wheel;
 }
 
-void wheel_destroy(wheel_t wheel)
+rt_err_t wheel_destroy(wheel_t whl)
 {
+    RT_ASSERT(whl != RT_NULL);
+
     LOG_D("Free wheel");
 
-    motor_destroy(wheel->w_motor);
-    encoder_destroy(wheel->w_encoder);
-    controller_destroy(wheel->w_controller);
+    motor_destroy(whl->w_motor);
+    encoder_destroy(whl->w_encoder);
+    controller_destroy(whl->w_controller);
 
-    rt_free(wheel);
+    rt_free(whl);
+
+    return RT_EOK;
 }
 
 rt_err_t wheel_enable(wheel_t whl)
 {
-    // TODO
+    RT_ASSERT(whl != RT_NULL);
+
     LOG_D("Enabling wheel");
 
     // Enable PWM for motor
@@ -45,7 +60,6 @@ rt_err_t wheel_enable(wheel_t whl)
 
     // Enable Encoder's interrupt
     encoder_enable(whl->w_encoder);
-    rt_thread_mdelay(whl->w_encoder->sample_time);
 
     // Enable control
     controller_enable(whl->w_controller);
@@ -55,7 +69,8 @@ rt_err_t wheel_enable(wheel_t whl)
 
 rt_err_t wheel_disable(wheel_t whl)
 {
-    // TODO
+    RT_ASSERT(whl != RT_NULL);
+
     LOG_D("Disabling wheel");
 
     // Disable PWM for motor
@@ -72,7 +87,16 @@ rt_err_t wheel_disable(wheel_t whl)
 
 rt_err_t wheel_reset(wheel_t whl)
 {
-    // TODO
+    RT_ASSERT(whl != RT_NULL);
+
+    // Reset Controller
+    controller_reset(whl->w_controller);
+
+    // Reset Motor
+    motor_reset(whl->w_motor);
+    
+    // Reset Encoder
+    encoder_reset(whl->w_encoder);
 
     return RT_EOK;
 }
@@ -80,12 +104,15 @@ rt_err_t wheel_reset(wheel_t whl)
 /** speed = rpm x 60 x 2 x PI x radius **/
 rt_err_t wheel_set_speed(wheel_t whl, double speed)
 {
+    RT_ASSERT(whl != RT_NULL);
+
     return wheel_set_rpm(whl, (rt_int16_t) (speed / 60.0 / 2.0 / whl->radius / PI));
 }
 
 rt_err_t wheel_set_rpm(wheel_t whl, rt_int16_t rpm)
 {
-    LOG_D("Set wheel speed %d rpm", rpm);
+    RT_ASSERT(whl != RT_NULL);
+
     controller_set_target(whl->w_controller, rpm);
     if(whl->w_controller->target == rpm)
     {
@@ -99,7 +126,8 @@ rt_err_t wheel_set_rpm(wheel_t whl, rt_int16_t rpm)
 
 void wheel_update(wheel_t whl)
 {
-    // TODO
+    RT_ASSERT(whl != RT_NULL);
+
     // Get current rpm
     whl->rpm = encoder_measure_rpm(whl->w_encoder);
 
@@ -112,8 +140,7 @@ void wheel_update(wheel_t whl)
 
 void wheel_stop(wheel_t whl)
 {
+    RT_ASSERT(whl != RT_NULL);
+
     motor_stop(whl->w_motor);
 }
-
-// Maximum Speed
-// Wheel Thread

wheel/wheel.h

@@ -1,3 +1,13 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-07-17     Wu Han       The first version
+ */
+
 #ifndef __WHEEL_H__
 #define __WHEEL_H__
 
@@ -22,7 +32,7 @@ struct wheel
 };
 
 wheel_t     wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_controller, float radius, rt_uint16_t gear_ratio);
-void        wheel_destroy(wheel_t wheel);
+rt_err_t    wheel_destroy(wheel_t wheel);
 
 rt_err_t    wheel_enable(wheel_t whl);
 rt_err_t    wheel_disable(wheel_t whl);