Diff: dynamixel_servo_controller.h

Revision:

2:92f3aa5245dc

Child:

3:51f72ee2d5c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel_servo_controller.h	Wed Jun 13 08:31:51 2018 +0000
@@ -0,0 +1,189 @@
+/*
+ * Copyright (c) 2018 Yusuke Okino
+
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef DYNAMIXEL_SERVO_CONTROLLER_H
+#define DYNAMIXEL_SERVO_CONTROLLER_H
+
+#define MBED_ENVIRONMENT
+
+#ifdef MBED_ENVIRONMENT
+    #include "mbed.h"
+#else
+
+#include <cstdint>
+    typedef uint16_t PinName;
+    const PinName PA_9 = 9;
+    const PinName PA_10 = 10;
+
+#endif
+
+/**
+ * @brief Dynamixel サーボコントローラ protocol v2
+ * 
+ */
+namespace dynamixel_servo_v2
+{
+
+namespace 
+{
+    // チェックサム計算クラス( 今後実装 )
+    #if 0
+    class CRC16 
+    {
+        private:
+
+        public:
+            /**
+             * @brief Construct a new CRC16 object
+             * 
+             * @param polynomial 
+             */
+            CRC16(const uint16_t polynomial);
+
+    };
+    #endif
+};
+
+/**
+ * @brief Dynamixel XM430サーボ制御用クラス
+ * 
+ */
+class XM430
+{
+    private:
+
+        // Checksum polynomial
+        const static uint16_t CRC16_POLY = 0x8005;
+
+        /********************* レジスタマップ ***********************/
+        /***************** instruction *******************/
+        const static uint8_t WRITE = 0x03;
+        /*************************************************/
+
+        /**************** Control table ******************/
+        const static uint16_t TORQUE_ENABLE = 64;   
+        const static uint16_t GOAL_POSITION = 116;
+        /*************************************************/
+        /***********************************************************/
+
+        uint16_t crc_tbl[256];  // CRC16 計算用テーブル
+        uint8_t tx_buf[128];    // シリアル送信バッファ
+        uint8_t rx_buf[128];    // シリアル受信バッファ
+        
+        #ifdef MBED_ENVIRONMENT
+        Serial uart;            // シリアル通信用クラス
+
+        /**
+         * @brief UART 受信割り込み関数
+         * 
+         */
+        void UART_Rx_Callback();
+        #endif
+
+    public:
+        /**
+         * @brief Construct a new XM430 object
+         * 
+         */
+        #ifdef MBED_ENVIRONMENT
+        XM430(PinName tx_pin, PinName rx_pin);
+        #else
+        XM430(PinName tx_pin, PinName rx_pin);
+        #endif
+
+    private:
+
+        /**
+         * @brief チェックサム計算( crc16 )
+         * 
+         * @param crc_init_val crc 初期値
+         * @param data 検査対象のデータ列
+         * @param data_length 
+         * @return uint16_t チェックサム計算結果
+         */
+        uint16_t CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length);
+
+        /**
+         * @brief パケット生成
+         * 
+         * @param servo_id 
+         * @param length 
+         * @param instruction 
+         * @param ctrl_reg control レジスタ
+         * @param data 送信データ
+         */
+        void Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
+                            const uint16_t ctrl_reg, uint8_t *data);
+
+        /**
+         * @brief シリアルバルク送信
+         * 
+         * @param buf 送信バッファ
+         * @param buf_length バッファ長
+         * @return true 正常に通信が終了
+         * @return false 通信が不正に終了
+         */
+        bool Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length);
+
+        /**
+         * @brief シリアルバルク受信
+         * 
+         * @param buf 受信バッファ
+         * @param buf_length バッファ長
+         * @param timeout 通信途絶を判断するまでの時間
+         * @return true 正常に受信
+         * @return false 受信失敗
+         */
+        bool Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout);
+
+    public:
+        /**
+         * @brief サーボのトルクをONにする
+         * 
+         * @param id サーボID
+         * @return true 通信が正常に終了
+         * @return false 通信が不正に終了
+         */
+        bool Torque_ON(uint8_t id);
+
+        /**
+         * @brief サーボのトルクをOFFにする
+         * 
+         * @param id サーボID
+         * @return true 通信が正常に終了
+         * @return false 通信が不正に終了
+         */
+        bool Torque_OFF(uint8_t id);
+
+        /**
+         * @brief サーボホーン位置を設定
+         * 
+         * @param id サーボID
+         * @param pos 目標回転位置
+         * @return true 通信が正常に終了
+         * @return false 通信が不正に終了
+         */
+        bool Set_Pos(uint8_t id, uint32_t pos);
+};
+};
+
+#endif