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@4:ce4fef97e7e5, 2018-06-13 (annotated)

Committer:
PicYusuke
Date:
Wed Jun 13 14:26:28 2018 +0000
Revision:
4:ce4fef97e7e5
Parent:
3:51f72ee2d5c2
Child:
5:4474c07c6274
Deleted Read_Bulk_Char() function.

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