Yusuke Okino / Mbed 2 deprecated dynamixel_servo_controller

Dynamixel servo controller. This program consists of 2 parts: "dynamixel_servo_controller.cpp/.h" and "main.cpp"( demo program ).

Dependencies:   mbed

Fork of dynamixel_servo_controller by Yusuke Okino

dynamixel_servo_controller.h@2:92f3aa5245dc, 2018-06-13 (annotated)

Committer:
PicYusuke
Date:
Wed Jun 13 08:31:51 2018 +0000
Revision:
2:92f3aa5245dc
Child:
3:51f72ee2d5c2
Separated into 3 files.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PicYusuke 2:92f3aa5245dc 1 /*
PicYusuke 2:92f3aa5245dc 2 * Copyright (c) 2018 Yusuke Okino
PicYusuke 2:92f3aa5245dc 3
PicYusuke 2:92f3aa5245dc 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
PicYusuke 2:92f3aa5245dc 5 * of this software and associated documentation files (the "Software"), to deal
PicYusuke 2:92f3aa5245dc 6 * in the Software without restriction, including without limitation the rights
PicYusuke 2:92f3aa5245dc 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
PicYusuke 2:92f3aa5245dc 8 * copies of the Software, and to permit persons to whom the Software is
PicYusuke 2:92f3aa5245dc 9 * furnished to do so, subject to the following conditions:
PicYusuke 2:92f3aa5245dc 10
PicYusuke 2:92f3aa5245dc 11 * The above copyright notice and this permission notice shall be included in all
PicYusuke 2:92f3aa5245dc 12 * copies or substantial portions of the Software.
PicYusuke 2:92f3aa5245dc 13
PicYusuke 2:92f3aa5245dc 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
PicYusuke 2:92f3aa5245dc 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
PicYusuke 2:92f3aa5245dc 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
PicYusuke 2:92f3aa5245dc 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
PicYusuke 2:92f3aa5245dc 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
PicYusuke 2:92f3aa5245dc 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
PicYusuke 2:92f3aa5245dc 20 * SOFTWARE.
PicYusuke 2:92f3aa5245dc 21 */
PicYusuke 2:92f3aa5245dc 22
PicYusuke 2:92f3aa5245dc 23 #ifndef DYNAMIXEL_SERVO_CONTROLLER_H
PicYusuke 2:92f3aa5245dc 24 #define DYNAMIXEL_SERVO_CONTROLLER_H
PicYusuke 2:92f3aa5245dc 25
PicYusuke 2:92f3aa5245dc 26 #define MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 27
PicYusuke 2:92f3aa5245dc 28 #ifdef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 29 #include "mbed.h"
PicYusuke 2:92f3aa5245dc 30 #else
PicYusuke 2:92f3aa5245dc 31
PicYusuke 2:92f3aa5245dc 32 #include <cstdint>
PicYusuke 2:92f3aa5245dc 33 typedef uint16_t PinName;
PicYusuke 2:92f3aa5245dc 34 const PinName PA_9 = 9;
PicYusuke 2:92f3aa5245dc 35 const PinName PA_10 = 10;
PicYusuke 2:92f3aa5245dc 36
PicYusuke 2:92f3aa5245dc 37 #endif
PicYusuke 2:92f3aa5245dc 38
PicYusuke 2:92f3aa5245dc 39 /**
PicYusuke 2:92f3aa5245dc 40 * @brief Dynamixel サーボコントローラ protocol v2
PicYusuke 2:92f3aa5245dc 41 *
PicYusuke 2:92f3aa5245dc 42 */
PicYusuke 2:92f3aa5245dc 43 namespace dynamixel_servo_v2
PicYusuke 2:92f3aa5245dc 44 {
PicYusuke 2:92f3aa5245dc 45
PicYusuke 2:92f3aa5245dc 46 namespace
PicYusuke 2:92f3aa5245dc 47 {
PicYusuke 2:92f3aa5245dc 48 // チェックサム計算クラス( 今後実装 )
PicYusuke 2:92f3aa5245dc 49 #if 0
PicYusuke 2:92f3aa5245dc 50 class CRC16
PicYusuke 2:92f3aa5245dc 51 {
PicYusuke 2:92f3aa5245dc 52 private:
PicYusuke 2:92f3aa5245dc 53
PicYusuke 2:92f3aa5245dc 54 public:
PicYusuke 2:92f3aa5245dc 55 /**
PicYusuke 2:92f3aa5245dc 56 * @brief Construct a new CRC16 object
PicYusuke 2:92f3aa5245dc 57 *
PicYusuke 2:92f3aa5245dc 58 * @param polynomial
PicYusuke 2:92f3aa5245dc 59 */
PicYusuke 2:92f3aa5245dc 60 CRC16(const uint16_t polynomial);
PicYusuke 2:92f3aa5245dc 61
PicYusuke 2:92f3aa5245dc 62 };
PicYusuke 2:92f3aa5245dc 63 #endif
PicYusuke 2:92f3aa5245dc 64 };
PicYusuke 2:92f3aa5245dc 65
PicYusuke 2:92f3aa5245dc 66 /**
PicYusuke 2:92f3aa5245dc 67 * @brief Dynamixel XM430サーボ制御用クラス
PicYusuke 2:92f3aa5245dc 68 *
PicYusuke 2:92f3aa5245dc 69 */
PicYusuke 2:92f3aa5245dc 70 class XM430
PicYusuke 2:92f3aa5245dc 71 {
PicYusuke 2:92f3aa5245dc 72 private:
PicYusuke 2:92f3aa5245dc 73
PicYusuke 2:92f3aa5245dc 74 // Checksum polynomial
PicYusuke 2:92f3aa5245dc 75 const static uint16_t CRC16_POLY = 0x8005;
PicYusuke 2:92f3aa5245dc 76
PicYusuke 2:92f3aa5245dc 77 /********************* レジスタマップ ***********************/
PicYusuke 2:92f3aa5245dc 78 /***************** instruction *******************/
PicYusuke 2:92f3aa5245dc 79 const static uint8_t WRITE = 0x03;
PicYusuke 2:92f3aa5245dc 80 /*************************************************/
PicYusuke 2:92f3aa5245dc 81
PicYusuke 2:92f3aa5245dc 82 /**************** Control table ******************/
PicYusuke 2:92f3aa5245dc 83 const static uint16_t TORQUE_ENABLE = 64;
PicYusuke 2:92f3aa5245dc 84 const static uint16_t GOAL_POSITION = 116;
PicYusuke 2:92f3aa5245dc 85 /*************************************************/
PicYusuke 2:92f3aa5245dc 86 /***********************************************************/
PicYusuke 2:92f3aa5245dc 87
PicYusuke 2:92f3aa5245dc 88 uint16_t crc_tbl[256]; // CRC16 計算用テーブル
PicYusuke 2:92f3aa5245dc 89 uint8_t tx_buf[128]; // シリアル送信バッファ
PicYusuke 2:92f3aa5245dc 90 uint8_t rx_buf[128]; // シリアル受信バッファ
PicYusuke 2:92f3aa5245dc 91
PicYusuke 2:92f3aa5245dc 92 #ifdef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 93 Serial uart; // シリアル通信用クラス
PicYusuke 2:92f3aa5245dc 94
PicYusuke 2:92f3aa5245dc 95 /**
PicYusuke 2:92f3aa5245dc 96 * @brief UART 受信割り込み関数
PicYusuke 2:92f3aa5245dc 97 *
PicYusuke 2:92f3aa5245dc 98 */
PicYusuke 2:92f3aa5245dc 99 void UART_Rx_Callback();
PicYusuke 2:92f3aa5245dc 100 #endif
PicYusuke 2:92f3aa5245dc 101
PicYusuke 2:92f3aa5245dc 102 public:
PicYusuke 2:92f3aa5245dc 103 /**
PicYusuke 2:92f3aa5245dc 104 * @brief Construct a new XM430 object
PicYusuke 2:92f3aa5245dc 105 *
PicYusuke 2:92f3aa5245dc 106 */
PicYusuke 2:92f3aa5245dc 107 #ifdef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 108 XM430(PinName tx_pin, PinName rx_pin);
PicYusuke 2:92f3aa5245dc 109 #else
PicYusuke 2:92f3aa5245dc 110 XM430(PinName tx_pin, PinName rx_pin);
PicYusuke 2:92f3aa5245dc 111 #endif
PicYusuke 2:92f3aa5245dc 112
PicYusuke 2:92f3aa5245dc 113 private:
PicYusuke 2:92f3aa5245dc 114
PicYusuke 2:92f3aa5245dc 115 /**
PicYusuke 2:92f3aa5245dc 116 * @brief チェックサム計算( crc16 )
PicYusuke 2:92f3aa5245dc 117 *
PicYusuke 2:92f3aa5245dc 118 * @param crc_init_val crc 初期値
PicYusuke 2:92f3aa5245dc 119 * @param data 検査対象のデータ列
PicYusuke 2:92f3aa5245dc 120 * @param data_length
PicYusuke 2:92f3aa5245dc 121 * @return uint16_t チェックサム計算結果
PicYusuke 2:92f3aa5245dc 122 */
PicYusuke 2:92f3aa5245dc 123 uint16_t CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length);
PicYusuke 2:92f3aa5245dc 124
PicYusuke 2:92f3aa5245dc 125 /**
PicYusuke 2:92f3aa5245dc 126 * @brief パケット生成
PicYusuke 2:92f3aa5245dc 127 *
PicYusuke 2:92f3aa5245dc 128 * @param servo_id
PicYusuke 2:92f3aa5245dc 129 * @param length
PicYusuke 2:92f3aa5245dc 130 * @param instruction
PicYusuke 2:92f3aa5245dc 131 * @param ctrl_reg control レジスタ
PicYusuke 2:92f3aa5245dc 132 * @param data 送信データ
PicYusuke 2:92f3aa5245dc 133 */
PicYusuke 2:92f3aa5245dc 134 void Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
PicYusuke 2:92f3aa5245dc 135 const uint16_t ctrl_reg, uint8_t *data);
PicYusuke 2:92f3aa5245dc 136
PicYusuke 2:92f3aa5245dc 137 /**
PicYusuke 2:92f3aa5245dc 138 * @brief シリアルバルク送信
PicYusuke 2:92f3aa5245dc 139 *
PicYusuke 2:92f3aa5245dc 140 * @param buf 送信バッファ
PicYusuke 2:92f3aa5245dc 141 * @param buf_length バッファ長
PicYusuke 2:92f3aa5245dc 142 * @return true 正常に通信が終了
PicYusuke 2:92f3aa5245dc 143 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 144 */
PicYusuke 2:92f3aa5245dc 145 bool Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length);
PicYusuke 2:92f3aa5245dc 146
PicYusuke 2:92f3aa5245dc 147 /**
PicYusuke 2:92f3aa5245dc 148 * @brief シリアルバルク受信
PicYusuke 2:92f3aa5245dc 149 *
PicYusuke 2:92f3aa5245dc 150 * @param buf 受信バッファ
PicYusuke 2:92f3aa5245dc 151 * @param buf_length バッファ長
PicYusuke 2:92f3aa5245dc 152 * @param timeout 通信途絶を判断するまでの時間
PicYusuke 2:92f3aa5245dc 153 * @return true 正常に受信
PicYusuke 2:92f3aa5245dc 154 * @return false 受信失敗
PicYusuke 2:92f3aa5245dc 155 */
PicYusuke 2:92f3aa5245dc 156 bool Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout);
PicYusuke 2:92f3aa5245dc 157
PicYusuke 2:92f3aa5245dc 158 public:
PicYusuke 2:92f3aa5245dc 159 /**
PicYusuke 2:92f3aa5245dc 160 * @brief サーボのトルクをONにする
PicYusuke 2:92f3aa5245dc 161 *
PicYusuke 2:92f3aa5245dc 162 * @param id サーボID
PicYusuke 2:92f3aa5245dc 163 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 164 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 165 */
PicYusuke 2:92f3aa5245dc 166 bool Torque_ON(uint8_t id);
PicYusuke 2:92f3aa5245dc 167
PicYusuke 2:92f3aa5245dc 168 /**
PicYusuke 2:92f3aa5245dc 169 * @brief サーボのトルクをOFFにする
PicYusuke 2:92f3aa5245dc 170 *
PicYusuke 2:92f3aa5245dc 171 * @param id サーボID
PicYusuke 2:92f3aa5245dc 172 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 173 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 174 */
PicYusuke 2:92f3aa5245dc 175 bool Torque_OFF(uint8_t id);
PicYusuke 2:92f3aa5245dc 176
PicYusuke 2:92f3aa5245dc 177 /**
PicYusuke 2:92f3aa5245dc 178 * @brief サーボホーン位置を設定
PicYusuke 2:92f3aa5245dc 179 *
PicYusuke 2:92f3aa5245dc 180 * @param id サーボID
PicYusuke 2:92f3aa5245dc 181 * @param pos 目標回転位置
PicYusuke 2:92f3aa5245dc 182 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 183 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 184 */
PicYusuke 2:92f3aa5245dc 185 bool Set_Pos(uint8_t id, uint32_t pos);
PicYusuke 2:92f3aa5245dc 186 };
PicYusuke 2:92f3aa5245dc 187 };
PicYusuke 2:92f3aa5245dc 188
PicYusuke 2:92f3aa5245dc 189 #endif