Diff: dynamixel_servo_controller.cpp

Revision:

2:92f3aa5245dc

Child:

3:51f72ee2d5c2

diff -r 2c82c636c43e -r 92f3aa5245dc dynamixel_servo_controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel_servo_controller.cpp	Wed Jun 13 08:31:51 2018 +0000
@@ -0,0 +1,324 @@
+/*
+ * 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.
+ */
+
+#include "dynamixel_servo_controller.h"
+
+#ifndef MBED_ENVIRONMENT
+    #include <iostream>
+    #include <cstdint>
+#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)
+            {
+                /******************** CRC16 用テーブル生成 *********************/
+
+                /**************************************************************/
+            }
+
+    };
+    #endif
+};
+
+    
+        
+#ifdef MBED_ENVIRONMENT
+
+/**
+ * @brief UART 受信割り込み関数
+ * 
+ */
+void XM430::UART_Rx_Callback()
+{
+
+}
+#endif
+
+
+/**
+ * @brief Construct a new XM430 object
+ * 
+ */
+#ifdef MBED_ENVIRONMENT
+XM430::XM430(PinName tx_pin, PinName rx_pin)
+:   uart(tx_pin, rx_pin)
+{
+    uart.baud(57600);
+    uart.format(8, Serial::None, 1);
+    // UART受信割り込み
+    uart.attach(callback(this, &XM430::UART_Rx_Callback), Serial::RxIrq);
+    
+#else
+XM430::XM430(PinName tx_pin, PinName rx_pin)
+{
+#endif
+    
+    /******************** CRC16 用テーブル生成 *********************/
+    uint16_t temp = 0;
+
+    for(uint32_t i = 0; i < 256; i ++)
+    {
+        temp = (uint16_t)(i << 8);
+
+        for(uint32_t j = 0; j < 8; j ++)
+        {
+            // tempの最上位ビットが0: ビットシフト
+            // tempの最上位ビットが1: ビットシフト+XOR
+            if((temp & 0x8000) == 0)
+            {
+                temp = (temp << 1);
+            }
+            else
+            {
+                temp = (temp << 1) ^ CRC16_POLY;
+            }
+        }
+        crc_tbl[i] = temp;
+    }
+    /**************************************************************/
+
+    #if 0
+    for(uint32_t i = 0; i < 256; i ++)
+    {
+        std::cout << std::hex << crc_tbl[i] << std::endl;
+    }
+    #endif
+
+    // Header
+    tx_buf[0] = 0xFF;
+    tx_buf[1] = 0xFF;
+    tx_buf[2] = 0xFD;
+
+    // Reserved
+    tx_buf[3] = 0x00;
+}
+
+/**
+ * @brief チェックサム計算( crc16 )
+ * 
+ * @param crc_init_val crc 初期値
+ * @param data 検査対象のデータ列
+ * @param data_length 
+ * @return uint16_t チェックサム計算結果
+ */
+uint16_t XM430::CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length)
+{
+    uint32_t index;
+    uint16_t temp = crc_init_val;
+
+    for(uint32_t i = 0; i < data_length; i++)
+    {
+        index = ((uint16_t)(temp >> 8) ^ data[i]) & 0xFF;
+        temp = (temp << 8) ^ crc_tbl[index];
+    }
+
+    return temp;
+}
+
+/**
+ * @brief パケット生成
+ * 
+ * @param servo_id 
+ * @param length 
+ * @param instruction 
+ * @param ctrl_reg control レジスタ
+ * @param data 送信データ
+ */
+void XM430::Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
+                    const uint16_t ctrl_reg, uint8_t *data)
+{
+    uint16_t checksum;
+
+    // ID
+    tx_buf[4] = servo_id;
+
+    // Length
+    tx_buf[5] = (uint8_t)(length & 0xFF);
+    tx_buf[6] = (uint8_t)(length >> 8);
+
+    // Instruction
+    tx_buf[7] = instruction;
+
+    // control register
+    tx_buf[8] = (uint8_t)(ctrl_reg & 0xFF);
+    tx_buf[9] = (uint8_t)(ctrl_reg >> 8);
+
+    // Data
+    for(uint32_t i = 0; i < ( length-5 ); i ++)
+    {
+        tx_buf[ i+10 ] = data[i];
+    }
+    
+    // Checksum
+    checksum = CRC16(0, tx_buf, ( length+5 ));
+    tx_buf[ length+5 ] = (uint8_t)(checksum & 0xFF);
+    tx_buf[ length+6 ] = (uint8_t)(checksum >> 8);
+}
+
+/**
+ * @brief シリアルバルク送信
+ * 
+ * @param buf 送信バッファ
+ * @param buf_length バッファ長
+ * @return true 正常に通信が終了
+ * @return false 通信が不正に終了
+ */
+bool XM430::Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length)
+{
+    #ifdef MBED_ENVIRONMENT
+        // ユーザ関数
+
+        return true;
+    #else
+        for(uint32_t i = 0; i < buf_length; i ++)
+        {
+            //std::cout << tx_buf[i] << std::endl;
+            std::cout << std::hex << std::showbase << std::uppercase << (uint32_t)tx_buf[i] << std::endl;
+        }
+
+        return true;
+    #endif
+
+    return false;
+}
+
+/**
+ * @brief シリアルバルク受信
+ * 
+ * @param buf 受信バッファ
+ * @param buf_length バッファ長
+ * @param timeout 通信途絶を判断するまでの時間
+ * @return true 正常に受信
+ * @return false 受信失敗
+ */
+bool XM430::Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout)
+{
+    #ifdef MBED_ENVIRONMENT
+    uint32_t count = 0;
+
+    count = 0;
+    while(0 == uart.readable())
+    {
+        if(count > timeout)
+        {
+            return false;
+        }
+    }
+    #endif
+
+    return false;
+}
+
+/**
+ * @brief サーボのトルクをONにする
+ * 
+ * @param id サーボID
+ * @return true 通信が正常に終了
+ * @return false 通信が不正に終了
+ */
+bool XM430::Torque_ON(uint8_t id)
+{
+    uint16_t length = 6;
+    uint8_t data[1];       // トルクON/OFF
+
+    bool ret_val;
+
+    data[0] = 1;            // トルクON
+
+    Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
+
+    ret_val = Send_Bulk_Char(tx_buf, 13);
+    return ret_val;
+}
+
+/**
+ * @brief サーボのトルクをOFFにする
+ * 
+ * @param id サーボID
+ * @return true 通信が正常に終了
+ * @return false 通信が不正に終了
+ */
+bool XM430::Torque_OFF(uint8_t id)
+{
+    uint16_t length = 6;
+    uint8_t data[1];       // トルクON/OFF
+
+    bool ret_val;
+
+    data[0] = 0;            // トルクOFF
+
+    Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
+
+    ret_val = Send_Bulk_Char(tx_buf, 13);
+    return ret_val;
+}
+
+/**
+ * @brief サーボホーン位置を設定
+ * 
+ * @param id サーボID
+ * @param pos 目標回転位置
+ * @return true 通信が正常に終了
+ * @return false 通信が不正に終了
+ */
+bool XM430::Set_Pos(uint8_t id, uint32_t pos)
+{
+    uint16_t length = 9;
+    uint8_t data[4];    // 位置データ
+
+    bool ret_val;
+
+    // Data
+    data[0] = (uint8_t)(pos & 0xFF);
+    data[1] = (uint8_t)((pos >> 8) & 0xFF);
+    data[2] = (uint8_t)((pos >> 16) & 0xFF);
+    data[3] = (uint8_t)((pos >> 24) & 0xFF);
+
+    Create_Packet(id, length, WRITE, GOAL_POSITION, data);
+    
+    ret_val = Send_Bulk_Char(tx_buf, 16);
+    return ret_val;
+}
+
+};
+