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.cpp@5:4474c07c6274, 2018-06-15 (annotated)

Committer:
PicYusuke
Date:
Fri Jun 15 10:41:34 2018 +0000
Revision:
5:4474c07c6274
Parent:
4:ce4fef97e7e5 
Considered compile speed.

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 #include "dynamixel_servo_controller.h"
PicYusuke 2:92f3aa5245dc 24
PicYusuke 2:92f3aa5245dc 25 #ifndef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 26 #include <iostream>
PicYusuke 2:92f3aa5245dc 27 #include <cstdint>
PicYusuke 2:92f3aa5245dc 28 #endif
PicYusuke 2:92f3aa5245dc 29
PicYusuke 2:92f3aa5245dc 30
PicYusuke 2:92f3aa5245dc 31 /**
PicYusuke 2:92f3aa5245dc 32 * @brief Dynamixel サーボコントローラ protocol v2
PicYusuke 2:92f3aa5245dc 33 *
PicYusuke 2:92f3aa5245dc 34 */
PicYusuke 2:92f3aa5245dc 35 namespace dynamixel_servo_v2
PicYusuke 2:92f3aa5245dc 36 {
PicYusuke 2:92f3aa5245dc 37
PicYusuke 5:4474c07c6274 38 // Checksum polynomia
PicYusuke 5:4474c07c6274 39 const uint16_t XM430::CRC16_POLY = 0x8005;
PicYusuke 5:4474c07c6274 40
PicYusuke 5:4474c07c6274 41 /********************* レジスタマップ ***********************/
PicYusuke 5:4474c07c6274 42 /***************** instruction *******************/
PicYusuke 5:4474c07c6274 43 const uint8_t XM430::WRITE = 0x03;
PicYusuke 5:4474c07c6274 44 /*************************************************/
PicYusuke 5:4474c07c6274 45
PicYusuke 5:4474c07c6274 46 /**************** Control table ******************/
PicYusuke 5:4474c07c6274 47 const uint16_t XM430::TORQUE_ENABLE = 64;
PicYusuke 5:4474c07c6274 48 const uint16_t XM430::GOAL_POSITION = 116;
PicYusuke 5:4474c07c6274 49 /*************************************************/
PicYusuke 5:4474c07c6274 50 /***********************************************************/
PicYusuke 5:4474c07c6274 51
PicYusuke 2:92f3aa5245dc 52 namespace
PicYusuke 2:92f3aa5245dc 53 {
PicYusuke 2:92f3aa5245dc 54 // チェックサム計算クラス( 今後実装 )
PicYusuke 2:92f3aa5245dc 55 #if 0
PicYusuke 2:92f3aa5245dc 56 class CRC16
PicYusuke 2:92f3aa5245dc 57 {
PicYusuke 2:92f3aa5245dc 58 private:
PicYusuke 2:92f3aa5245dc 59
PicYusuke 2:92f3aa5245dc 60 public:
PicYusuke 2:92f3aa5245dc 61 /**
PicYusuke 2:92f3aa5245dc 62 * @brief Construct a new CRC16 object
PicYusuke 2:92f3aa5245dc 63 *
PicYusuke 2:92f3aa5245dc 64 * @param polynomial
PicYusuke 2:92f3aa5245dc 65 */
PicYusuke 2:92f3aa5245dc 66 CRC16(const uint16_t polynomial)
PicYusuke 2:92f3aa5245dc 67 {
PicYusuke 2:92f3aa5245dc 68 /******************** CRC16 用テーブル生成 *********************/
PicYusuke 2:92f3aa5245dc 69
PicYusuke 2:92f3aa5245dc 70 /**************************************************************/
PicYusuke 2:92f3aa5245dc 71 }
PicYusuke 2:92f3aa5245dc 72
PicYusuke 2:92f3aa5245dc 73 };
PicYusuke 2:92f3aa5245dc 74 #endif
PicYusuke 2:92f3aa5245dc 75 };
PicYusuke 2:92f3aa5245dc 76
PicYusuke 2:92f3aa5245dc 77
PicYusuke 2:92f3aa5245dc 78
PicYusuke 2:92f3aa5245dc 79 #ifdef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 80
PicYusuke 2:92f3aa5245dc 81 /**
PicYusuke 2:92f3aa5245dc 82 * @brief UART 受信割り込み関数
PicYusuke 2:92f3aa5245dc 83 *
PicYusuke 2:92f3aa5245dc 84 */
PicYusuke 2:92f3aa5245dc 85 void XM430::UART_Rx_Callback()
PicYusuke 2:92f3aa5245dc 86 {
PicYusuke 2:92f3aa5245dc 87
PicYusuke 2:92f3aa5245dc 88 }
PicYusuke 2:92f3aa5245dc 89 #endif
PicYusuke 2:92f3aa5245dc 90
PicYusuke 2:92f3aa5245dc 91
PicYusuke 2:92f3aa5245dc 92 /**
PicYusuke 2:92f3aa5245dc 93 * @brief Construct a new XM430 object
PicYusuke 2:92f3aa5245dc 94 *
PicYusuke 2:92f3aa5245dc 95 */
PicYusuke 2:92f3aa5245dc 96 #ifdef MBED_ENVIRONMENT
PicYusuke 2:92f3aa5245dc 97 XM430::XM430(PinName tx_pin, PinName rx_pin)
PicYusuke 2:92f3aa5245dc 98 : uart(tx_pin, rx_pin)
PicYusuke 2:92f3aa5245dc 99 {
PicYusuke 2:92f3aa5245dc 100 uart.baud(57600);
PicYusuke 2:92f3aa5245dc 101 uart.format(8, Serial::None, 1);
PicYusuke 2:92f3aa5245dc 102 // UART受信割り込み
PicYusuke 2:92f3aa5245dc 103 uart.attach(callback(this, &XM430::UART_Rx_Callback), Serial::RxIrq);
PicYusuke 2:92f3aa5245dc 104
PicYusuke 2:92f3aa5245dc 105 #else
PicYusuke 2:92f3aa5245dc 106 XM430::XM430(PinName tx_pin, PinName rx_pin)
PicYusuke 2:92f3aa5245dc 107 {
PicYusuke 2:92f3aa5245dc 108 #endif
PicYusuke 2:92f3aa5245dc 109
PicYusuke 2:92f3aa5245dc 110 /******************** CRC16 用テーブル生成 *********************/
PicYusuke 2:92f3aa5245dc 111 uint16_t temp = 0;
PicYusuke 2:92f3aa5245dc 112
PicYusuke 2:92f3aa5245dc 113 for(uint32_t i = 0; i < 256; i ++)
PicYusuke 2:92f3aa5245dc 114 {
PicYusuke 2:92f3aa5245dc 115 temp = (uint16_t)(i << 8);
PicYusuke 2:92f3aa5245dc 116
PicYusuke 2:92f3aa5245dc 117 for(uint32_t j = 0; j < 8; j ++)
PicYusuke 2:92f3aa5245dc 118 {
PicYusuke 2:92f3aa5245dc 119 // tempの最上位ビットが0: ビットシフト
PicYusuke 2:92f3aa5245dc 120 // tempの最上位ビットが1: ビットシフト+XOR
PicYusuke 2:92f3aa5245dc 121 if((temp & 0x8000) == 0)
PicYusuke 2:92f3aa5245dc 122 {
PicYusuke 2:92f3aa5245dc 123 temp = (temp << 1);
PicYusuke 2:92f3aa5245dc 124 }
PicYusuke 2:92f3aa5245dc 125 else
PicYusuke 2:92f3aa5245dc 126 {
PicYusuke 2:92f3aa5245dc 127 temp = (temp << 1) ^ CRC16_POLY;
PicYusuke 2:92f3aa5245dc 128 }
PicYusuke 2:92f3aa5245dc 129 }
PicYusuke 2:92f3aa5245dc 130 crc_tbl[i] = temp;
PicYusuke 2:92f3aa5245dc 131 }
PicYusuke 2:92f3aa5245dc 132 /**************************************************************/
PicYusuke 2:92f3aa5245dc 133
PicYusuke 2:92f3aa5245dc 134 #if 0
PicYusuke 2:92f3aa5245dc 135 for(uint32_t i = 0; i < 256; i ++)
PicYusuke 2:92f3aa5245dc 136 {
PicYusuke 2:92f3aa5245dc 137 std::cout << std::hex << crc_tbl[i] << std::endl;
PicYusuke 2:92f3aa5245dc 138 }
PicYusuke 2:92f3aa5245dc 139 #endif
PicYusuke 2:92f3aa5245dc 140
PicYusuke 2:92f3aa5245dc 141 // Header
PicYusuke 2:92f3aa5245dc 142 tx_buf[0] = 0xFF;
PicYusuke 2:92f3aa5245dc 143 tx_buf[1] = 0xFF;
PicYusuke 2:92f3aa5245dc 144 tx_buf[2] = 0xFD;
PicYusuke 2:92f3aa5245dc 145
PicYusuke 2:92f3aa5245dc 146 // Reserved
PicYusuke 2:92f3aa5245dc 147 tx_buf[3] = 0x00;
PicYusuke 2:92f3aa5245dc 148 }
PicYusuke 2:92f3aa5245dc 149
PicYusuke 2:92f3aa5245dc 150 /**
PicYusuke 2:92f3aa5245dc 151 * @brief チェックサム計算( crc16 )
PicYusuke 2:92f3aa5245dc 152 *
PicYusuke 2:92f3aa5245dc 153 * @param crc_init_val crc 初期値
PicYusuke 2:92f3aa5245dc 154 * @param data 検査対象のデータ列
PicYusuke 2:92f3aa5245dc 155 * @param data_length
PicYusuke 2:92f3aa5245dc 156 * @return uint16_t チェックサム計算結果
PicYusuke 2:92f3aa5245dc 157 */
PicYusuke 2:92f3aa5245dc 158 uint16_t XM430::CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length)
PicYusuke 2:92f3aa5245dc 159 {
PicYusuke 2:92f3aa5245dc 160 uint32_t index;
PicYusuke 2:92f3aa5245dc 161 uint16_t temp = crc_init_val;
PicYusuke 2:92f3aa5245dc 162
PicYusuke 2:92f3aa5245dc 163 for(uint32_t i = 0; i < data_length; i++)
PicYusuke 2:92f3aa5245dc 164 {
PicYusuke 2:92f3aa5245dc 165 index = ((uint16_t)(temp >> 8) ^ data[i]) & 0xFF;
PicYusuke 2:92f3aa5245dc 166 temp = (temp << 8) ^ crc_tbl[index];
PicYusuke 2:92f3aa5245dc 167 }
PicYusuke 2:92f3aa5245dc 168
PicYusuke 2:92f3aa5245dc 169 return temp;
PicYusuke 2:92f3aa5245dc 170 }
PicYusuke 2:92f3aa5245dc 171
PicYusuke 2:92f3aa5245dc 172 /**
PicYusuke 2:92f3aa5245dc 173 * @brief パケット生成
PicYusuke 2:92f3aa5245dc 174 *
PicYusuke 2:92f3aa5245dc 175 * @param servo_id
PicYusuke 2:92f3aa5245dc 176 * @param length
PicYusuke 2:92f3aa5245dc 177 * @param instruction
PicYusuke 2:92f3aa5245dc 178 * @param ctrl_reg control レジスタ
PicYusuke 2:92f3aa5245dc 179 * @param data 送信データ
PicYusuke 2:92f3aa5245dc 180 */
PicYusuke 2:92f3aa5245dc 181 void XM430::Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
PicYusuke 2:92f3aa5245dc 182 const uint16_t ctrl_reg, uint8_t *data)
PicYusuke 2:92f3aa5245dc 183 {
PicYusuke 2:92f3aa5245dc 184 uint16_t checksum;
PicYusuke 2:92f3aa5245dc 185
PicYusuke 2:92f3aa5245dc 186 // ID
PicYusuke 2:92f3aa5245dc 187 tx_buf[4] = servo_id;
PicYusuke 2:92f3aa5245dc 188
PicYusuke 2:92f3aa5245dc 189 // Length
PicYusuke 2:92f3aa5245dc 190 tx_buf[5] = (uint8_t)(length & 0xFF);
PicYusuke 2:92f3aa5245dc 191 tx_buf[6] = (uint8_t)(length >> 8);
PicYusuke 2:92f3aa5245dc 192
PicYusuke 2:92f3aa5245dc 193 // Instruction
PicYusuke 2:92f3aa5245dc 194 tx_buf[7] = instruction;
PicYusuke 2:92f3aa5245dc 195
PicYusuke 2:92f3aa5245dc 196 // control register
PicYusuke 2:92f3aa5245dc 197 tx_buf[8] = (uint8_t)(ctrl_reg & 0xFF);
PicYusuke 2:92f3aa5245dc 198 tx_buf[9] = (uint8_t)(ctrl_reg >> 8);
PicYusuke 2:92f3aa5245dc 199
PicYusuke 2:92f3aa5245dc 200 // Data
PicYusuke 2:92f3aa5245dc 201 for(uint32_t i = 0; i < ( length-5 ); i ++)
PicYusuke 2:92f3aa5245dc 202 {
PicYusuke 2:92f3aa5245dc 203 tx_buf[ i+10 ] = data[i];
PicYusuke 2:92f3aa5245dc 204 }
PicYusuke 2:92f3aa5245dc 205
PicYusuke 2:92f3aa5245dc 206 // Checksum
PicYusuke 2:92f3aa5245dc 207 checksum = CRC16(0, tx_buf, ( length+5 ));
PicYusuke 2:92f3aa5245dc 208 tx_buf[ length+5 ] = (uint8_t)(checksum & 0xFF);
PicYusuke 2:92f3aa5245dc 209 tx_buf[ length+6 ] = (uint8_t)(checksum >> 8);
PicYusuke 2:92f3aa5245dc 210 }
PicYusuke 2:92f3aa5245dc 211
PicYusuke 2:92f3aa5245dc 212 /**
PicYusuke 2:92f3aa5245dc 213 * @brief シリアルバルク送信
PicYusuke 2:92f3aa5245dc 214 *
PicYusuke 2:92f3aa5245dc 215 * @param buf 送信バッファ
PicYusuke 2:92f3aa5245dc 216 * @param buf_length バッファ長
PicYusuke 2:92f3aa5245dc 217 * @return true 正常に通信が終了
PicYusuke 2:92f3aa5245dc 218 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 219 */
PicYusuke 2:92f3aa5245dc 220 bool XM430::Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length)
PicYusuke 2:92f3aa5245dc 221 {
PicYusuke 3:51f72ee2d5c2 222 for(uint32_t i = 0; i < buf_length; i ++)
PicYusuke 3:51f72ee2d5c2 223 {
PicYusuke 3:51f72ee2d5c2 224 #ifdef MBED_ENVIRONMENT
PicYusuke 3:51f72ee2d5c2 225 // ユーザ処理
PicYusuke 3:51f72ee2d5c2 226 uart.putc(buf[i]);
PicYusuke 3:51f72ee2d5c2 227 #else
PicYusuke 3:51f72ee2d5c2 228 std::cout << std::hex << std::showbase << std::uppercase << (uint32_t)tx_buf[i] << std::endl;
PicYusuke 3:51f72ee2d5c2 229 #endif
PicYusuke 3:51f72ee2d5c2 230 }
PicYusuke 3:51f72ee2d5c2 231 return true;
PicYusuke 2:92f3aa5245dc 232 }
PicYusuke 2:92f3aa5245dc 233
PicYusuke 2:92f3aa5245dc 234 /**
PicYusuke 2:92f3aa5245dc 235 * @brief サーボのトルクをONにする
PicYusuke 2:92f3aa5245dc 236 *
PicYusuke 2:92f3aa5245dc 237 * @param id サーボID
PicYusuke 2:92f3aa5245dc 238 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 239 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 240 */
PicYusuke 2:92f3aa5245dc 241 bool XM430::Torque_ON(uint8_t id)
PicYusuke 2:92f3aa5245dc 242 {
PicYusuke 2:92f3aa5245dc 243 uint16_t length = 6;
PicYusuke 2:92f3aa5245dc 244 uint8_t data[1]; // トルクON/OFF
PicYusuke 2:92f3aa5245dc 245
PicYusuke 2:92f3aa5245dc 246 bool ret_val;
PicYusuke 2:92f3aa5245dc 247
PicYusuke 2:92f3aa5245dc 248 data[0] = 1; // トルクON
PicYusuke 2:92f3aa5245dc 249
PicYusuke 2:92f3aa5245dc 250 Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
PicYusuke 2:92f3aa5245dc 251
PicYusuke 2:92f3aa5245dc 252 ret_val = Send_Bulk_Char(tx_buf, 13);
PicYusuke 2:92f3aa5245dc 253 return ret_val;
PicYusuke 2:92f3aa5245dc 254 }
PicYusuke 2:92f3aa5245dc 255
PicYusuke 2:92f3aa5245dc 256 /**
PicYusuke 2:92f3aa5245dc 257 * @brief サーボのトルクをOFFにする
PicYusuke 2:92f3aa5245dc 258 *
PicYusuke 2:92f3aa5245dc 259 * @param id サーボID
PicYusuke 2:92f3aa5245dc 260 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 261 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 262 */
PicYusuke 2:92f3aa5245dc 263 bool XM430::Torque_OFF(uint8_t id)
PicYusuke 2:92f3aa5245dc 264 {
PicYusuke 2:92f3aa5245dc 265 uint16_t length = 6;
PicYusuke 2:92f3aa5245dc 266 uint8_t data[1]; // トルクON/OFF
PicYusuke 2:92f3aa5245dc 267
PicYusuke 2:92f3aa5245dc 268 bool ret_val;
PicYusuke 2:92f3aa5245dc 269
PicYusuke 2:92f3aa5245dc 270 data[0] = 0; // トルクOFF
PicYusuke 2:92f3aa5245dc 271
PicYusuke 2:92f3aa5245dc 272 Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
PicYusuke 2:92f3aa5245dc 273
PicYusuke 2:92f3aa5245dc 274 ret_val = Send_Bulk_Char(tx_buf, 13);
PicYusuke 2:92f3aa5245dc 275 return ret_val;
PicYusuke 2:92f3aa5245dc 276 }
PicYusuke 2:92f3aa5245dc 277
PicYusuke 2:92f3aa5245dc 278 /**
PicYusuke 2:92f3aa5245dc 279 * @brief サーボホーン位置を設定
PicYusuke 2:92f3aa5245dc 280 *
PicYusuke 2:92f3aa5245dc 281 * @param id サーボID
PicYusuke 2:92f3aa5245dc 282 * @param pos 目標回転位置
PicYusuke 2:92f3aa5245dc 283 * @return true 通信が正常に終了
PicYusuke 2:92f3aa5245dc 284 * @return false 通信が不正に終了
PicYusuke 2:92f3aa5245dc 285 */
PicYusuke 2:92f3aa5245dc 286 bool XM430::Set_Pos(uint8_t id, uint32_t pos)
PicYusuke 2:92f3aa5245dc 287 {
PicYusuke 2:92f3aa5245dc 288 uint16_t length = 9;
PicYusuke 2:92f3aa5245dc 289 uint8_t data[4]; // 位置データ
PicYusuke 2:92f3aa5245dc 290
PicYusuke 2:92f3aa5245dc 291 bool ret_val;
PicYusuke 2:92f3aa5245dc 292
PicYusuke 2:92f3aa5245dc 293 // Data
PicYusuke 2:92f3aa5245dc 294 data[0] = (uint8_t)(pos & 0xFF);
PicYusuke 2:92f3aa5245dc 295 data[1] = (uint8_t)((pos >> 8) & 0xFF);
PicYusuke 2:92f3aa5245dc 296 data[2] = (uint8_t)((pos >> 16) & 0xFF);
PicYusuke 2:92f3aa5245dc 297 data[3] = (uint8_t)((pos >> 24) & 0xFF);
PicYusuke 2:92f3aa5245dc 298
PicYusuke 2:92f3aa5245dc 299 Create_Packet(id, length, WRITE, GOAL_POSITION, data);
PicYusuke 2:92f3aa5245dc 300
PicYusuke 2:92f3aa5245dc 301 ret_val = Send_Bulk_Char(tx_buf, 16);
PicYusuke 2:92f3aa5245dc 302 return ret_val;
PicYusuke 2:92f3aa5245dc 303 }
PicYusuke 2:92f3aa5245dc 304
PicYusuke 2:92f3aa5245dc 305 };
PicYusuke 2:92f3aa5245dc 306
PicYusuke 5:4474c07c6274 307