// ライブラリの読み込み #include // IDの定義 #define motor_ID 3 // Dynamixelのモータのアドレス（Protocol 2.0） #define ID_ADDRESS 7 #define PWM_LIMIT_ADDRESS 36 #define MAX_POSITION_LIMIT_ADDRESS 48 #define MIN_POSITION_LIMIT_ADDRESS 52 #define TORQUE_ENABLE_ADDRESS 64 #define LED_ADDRESS 65 #define GOAL_VELOCITY_ADDRESS 104 #define PROFILE_ACCELERATION_ADDRESS 108 #define PROFILE_VELOCITY_ADDRESS 112 #define GOAL_POSITION_ADDRESS 116 #define PRESENT_POSITION_ADDRESS 132 // グローバル変数の設定 uint32_t Motor_present_position; // モータの現在の位置（角度）情報格納用変数 void setup() { // put your setup code here, to run once: Serial.begin(9600); // PCとのシリアル通信の速度を9600bps設定 Serial1.begin(1000000); // Dynamixelとのシリアル通信の1000000bps速度を設定 Serial1.direction(HALFDUPLEX); Serial1.setTimeout(50); // Torque Enableを1に設定確認 Write_1ByteData(motor_ID, TORQUE_ENABLE_ADDRESS, 1); // Torque_Enable = 1に設定 // Profile関係の設定 Write_4ByteData(motor_ID, PROFILE_VELOCITY_ADDRESS, 100); delay (100); } void loop() { // 移動1 Write_4ByteData(motor_ID, GOAL_POSITION_ADDRESS, 1024); // Set Goal Position delay(2000); // 現在のモータの角度を取得（32bit） Read_4ByteData(motor_ID, PRESENT_POSITION_ADDRESS, &Motor_present_position); // モータの角度を表示 Serial.print("Motor(now) : "); Serial.print(Motor_present_position); Serial.print("\n"); delay(1000); // 移動2 Write_4ByteData(motor_ID, GOAL_POSITION_ADDRESS, 3072); // Set Goal Position delay(2000); // 現在のモータの角度を取得（32bit） Read_4ByteData(motor_ID, PRESENT_POSITION_ADDRESS, &Motor_present_position); // モータの角度を表示 Serial.print("Motor(now) : "); Serial.print(Motor_present_position); Serial.print("\n"); delay(1000); } //////////////////// Function //////////////////// // CRC-16-IBMを計算する関数1 uint16_t CRC_calc_1(uint8_t *data, int datasize) { uint16_t crc16; int i,j; crc16 = 0x0000; // 初期値=0 for(i=0;i> 8) ^ data_blk_ptr[j]) & 0xFF; crc_accum = (crc_accum << 8) ^ crc_table[i]; } return crc_accum; } // 指定したアドレスに1byteの情報を書き込む関数 bool Write_1ByteData(uint8_t id, uint16_t address, uint8_t data){ // 送受信用バッファの設定 uint8_t rx_buf[11]; uint8_t tx_buf[] = { 0xff, 0xff, 0xfd, // Header （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） 0x00, // Reserved （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） id, // ID （通信先のID） 6, 0, // size （inst以降のデータ数） 0x03, // inst （1:PING, 2:READ, 3:WRITE,...） (uint8_t)(address & 0xff), // address_L (uint8_t)((address >> 8) & 0xff), // address_H data, 0, // CRC_L 0 // CRC_H }; // CRC計算 uint16_t crc = CRC_calc_2(0, tx_buf, 11); // CRC-16-IBMの計算 tx_buf[11] = (uint8_t)crc & 0xff; // CRC_L tx_buf[12] = (uint8_t)((crc >> 8) & 0xff); // CRC_H // 初期化 while (Serial1.available ()) Serial1.read(); // clear buffer // データの送信 Serial1.write (tx_buf, 13); // send inst packet Serial1.flush(); // データの受信 if (Serial1.readBytes(rx_buf, 11) == 11){ // read stat packet if(rx_buf[4] != id){ // IDの一致をチェック Serial.println("ID not matched!"); return false; } if(rx_buf[8] != 0){ // エラーをチェック Serial.print("Status Error = "); Serial.println(rx_buf[8],HEX); return false; } return true; }else{ Serial.println("Bad response!"); return false; } } // 指定したアドレスに4byteの情報を書き込む関数 bool Write_4ByteData(uint8_t id, uint16_t address, uint32_t data){ // 送受信用バッファの設定 uint8_t rx_buf[11]; uint8_t tx_buf[] = { 0xff, 0xff, 0xfd, // Header （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） 0x00, // Reserved （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） id, // ID （通信先のID） 0x09, 0x00, // size （inst以降のデータ数） 0x03, // inst （1:PING, 2:READ, 3:WRITE,...） (uint8_t)address & 0xff, // address_L (uint8_t)((address >> 8) & 0xff), // address_H (uint8_t)(data & 0xff), (uint8_t)((data >> 8) & 0xff), (uint8_t)((data >> 16) & 0xff), (uint8_t)((data >> 24) & 0xff), 0, // CRC_L 0 // CRC_H }; // CRC計算 uint16_t crc = CRC_calc_2(0, tx_buf, 14); // CRC-16-IBMの計算 tx_buf[14] = (uint8_t)(crc & 0xff); // CRC_L tx_buf[15] = (uint8_t)((crc >> 8) & 0xff); // CRC_H // 初期化 while (Serial1.available ()) Serial1.read(); // clear buffer // データの送信 Serial1.write (tx_buf, 16); // send inst packet Serial1.flush(); // データの受信 if (Serial1.readBytes(rx_buf, 11) == 11){ // read stat packet if(rx_buf[4] != id){ // IDの一致をチェック Serial.println("ID not matched!"); return false; } if(rx_buf[8] != 0){ // エラーをチェック Serial.print("Status Error = "); Serial.println(rx_buf[8],HEX); return false; } return true; }else{ Serial.println("Bad response!"); return false; } } // 指定したアドレスから1byteの情報を取得して指定したuint8_t型変数に格納する関数 bool Read_1ByteData(uint8_t id, uint16_t address, uint8_t *data){ // 送受信用バッファの設定 uint8_t rx_buf[11]; uint8_t tx_buf[] = { 0xff, 0xff, 0xfd, // Header （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） 0x00, // Reserved （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） id, // ID （通信先のID） 7, 0, // size （inst以降のデータ数） 2, // inst （1:PING, 2:READ, 3:WRITE,...） (uint8_t)address & 0xff, // address_L (uint8_t)((address >> 8) & 0xff), // address_H 1, // byte_size_L 0, // byte_size_H 0, // CRC_L 0 // CRC_H }; // CRC計算 uint16_t crc = CRC_calc_2(0, tx_buf, 12); // CRC-16-IBMの計算 tx_buf[12] = (uint8_t)(crc & 0xff); // CRC_L tx_buf[13] = (uint8_t)((crc >> 8) & 0xff); // CRC_H // 初期化 while (Serial1.available ()) Serial1.read(); // clear buffer // データの送信 Serial1.write (tx_buf, 14); // send inst packet Serial1.flush(); // データの受信 if (Serial1.readBytes(rx_buf, 12) == 12){ // read stat packet if(rx_buf[4] != id){ // IDの一致をチェック Serial.println("ID not matched!"); return false; } if(rx_buf[8] != 0){ // エラーをチェック Serial.print("Status Error = "); Serial.println(rx_buf[8]); return false; } *data = rx_buf[9] & 0xff; return true; }else{ Serial.println("Bad response!"); return false; } } // 指定したアドレスから4byteの情報を取得して指定したuint32_t型変数に格納する関数 bool Read_4ByteData(uint8_t id, uint16_t address, uint32_t *data){ // 変数定義 uint32_t A,B,C,D; // 送受信用バッファの設定 uint8_t rx_buf[15]; uint8_t tx_buf[] = { 0xff, 0xff, 0xfd, // Header （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） 0x00, // Reserved （先頭4バイトは0xFF,0xFF,0xFD,0x00の固定値） id, // ID （通信先のID） 7, 0, // size （inst以降のデータ数） 2, // inst （1:PING, 2:READ, 3:WRITE,...） (uint8_t)address & 0xff, // address_L (uint8_t)((address >> 8) & 0xff), // address_H 4, // byte_size_L 0, // byte_size_H 0, // CRC_L 0 // CRC_H }; // CRC計算 uint16_t crc = CRC_calc_2(0, tx_buf, 12); // CRC-16-IBMの計算 tx_buf[12] = (uint8_t)(crc & 0xff); // CRC_L tx_buf[13] = (uint8_t)((crc >> 8) & 0xff); // CRC_H // 初期化 while (Serial1.available ()) Serial1.read(); // clear buffer // データの送信 Serial1.write (tx_buf, 14); // send inst packet Serial1.flush(); // データの受信 if (Serial1.readBytes(rx_buf, 15) == 15){ // read stat packet if(rx_buf[4] != id){ // IDの一致をチェック Serial.println("ID not matched!"); return false; } if(rx_buf[8] != 0){ // エラーをチェック Serial.print("Status Error = "); Serial.println(rx_buf[8]); return false; } A = ((uint32_t)rx_buf[12] << 24) & 0xff000000; B = ((uint32_t)rx_buf[11] << 16) & 0x00ff0000; C = ((uint32_t)rx_buf[10] << 8) & 0x0000ff00; D = (uint32_t)rx_buf[9] & 0x000000ff; *data = A+B+C+D; return true; }else{ Serial.println("Bad response!"); return false; } }