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@3:51f72ee2d5c2, 2018-06-13 (annotated)

Committer:
PicYusuke
Date:
Wed Jun 13 14:20:24 2018 +0000
Revision:
3:51f72ee2d5c2
Parent:
2:92f3aa5245dc
Child:
4:ce4fef97e7e5
Simplified Send_Bulk_Char() function in dynamixel_servo_controller.cpp.

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 XM430(PinName tx_pin, PinName rx_pin);
PicYusuke 2:92f3aa5245dc 108
PicYusuke 2:92f3aa5245dc 109 private:
PicYusuke 2:92f3aa5245dc 110
PicYusuke 2:92f3aa5245dc 111 /**
PicYusuke 2:92f3aa5245dc 112 * @brief チェックサム計算( crc16 )
PicYusuke 2:92f3aa5245dc 113 *
PicYusuke 2:92f3aa5245dc 114 * @param crc_init_val crc 初期値
PicYusuke 2:92f3aa5245dc 115 * @param data 検査対象のデータ列
PicYusuke 2:92f3aa5245dc 116 * @param data_length
PicYusuke 2:92f3aa5245dc 117 * @return uint16_t チェックサム計算結果
PicYusuke 2:92f3aa5245dc 118 */
PicYusuke 2:92f3aa5245dc 119 uint16_t CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length);
PicYusuke 2:92f3aa5245dc 120
PicYusuke 2:92f3aa5245dc 121 /**
PicYusuke 2:92f3aa5245dc 122 * @brief パケット生成
PicYusuke 2:92f3aa5245dc 123 *
PicYusuke 2:92f3aa5245dc 124 * @param servo_id
PicYusuke 2:92f3aa5245dc 125 * @param length
PicYusuke 2:92f3aa5245dc 126 * @param instruction
PicYusuke 2:92f3aa5245dc 127 * @param ctrl_reg control レジスタ
PicYusuke 2:92f3aa5245dc 128 * @param data 送信データ
PicYusuke 2:92f3aa5245dc 129 */
PicYusuke 2:92f3aa5245dc 130 void Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
PicYusuke 2:92f3aa5245dc 131 const uint16_t ctrl_reg, uint8_t *data);
PicYusuke 2:92f3aa5245dc 132
PicYusuke 2:92f3aa5245dc 133 /**
PicYusuke 2:92f3aa5245dc 134 * @brief シリアルバルク送信
PicYusuke 2:92f3aa5245dc 135 *
PicYusuke 2:92f3aa5245dc 136 * @param buf 送信バッファ
PicYusuke 2:92f3aa5245dc 137 * @param buf_length バッファ長
PicYusuke 2:92f3aa5245dc 138 * @return true 正常に通信が終了
PicYusuke 2:92f3aa5245dc 139 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 140 */
PicYusuke 2:92f3aa5245dc 141 bool Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length);
PicYusuke 2:92f3aa5245dc 142
PicYusuke 2:92f3aa5245dc 143 /**
PicYusuke 2:92f3aa5245dc 144 * @brief シリアルバルク受信
PicYusuke 2:92f3aa5245dc 145 *
PicYusuke 2:92f3aa5245dc 146 * @param buf 受信バッファ
PicYusuke 2:92f3aa5245dc 147 * @param buf_length バッファ長
PicYusuke 2:92f3aa5245dc 148 * @param timeout 通信途絶を判断するまでの時間
PicYusuke 2:92f3aa5245dc 149 * @return true 正常に受信
PicYusuke 2:92f3aa5245dc 150 * @return false 受信失敗
PicYusuke 2:92f3aa5245dc 151 */
PicYusuke 2:92f3aa5245dc 152 bool Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout);
PicYusuke 2:92f3aa5245dc 153
PicYusuke 2:92f3aa5245dc 154 public:
PicYusuke 2:92f3aa5245dc 155 /**
PicYusuke 2:92f3aa5245dc 156 * @brief サーボのトルクをONにする
PicYusuke 2:92f3aa5245dc 157 *
PicYusuke 2:92f3aa5245dc 158 * @param id サーボID
PicYusuke 2:92f3aa5245dc 159 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 160 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 161 */
PicYusuke 2:92f3aa5245dc 162 bool Torque_ON(uint8_t id);
PicYusuke 2:92f3aa5245dc 163
PicYusuke 2:92f3aa5245dc 164 /**
PicYusuke 2:92f3aa5245dc 165 * @brief サーボのトルクをOFFにする
PicYusuke 2:92f3aa5245dc 166 *
PicYusuke 2:92f3aa5245dc 167 * @param id サーボID
PicYusuke 2:92f3aa5245dc 168 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 169 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 170 */
PicYusuke 2:92f3aa5245dc 171 bool Torque_OFF(uint8_t id);
PicYusuke 2:92f3aa5245dc 172
PicYusuke 2:92f3aa5245dc 173 /**
PicYusuke 2:92f3aa5245dc 174 * @brief サーボホーン位置を設定
PicYusuke 2:92f3aa5245dc 175 *
PicYusuke 2:92f3aa5245dc 176 * @param id サーボID
PicYusuke 2:92f3aa5245dc 177 * @param pos 目標回転位置
PicYusuke 2:92f3aa5245dc 178 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 179 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 180 */
PicYusuke 2:92f3aa5245dc 181 bool Set_Pos(uint8_t id, uint32_t pos);
PicYusuke 2:92f3aa5245dc 182 };
PicYusuke 2:92f3aa5245dc 183 };
PicYusuke 2:92f3aa5245dc 184
PicYusuke 2:92f3aa5245dc 185 #endif