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

main.cpp@1:2c82c636c43e, 2018-06-13 (annotated)

Committer:
PicYusuke
Date:
Wed Jun 13 03:50:30 2018 +0000
Revision:
1:2c82c636c43e
Parent:
0:c9df5db1b525
Child:
2:92f3aa5245dc
First version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PicYusuke 1:2c82c636c43e 1 /*
PicYusuke 1:2c82c636c43e 2 * Copyright (c) 2018 Yusuke Okino
PicYusuke 1:2c82c636c43e 3
PicYusuke 1:2c82c636c43e 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
PicYusuke 1:2c82c636c43e 5 * of this software and associated documentation files (the "Software"), to deal
PicYusuke 1:2c82c636c43e 6 * in the Software without restriction, including without limitation the rights
PicYusuke 1:2c82c636c43e 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
PicYusuke 1:2c82c636c43e 8 * copies of the Software, and to permit persons to whom the Software is
PicYusuke 1:2c82c636c43e 9 * furnished to do so, subject to the following conditions:
PicYusuke 1:2c82c636c43e 10
PicYusuke 1:2c82c636c43e 11 * The above copyright notice and this permission notice shall be included in all
PicYusuke 1:2c82c636c43e 12 * copies or substantial portions of the Software.
PicYusuke 1:2c82c636c43e 13
PicYusuke 1:2c82c636c43e 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
PicYusuke 1:2c82c636c43e 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
PicYusuke 1:2c82c636c43e 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
PicYusuke 1:2c82c636c43e 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
PicYusuke 1:2c82c636c43e 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
PicYusuke 1:2c82c636c43e 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
PicYusuke 1:2c82c636c43e 20 * SOFTWARE.
PicYusuke 0:c9df5db1b525 21 */
PicYusuke 0:c9df5db1b525 22
PicYusuke 1:2c82c636c43e 23
PicYusuke 1:2c82c636c43e 24 #define MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 25
PicYusuke 1:2c82c636c43e 26 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 27 #include "mbed.h"
PicYusuke 1:2c82c636c43e 28 #else
PicYusuke 1:2c82c636c43e 29 #include <iostream>
PicYusuke 1:2c82c636c43e 30 #include <cstdint>
PicYusuke 1:2c82c636c43e 31 #endif
PicYusuke 0:c9df5db1b525 32
PicYusuke 1:2c82c636c43e 33 #ifndef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 34 typedef uint16_t PinName;
PicYusuke 1:2c82c636c43e 35 const PinName PA_9 = 9;
PicYusuke 1:2c82c636c43e 36 const PinName PA_10 = 10;
PicYusuke 1:2c82c636c43e 37 #endif
PicYusuke 1:2c82c636c43e 38
PicYusuke 1:2c82c636c43e 39 /**
PicYusuke 1:2c82c636c43e 40 * @brief Dynamixel サーボコントローラ protocol v2
PicYusuke 1:2c82c636c43e 41 *
PicYusuke 1:2c82c636c43e 42 */
PicYusuke 1:2c82c636c43e 43 namespace dynamixel_servo_v2
PicYusuke 1:2c82c636c43e 44 {
PicYusuke 0:c9df5db1b525 45
PicYusuke 1:2c82c636c43e 46 namespace
PicYusuke 1:2c82c636c43e 47 {
PicYusuke 1:2c82c636c43e 48 // チェックサム計算クラス( 今後実装 )
PicYusuke 1:2c82c636c43e 49 #if 0
PicYusuke 1:2c82c636c43e 50 class CRC16
PicYusuke 1:2c82c636c43e 51 {
PicYusuke 1:2c82c636c43e 52 private:
PicYusuke 0:c9df5db1b525 53
PicYusuke 1:2c82c636c43e 54 public:
PicYusuke 1:2c82c636c43e 55 /**
PicYusuke 1:2c82c636c43e 56 * @brief Construct a new CRC16 object
PicYusuke 1:2c82c636c43e 57 *
PicYusuke 1:2c82c636c43e 58 * @param polynomial
PicYusuke 1:2c82c636c43e 59 */
PicYusuke 1:2c82c636c43e 60 CRC16(const uint16_t polynomial)
PicYusuke 1:2c82c636c43e 61 {
PicYusuke 1:2c82c636c43e 62 /******************** CRC16 用テーブル生成 *********************/
PicYusuke 1:2c82c636c43e 63
PicYusuke 1:2c82c636c43e 64 /**************************************************************/
PicYusuke 1:2c82c636c43e 65 }
PicYusuke 1:2c82c636c43e 66
PicYusuke 1:2c82c636c43e 67 };
PicYusuke 1:2c82c636c43e 68 #endif
PicYusuke 1:2c82c636c43e 69 };
PicYusuke 0:c9df5db1b525 70
PicYusuke 0:c9df5db1b525 71 /**
PicYusuke 0:c9df5db1b525 72 * @brief Dynamixel XM430サーボ制御用クラス
PicYusuke 0:c9df5db1b525 73 *
PicYusuke 0:c9df5db1b525 74 */
PicYusuke 0:c9df5db1b525 75 class XM430
PicYusuke 0:c9df5db1b525 76 {
PicYusuke 0:c9df5db1b525 77 private:
PicYusuke 0:c9df5db1b525 78
PicYusuke 1:2c82c636c43e 79 // Checksum polynomial
PicYusuke 1:2c82c636c43e 80 const static uint16_t CRC16_POLY = 0x8005;
PicYusuke 1:2c82c636c43e 81
PicYusuke 1:2c82c636c43e 82 /********************* レジスタマップ ***********************/
PicYusuke 1:2c82c636c43e 83 /***************** instruction *******************/
PicYusuke 1:2c82c636c43e 84 const static uint8_t WRITE = 0x03;
PicYusuke 1:2c82c636c43e 85 /*************************************************/
PicYusuke 1:2c82c636c43e 86
PicYusuke 1:2c82c636c43e 87 /**************** Control table ******************/
PicYusuke 1:2c82c636c43e 88 const static uint16_t TORQUE_ENABLE = 64;
PicYusuke 1:2c82c636c43e 89 const static uint16_t GOAL_POSITION = 116;
PicYusuke 1:2c82c636c43e 90 /*************************************************/
PicYusuke 1:2c82c636c43e 91 /***********************************************************/
PicYusuke 1:2c82c636c43e 92
PicYusuke 1:2c82c636c43e 93 uint16_t crc_tbl[256]; // CRC16 計算用テーブル
PicYusuke 1:2c82c636c43e 94 uint8_t tx_buf[128]; // シリアル送信バッファ
PicYusuke 1:2c82c636c43e 95 uint8_t rx_buf[128]; // シリアル受信バッファ
PicYusuke 1:2c82c636c43e 96
PicYusuke 1:2c82c636c43e 97 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 98 Serial uart; // シリアル通信用クラス
PicYusuke 0:c9df5db1b525 99
PicYusuke 0:c9df5db1b525 100 /**
PicYusuke 1:2c82c636c43e 101 * @brief UART 受信割り込み関数
PicYusuke 1:2c82c636c43e 102 *
PicYusuke 1:2c82c636c43e 103 */
PicYusuke 1:2c82c636c43e 104 void UART_Rx_Callback()
PicYusuke 1:2c82c636c43e 105 {
PicYusuke 1:2c82c636c43e 106
PicYusuke 1:2c82c636c43e 107 }
PicYusuke 1:2c82c636c43e 108 #endif
PicYusuke 1:2c82c636c43e 109
PicYusuke 1:2c82c636c43e 110 public:
PicYusuke 1:2c82c636c43e 111 /**
PicYusuke 1:2c82c636c43e 112 * @brief Construct a new XM430 object
PicYusuke 0:c9df5db1b525 113 *
PicYusuke 0:c9df5db1b525 114 */
PicYusuke 1:2c82c636c43e 115 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 116 XM430(PinName tx_pin, PinName rx_pin)
PicYusuke 1:2c82c636c43e 117 : uart(tx_pin, rx_pin)
PicYusuke 1:2c82c636c43e 118 {
PicYusuke 1:2c82c636c43e 119 uart.baud(57600);
PicYusuke 1:2c82c636c43e 120 uart.format(8, Serial::None, 1);
PicYusuke 1:2c82c636c43e 121 // UART受信割り込み
PicYusuke 1:2c82c636c43e 122 uart.attach(callback(this, &XM430::UART_Rx_Callback), Serial::RxIrq);
PicYusuke 1:2c82c636c43e 123
PicYusuke 1:2c82c636c43e 124 #else
PicYusuke 1:2c82c636c43e 125 XM430(PinName tx_pin, PinName rx_pin)
PicYusuke 0:c9df5db1b525 126 {
PicYusuke 1:2c82c636c43e 127 #endif
PicYusuke 1:2c82c636c43e 128
PicYusuke 1:2c82c636c43e 129 /******************** CRC16 用テーブル生成 *********************/
PicYusuke 1:2c82c636c43e 130 uint16_t temp = 0;
PicYusuke 1:2c82c636c43e 131
PicYusuke 1:2c82c636c43e 132 for(uint32_t i = 0; i < 256; i ++)
PicYusuke 1:2c82c636c43e 133 {
PicYusuke 1:2c82c636c43e 134 temp = (uint16_t)(i << 8);
PicYusuke 1:2c82c636c43e 135
PicYusuke 1:2c82c636c43e 136 for(uint32_t j = 0; j < 8; j ++)
PicYusuke 1:2c82c636c43e 137 {
PicYusuke 1:2c82c636c43e 138 // tempの最上位ビットが0: ビットシフト
PicYusuke 1:2c82c636c43e 139 // tempの最上位ビットが1: ビットシフト+XOR
PicYusuke 1:2c82c636c43e 140 if((temp & 0x8000) == 0)
PicYusuke 1:2c82c636c43e 141 {
PicYusuke 1:2c82c636c43e 142 temp = (temp << 1);
PicYusuke 1:2c82c636c43e 143 }
PicYusuke 1:2c82c636c43e 144 else
PicYusuke 1:2c82c636c43e 145 {
PicYusuke 1:2c82c636c43e 146 temp = (temp << 1) ^ CRC16_POLY;
PicYusuke 1:2c82c636c43e 147 }
PicYusuke 1:2c82c636c43e 148 }
PicYusuke 1:2c82c636c43e 149 crc_tbl[i] = temp;
PicYusuke 1:2c82c636c43e 150 }
PicYusuke 1:2c82c636c43e 151 /**************************************************************/
PicYusuke 0:c9df5db1b525 152
PicYusuke 1:2c82c636c43e 153 #if 0
PicYusuke 1:2c82c636c43e 154 for(uint32_t i = 0; i < 256; i ++)
PicYusuke 0:c9df5db1b525 155 {
PicYusuke 1:2c82c636c43e 156 std::cout << std::hex << crc_tbl[i] << std::endl;
PicYusuke 1:2c82c636c43e 157 }
PicYusuke 1:2c82c636c43e 158 #endif
PicYusuke 1:2c82c636c43e 159
PicYusuke 1:2c82c636c43e 160 // Header
PicYusuke 1:2c82c636c43e 161 tx_buf[0] = 0xFF;
PicYusuke 1:2c82c636c43e 162 tx_buf[1] = 0xFF;
PicYusuke 1:2c82c636c43e 163 tx_buf[2] = 0xFD;
PicYusuke 1:2c82c636c43e 164
PicYusuke 1:2c82c636c43e 165 // Reserved
PicYusuke 1:2c82c636c43e 166 tx_buf[3] = 0x00;
PicYusuke 1:2c82c636c43e 167 }
PicYusuke 1:2c82c636c43e 168
PicYusuke 1:2c82c636c43e 169
PicYusuke 1:2c82c636c43e 170 private:
PicYusuke 1:2c82c636c43e 171
PicYusuke 1:2c82c636c43e 172 /**
PicYusuke 1:2c82c636c43e 173 * @brief チェックサム計算( crc16 )
PicYusuke 1:2c82c636c43e 174 *
PicYusuke 1:2c82c636c43e 175 * @param crc_init_val crc 初期値
PicYusuke 1:2c82c636c43e 176 * @param data 検査対象のデータ列
PicYusuke 1:2c82c636c43e 177 * @param data_length
PicYusuke 1:2c82c636c43e 178 * @return uint16_t チェックサム計算結果
PicYusuke 1:2c82c636c43e 179 */
PicYusuke 1:2c82c636c43e 180 uint16_t CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length)
PicYusuke 1:2c82c636c43e 181 {
PicYusuke 1:2c82c636c43e 182 uint32_t index;
PicYusuke 1:2c82c636c43e 183 uint16_t temp = crc_init_val;
PicYusuke 1:2c82c636c43e 184
PicYusuke 1:2c82c636c43e 185 for(uint32_t i = 0; i < data_length; i++)
PicYusuke 1:2c82c636c43e 186 {
PicYusuke 1:2c82c636c43e 187 index = ((uint16_t)(temp >> 8) ^ data[i]) & 0xFF;
PicYusuke 1:2c82c636c43e 188 temp = (temp << 8) ^ crc_tbl[index];
PicYusuke 0:c9df5db1b525 189 }
PicYusuke 0:c9df5db1b525 190
PicYusuke 1:2c82c636c43e 191 return temp;
PicYusuke 0:c9df5db1b525 192 }
PicYusuke 0:c9df5db1b525 193
PicYusuke 0:c9df5db1b525 194 /**
PicYusuke 1:2c82c636c43e 195 * @brief パケット生成
PicYusuke 1:2c82c636c43e 196 *
PicYusuke 1:2c82c636c43e 197 * @param servo_id
PicYusuke 1:2c82c636c43e 198 * @param length
PicYusuke 1:2c82c636c43e 199 * @param instruction
PicYusuke 1:2c82c636c43e 200 * @param ctrl_reg control レジスタ
PicYusuke 1:2c82c636c43e 201 * @param data 送信データ
PicYusuke 1:2c82c636c43e 202 */
PicYusuke 1:2c82c636c43e 203 void Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
PicYusuke 1:2c82c636c43e 204 const uint16_t ctrl_reg, uint8_t *data)
PicYusuke 1:2c82c636c43e 205 {
PicYusuke 1:2c82c636c43e 206 uint16_t checksum;
PicYusuke 1:2c82c636c43e 207
PicYusuke 1:2c82c636c43e 208 // ID
PicYusuke 1:2c82c636c43e 209 tx_buf[4] = servo_id;
PicYusuke 1:2c82c636c43e 210
PicYusuke 1:2c82c636c43e 211 // Length
PicYusuke 1:2c82c636c43e 212 tx_buf[5] = (uint8_t)(length & 0xFF);
PicYusuke 1:2c82c636c43e 213 tx_buf[6] = (uint8_t)(length >> 8);
PicYusuke 1:2c82c636c43e 214
PicYusuke 1:2c82c636c43e 215 // Instruction
PicYusuke 1:2c82c636c43e 216 tx_buf[7] = instruction;
PicYusuke 1:2c82c636c43e 217
PicYusuke 1:2c82c636c43e 218 // control register
PicYusuke 1:2c82c636c43e 219 tx_buf[8] = (uint8_t)(ctrl_reg & 0xFF);
PicYusuke 1:2c82c636c43e 220 tx_buf[9] = (uint8_t)(ctrl_reg >> 8);
PicYusuke 1:2c82c636c43e 221
PicYusuke 1:2c82c636c43e 222 // Data
PicYusuke 1:2c82c636c43e 223 for(uint32_t i = 0; i < ( length-5 ); i ++)
PicYusuke 1:2c82c636c43e 224 {
PicYusuke 1:2c82c636c43e 225 tx_buf[ i+10 ] = data[i];
PicYusuke 1:2c82c636c43e 226 }
PicYusuke 1:2c82c636c43e 227
PicYusuke 1:2c82c636c43e 228 // Checksum
PicYusuke 1:2c82c636c43e 229 checksum = CRC16(0, tx_buf, ( length+5 ));
PicYusuke 1:2c82c636c43e 230 tx_buf[ length+5 ] = (uint8_t)(checksum & 0xFF);
PicYusuke 1:2c82c636c43e 231 tx_buf[ length+6 ] = (uint8_t)(checksum >> 8);
PicYusuke 1:2c82c636c43e 232 }
PicYusuke 1:2c82c636c43e 233
PicYusuke 1:2c82c636c43e 234 /**
PicYusuke 1:2c82c636c43e 235 * @brief シリアルバルク送信
PicYusuke 0:c9df5db1b525 236 *
PicYusuke 0:c9df5db1b525 237 * @param buf 送信バッファ
PicYusuke 0:c9df5db1b525 238 * @param buf_length バッファ長
PicYusuke 0:c9df5db1b525 239 * @return true 正常に通信が終了
PicYusuke 0:c9df5db1b525 240 * @return false 通信が不正に終了
PicYusuke 0:c9df5db1b525 241 */
PicYusuke 1:2c82c636c43e 242 bool Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length)
PicYusuke 0:c9df5db1b525 243 {
PicYusuke 1:2c82c636c43e 244 #ifdef MBED_ENVIRONMENT
PicYusuke 0:c9df5db1b525 245 // ユーザ関数
PicYusuke 1:2c82c636c43e 246
PicYusuke 1:2c82c636c43e 247 return true;
PicYusuke 0:c9df5db1b525 248 #else
PicYusuke 0:c9df5db1b525 249 for(uint32_t i = 0; i < buf_length; i ++)
PicYusuke 0:c9df5db1b525 250 {
PicYusuke 0:c9df5db1b525 251 //std::cout << tx_buf[i] << std::endl;
PicYusuke 0:c9df5db1b525 252 std::cout << std::hex << std::showbase << std::uppercase << (uint32_t)tx_buf[i] << std::endl;
PicYusuke 0:c9df5db1b525 253 }
PicYusuke 0:c9df5db1b525 254
PicYusuke 0:c9df5db1b525 255 return true;
PicYusuke 0:c9df5db1b525 256 #endif
PicYusuke 0:c9df5db1b525 257
PicYusuke 0:c9df5db1b525 258 return false;
PicYusuke 0:c9df5db1b525 259 }
PicYusuke 0:c9df5db1b525 260
PicYusuke 1:2c82c636c43e 261 /**
PicYusuke 1:2c82c636c43e 262 * @brief シリアルバルク受信
PicYusuke 1:2c82c636c43e 263 *
PicYusuke 1:2c82c636c43e 264 * @param buf 受信バッファ
PicYusuke 1:2c82c636c43e 265 * @param buf_length バッファ長
PicYusuke 1:2c82c636c43e 266 * @param timeout 通信途絶を判断するまでの時間
PicYusuke 1:2c82c636c43e 267 * @return true 正常に受信
PicYusuke 1:2c82c636c43e 268 * @return false 受信失敗
PicYusuke 1:2c82c636c43e 269 */
PicYusuke 1:2c82c636c43e 270 bool Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout)
PicYusuke 1:2c82c636c43e 271 {
PicYusuke 1:2c82c636c43e 272 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 273 uint32_t count = 0;
PicYusuke 1:2c82c636c43e 274
PicYusuke 1:2c82c636c43e 275 count = 0;
PicYusuke 1:2c82c636c43e 276 while(0 == uart.readable())
PicYusuke 1:2c82c636c43e 277 {
PicYusuke 1:2c82c636c43e 278 if(count > timeout)
PicYusuke 1:2c82c636c43e 279 {
PicYusuke 1:2c82c636c43e 280 return false;
PicYusuke 1:2c82c636c43e 281 }
PicYusuke 1:2c82c636c43e 282 }
PicYusuke 1:2c82c636c43e 283 #endif
PicYusuke 1:2c82c636c43e 284
PicYusuke 1:2c82c636c43e 285 return false;
PicYusuke 1:2c82c636c43e 286 }
PicYusuke 0:c9df5db1b525 287
PicYusuke 0:c9df5db1b525 288 public:
PicYusuke 0:c9df5db1b525 289 /**
PicYusuke 0:c9df5db1b525 290 * @brief サーボのトルクをONにする
PicYusuke 0:c9df5db1b525 291 *
PicYusuke 0:c9df5db1b525 292 * @param id サーボID
PicYusuke 0:c9df5db1b525 293 * @return true 通信が正常に終了
PicYusuke 0:c9df5db1b525 294 * @return false 通信が不正に終了
PicYusuke 0:c9df5db1b525 295 */
PicYusuke 1:2c82c636c43e 296 bool Torque_ON(uint8_t id)
PicYusuke 0:c9df5db1b525 297 {
PicYusuke 0:c9df5db1b525 298 uint16_t length = 6;
PicYusuke 1:2c82c636c43e 299 uint8_t data[1]; // トルクON/OFF
PicYusuke 0:c9df5db1b525 300
PicYusuke 0:c9df5db1b525 301 bool ret_val;
PicYusuke 0:c9df5db1b525 302
PicYusuke 1:2c82c636c43e 303 data[0] = 1; // トルクON
PicYusuke 1:2c82c636c43e 304
PicYusuke 1:2c82c636c43e 305 Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
PicYusuke 0:c9df5db1b525 306
PicYusuke 1:2c82c636c43e 307 ret_val = Send_Bulk_Char(tx_buf, 13);
PicYusuke 1:2c82c636c43e 308 return ret_val;
PicYusuke 1:2c82c636c43e 309 }
PicYusuke 0:c9df5db1b525 310
PicYusuke 1:2c82c636c43e 311 /**
PicYusuke 1:2c82c636c43e 312 * @brief サーボのトルクをOFFにする
PicYusuke 1:2c82c636c43e 313 *
PicYusuke 1:2c82c636c43e 314 * @param id サーボID
PicYusuke 1:2c82c636c43e 315 * @return true 通信が正常に終了
PicYusuke 1:2c82c636c43e 316 * @return false 通信が不正に終了
PicYusuke 1:2c82c636c43e 317 */
PicYusuke 1:2c82c636c43e 318 bool Torque_OFF(uint8_t id)
PicYusuke 1:2c82c636c43e 319 {
PicYusuke 1:2c82c636c43e 320 uint16_t length = 6;
PicYusuke 1:2c82c636c43e 321 uint8_t data[1]; // トルクON/OFF
PicYusuke 0:c9df5db1b525 322
PicYusuke 1:2c82c636c43e 323 bool ret_val;
PicYusuke 0:c9df5db1b525 324
PicYusuke 1:2c82c636c43e 325 data[0] = 0; // トルクOFF
PicYusuke 0:c9df5db1b525 326
PicYusuke 1:2c82c636c43e 327 Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
PicYusuke 0:c9df5db1b525 328
PicYusuke 0:c9df5db1b525 329 ret_val = Send_Bulk_Char(tx_buf, 13);
PicYusuke 0:c9df5db1b525 330 return ret_val;
PicYusuke 0:c9df5db1b525 331 }
PicYusuke 0:c9df5db1b525 332
PicYusuke 0:c9df5db1b525 333 /**
PicYusuke 0:c9df5db1b525 334 * @brief サーボホーン位置を設定
PicYusuke 0:c9df5db1b525 335 *
PicYusuke 0:c9df5db1b525 336 * @param id サーボID
PicYusuke 0:c9df5db1b525 337 * @param pos 目標回転位置
PicYusuke 0:c9df5db1b525 338 * @return true 通信が正常に終了
PicYusuke 0:c9df5db1b525 339 * @return false 通信が不正に終了
PicYusuke 0:c9df5db1b525 340 */
PicYusuke 1:2c82c636c43e 341 bool Set_Pos(uint8_t id, uint32_t pos)
PicYusuke 0:c9df5db1b525 342 {
PicYusuke 0:c9df5db1b525 343 uint16_t length = 9;
PicYusuke 1:2c82c636c43e 344 uint8_t data[4]; // 位置データ
PicYusuke 0:c9df5db1b525 345
PicYusuke 0:c9df5db1b525 346 bool ret_val;
PicYusuke 0:c9df5db1b525 347
PicYusuke 1:2c82c636c43e 348 // Data
PicYusuke 1:2c82c636c43e 349 data[0] = (uint8_t)(pos & 0xFF);
PicYusuke 1:2c82c636c43e 350 data[1] = (uint8_t)((pos >> 8) & 0xFF);
PicYusuke 1:2c82c636c43e 351 data[2] = (uint8_t)((pos >> 16) & 0xFF);
PicYusuke 1:2c82c636c43e 352 data[3] = (uint8_t)((pos >> 24) & 0xFF);
PicYusuke 0:c9df5db1b525 353
PicYusuke 1:2c82c636c43e 354 Create_Packet(id, length, WRITE, GOAL_POSITION, data);
PicYusuke 1:2c82c636c43e 355
PicYusuke 0:c9df5db1b525 356 ret_val = Send_Bulk_Char(tx_buf, 16);
PicYusuke 0:c9df5db1b525 357 return ret_val;
PicYusuke 0:c9df5db1b525 358 }
PicYusuke 0:c9df5db1b525 359 };
PicYusuke 1:2c82c636c43e 360 };
PicYusuke 1:2c82c636c43e 361
PicYusuke 0:c9df5db1b525 362
PicYusuke 0:c9df5db1b525 363 int main()
PicYusuke 0:c9df5db1b525 364 {
PicYusuke 0:c9df5db1b525 365 const uint8_t servo_id = 0x01;
PicYusuke 0:c9df5db1b525 366
PicYusuke 1:2c82c636c43e 367 dynamixel_servo_v2::XM430 xm430(PA_9, PA_10);
PicYusuke 1:2c82c636c43e 368
PicYusuke 1:2c82c636c43e 369 xm430.Torque_ON(servo_id);
PicYusuke 1:2c82c636c43e 370
PicYusuke 1:2c82c636c43e 371 // 少し待つ処理が必要( サーボからの返答を受信する処理を入れたら不要になる )
PicYusuke 1:2c82c636c43e 372 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 373 wait(0.1);
PicYusuke 1:2c82c636c43e 374 #endif
PicYusuke 0:c9df5db1b525 375
PicYusuke 0:c9df5db1b525 376 xm430.Set_Pos(servo_id, 999);
PicYusuke 1:2c82c636c43e 377
PicYusuke 1:2c82c636c43e 378
PicYusuke 1:2c82c636c43e 379 #ifdef MBED_ENVIRONMENT
PicYusuke 1:2c82c636c43e 380 while(1)
PicYusuke 1:2c82c636c43e 381 {
PicYusuke 1:2c82c636c43e 382
PicYusuke 1:2c82c636c43e 383 }
PicYusuke 1:2c82c636c43e 384 #endif
PicYusuke 0:c9df5db1b525 385
PicYusuke 0:c9df5db1b525 386 return 0;
PicYusuke 1:2c82c636c43e 387 }
PicYusuke 1:2c82c636c43e 388
PicYusuke 1:2c82c636c43e 389