From a43ecda3932606434c5a3d7da49c29798b5152bd Mon Sep 17 00:00:00 2001 From: "david.park" Date: Fri, 9 Jul 2021 19:24:25 +0900 Subject: [PATCH 01/10] v1 --- .../Blender_Motion_Trajectory_v4.1.ino | 906 ++++++++++++++++++ 1 file changed, 906 insertions(+) create mode 100644 examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino diff --git a/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino b/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino new file mode 100644 index 0000000..13f993b --- /dev/null +++ b/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino @@ -0,0 +1,906 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +// Data struct for SyncRead +typedef struct sr_data{ + int32_t present_position; +} __attribute__((packed)) sr_data_t; + +// Data struct for SyncWrite +typedef struct sw_data{ + int32_t goal_position; +} __attribute__((packed)) sw_data_t; + +//********** SyncWrite********* +sw_data_t sw_data[5]; +DYNAMIXEL::InfoSyncWriteInst_t sw_infos; +DYNAMIXEL::XELInfoSyncWrite_t arr_info_xels_sw[5]; + +//********** SyncRead********** +sr_data_t sr_data[5]; +DYNAMIXEL::InfoSyncReadInst_t sr_infos; +DYNAMIXEL::XELInfoSyncRead_t arr_info_xels_sr[5]; + +const float DXL_PROTOCOL_VERSION = 2.0; + +const uint8_t arr_dxl_id[5] = {1,2,3,4,5}; + +unsigned long now = 0; +unsigned long past = 0; + +const uint16_t g_user_pkt_buf_cap = 128; +uint8_t g_user_pkt_buf[g_user_pkt_buf_cap]; + +const uint16_t SR_START_ADDR = 132; +const uint16_t SR_ADDR_LEN = 4; + +//Goal Position +const uint16_t SW_START_ADDR = 116; + +const uint16_t SW_ADDR_LEN = 4; + +int loop_counts = 2; // The number of times of loop + +int g_frame_time = 0; + +// The number of information in frames +const int g_total_row = 6; + +// Paste all keyframes. +const int arr_trajectory [][g_total_row]{ +//Time[ms],ID1,ID2,ID3,ID4,Hand, +{100,2047,1535,2957,1649,2039}, +{200,2047,1535,2957,1650,2039}, +{300,2047,1534,2956,1652,2039}, +{400,2047,1533,2956,1655,2039}, +{500,2047,1531,2954,1660,2039}, +{600,2047,1529,2953,1666,2039}, +{700,2047,1526,2951,1674,2039}, +{800,2047,1523,2949,1682,2039}, +{900,2047,1520,2947,1692,2039}, +{1000,2047,1516,2944,1702,2039}, +{1100,2047,1511,2941,1714,2039}, +{1200,2047,1507,2938,1726,2039}, +{1300,2047,1502,2934,1740,2039}, +{1400,2047,1497,2930,1754,2039}, +{1500,2047,1492,2926,1769,2039}, +{1600,2047,1486,2921,1784,2039}, +{1700,2047,1480,2916,1800,2039}, +{1800,2047,1474,2911,1817,2039}, +{1900,2047,1468,2905,1834,2039}, +{2000,2047,1462,2899,1851,2039}, +{2100,2047,1455,2893,1869,2039}, +{2200,2047,1448,2886,1887,2039}, +{2300,2047,1442,2879,1906,2039}, +{2400,2047,1435,2872,1924,2039}, +{2500,2047,1428,2865,1943,2039}, +{2600,2047,1421,2857,1962,2039}, +{2700,2047,1415,2849,1980,2039}, +{2800,2047,1408,2840,1999,2039}, +{2900,2047,1401,2831,2018,2039}, +{3000,2047,1394,2822,2036,2039}, +{3100,2047,1388,2813,2054,2039}, +{3200,2047,1381,2803,2072,2039}, +{3300,2047,1375,2793,2090,2039}, +{3400,2047,1369,2783,2107,2039}, +{3500,2047,1363,2772,2123,2039}, +{3600,2047,1357,2761,2139,2039}, +{3700,2047,1351,2750,2155,2039}, +{3800,2047,1346,2738,2170,2039}, +{3900,2047,1341,2726,2184,2039}, +{4000,2047,1336,2714,2197,2039}, +{4100,2047,1331,2702,2209,2039}, +{4200,2047,1327,2689,2221,2039}, +{4300,2047,1323,2676,2232,2039}, +{4400,2047,1320,2662,2241,2039}, +{4500,2047,1317,2648,2250,2039}, +{4600,2047,1314,2634,2257,2039}, +{4700,2047,1312,2620,2263,2039}, +{4800,2047,1310,2605,2268,2039}, +{4900,2047,1309,2590,2272,2039}, +{5000,2047,1308,2575,2274,2039}, +{5100,2047,1308,2559,2274,2039}, +{5200,2048,1308,2543,2274,2039}, +{5300,2049,1308,2527,2274,2039}, +{5400,2052,1308,2510,2274,2039}, +{5500,2056,1308,2493,2274,2039}, +{5600,2061,1308,2476,2274,2039}, +{5700,2067,1308,2459,2274,2039}, +{5800,2074,1308,2442,2274,2039}, +{5900,2082,1308,2424,2274,2039}, +{6000,2091,1308,2407,2274,2039}, +{6100,2100,1308,2389,2274,2039}, +{6200,2110,1308,2372,2274,2039}, +{6300,2121,1308,2354,2274,2039}, +{6400,2133,1308,2336,2274,2039}, +{6500,2145,1308,2318,2274,2039}, +{6600,2158,1308,2301,2274,2039}, +{6700,2171,1308,2283,2274,2039}, +{6800,2184,1308,2265,2274,2039}, +{6900,2198,1308,2248,2274,2039}, +{7000,2213,1308,2231,2274,2039}, +{7100,2227,1308,2214,2274,2039}, +{7200,2242,1308,2197,2274,2039}, +{7300,2257,1308,2180,2274,2039}, +{7400,2272,1308,2164,2274,2039}, +{7500,2288,1308,2147,2274,2039}, +{7600,2303,1308,2131,2274,2039}, +{7700,2318,1308,2116,2274,2039}, +{7800,2334,1308,2101,2274,2039}, +{7900,2349,1308,2086,2274,2039}, +{8000,2364,1308,2071,2274,2039}, +{8100,2379,1308,2057,2274,2039}, +{8200,2393,1308,2044,2274,2039}, +{8300,2408,1308,2030,2274,2039}, +{8400,2422,1308,2018,2274,2039}, +{8500,2435,1308,2006,2274,2039}, +{8600,2448,1308,1994,2274,2039}, +{8700,2461,1308,1983,2274,2039}, +{8800,2473,1308,1972,2274,2039}, +{8900,2485,1308,1963,2274,2039}, +{9000,2495,1308,1953,2274,2039}, +{9100,2506,1308,1945,2274,2039}, +{9200,2515,1308,1937,2274,2039}, +{9300,2524,1308,1930,2274,2039}, +{9400,2532,1308,1924,2274,2039}, +{9500,2539,1308,1918,2274,2039}, +{9600,2545,1308,1913,2274,2039}, +{9700,2550,1308,1909,2274,2039}, +{9800,2554,1308,1906,2274,2039}, +{9900,2556,1308,1904,2274,2039}, +{10000,2558,1308,1902,2274,2039}, +{10100,2559,1308,1902,2274,2039}, +{10200,2559,1308,1902,2274,2039}, +{10300,2559,1309,1903,2274,2039}, +{10400,2559,1312,1903,2274,2039}, +{10500,2559,1315,1904,2273,2039}, +{10600,2559,1319,1906,2273,2039}, +{10700,2559,1323,1908,2272,2039}, +{10800,2559,1329,1910,2271,2039}, +{10900,2559,1335,1912,2271,2039}, +{11000,2559,1342,1914,2270,2039}, +{11100,2559,1349,1917,2269,2039}, +{11200,2559,1357,1920,2267,2039}, +{11300,2559,1365,1923,2266,2039}, +{11400,2559,1374,1926,2265,2039}, +{11500,2559,1384,1930,2264,2039}, +{11600,2559,1394,1933,2262,2039}, +{11700,2559,1404,1937,2261,2039}, +{11800,2559,1414,1941,2259,2039}, +{11900,2559,1425,1945,2258,2039}, +{12000,2559,1436,1949,2256,2039}, +{12100,2559,1448,1953,2254,2039}, +{12200,2559,1459,1957,2253,2039}, +{12300,2559,1471,1961,2251,2039}, +{12400,2559,1483,1966,2249,2039}, +{12500,2559,1495,1970,2248,2039}, +{12600,2559,1507,1974,2246,2039}, +{12700,2559,1519,1979,2244,2039}, +{12800,2559,1531,1983,2243,2039}, +{12900,2559,1542,1987,2241,2039}, +{13000,2559,1554,1992,2239,2039}, +{13100,2559,1566,1996,2238,2039}, +{13200,2559,1577,2000,2236,2039}, +{13300,2559,1588,2004,2234,2039}, +{13400,2559,1599,2008,2233,2039}, +{13500,2559,1610,2012,2231,2039}, +{13600,2559,1620,2016,2230,2039}, +{13700,2559,1630,2019,2229,2039}, +{13800,2559,1639,2023,2227,2039}, +{13900,2559,1648,2026,2226,2039}, +{14000,2559,1656,2029,2225,2039}, +{14100,2559,1664,2032,2224,2039}, +{14200,2559,1672,2035,2222,2039}, +{14300,2559,1678,2037,2222,2039}, +{14400,2559,1685,2039,2221,2039}, +{14500,2559,1690,2041,2220,2039}, +{14600,2559,1695,2043,2219,2039}, +{14700,2559,1699,2044,2219,2039}, +{14800,2559,1702,2045,2218,2039}, +{14900,2559,1704,2046,2218,2039}, +{15000,2559,1705,2047,2218,2039}, +{15100,2559,1706,2047,2218,2039}, +{15200,2559,1706,2047,2218,2038}, +{15300,2559,1706,2047,2218,2033}, +{15400,2559,1706,2047,2218,2024}, +{15500,2559,1706,2047,2218,2013}, +{15600,2559,1706,2047,2218,1998}, +{15700,2559,1706,2047,2218,1981}, +{15800,2559,1706,2047,2218,1961}, +{15900,2559,1706,2047,2218,1939}, +{16000,2559,1706,2047,2218,1914}, +{16100,2559,1706,2047,2218,1887}, +{16200,2559,1706,2047,2218,1857}, +{16300,2559,1706,2047,2218,1826}, +{16400,2559,1706,2047,2218,1793}, +{16500,2559,1706,2047,2218,1758}, +{16600,2559,1706,2047,2218,1722}, +{16700,2559,1706,2047,2218,1684}, +{16800,2559,1706,2047,2218,1645}, +{16900,2559,1706,2047,2218,1605}, +{17000,2559,1706,2047,2218,1564}, +{17100,2559,1706,2047,2218,1522}, +{17200,2559,1706,2047,2218,1479}, +{17300,2559,1706,2047,2218,1436}, +{17400,2559,1706,2047,2218,1393}, +{17500,2559,1706,2047,2218,1349}, +{17600,2559,1706,2047,2218,1304}, +{17700,2559,1706,2047,2218,1260}, +{17800,2559,1706,2047,2218,1216}, +{17900,2559,1706,2047,2218,1173}, +{18000,2559,1706,2047,2218,1130}, +{18100,2559,1706,2047,2218,1087}, +{18200,2559,1706,2047,2218,1045}, +{18300,2559,1706,2047,2218,1004}, +{18400,2559,1706,2047,2218,964}, +{18500,2559,1706,2047,2218,925}, +{18600,2559,1706,2047,2218,887}, +{18700,2559,1706,2047,2218,851}, +{18800,2559,1706,2047,2218,816}, +{18900,2559,1706,2047,2218,783}, +{19000,2559,1706,2047,2218,752}, +{19100,2559,1706,2047,2218,722}, +{19200,2559,1706,2047,2218,695}, +{19300,2559,1706,2047,2218,670}, +{19400,2559,1706,2047,2218,648}, +{19500,2559,1706,2047,2218,628}, +{19600,2559,1706,2047,2218,611}, +{19700,2559,1706,2047,2218,596}, +{19800,2559,1706,2047,2218,585}, +{19900,2559,1706,2047,2218,576}, +{20000,2559,1706,2047,2218,571}, +{20100,2559,1706,2047,2218,570}, +{20200,2559,1705,2047,2218,570}, +{20300,2559,1704,2047,2220,570}, +{20400,2559,1702,2047,2224,570}, +{20500,2559,1699,2047,2228,570}, +{20600,2559,1695,2047,2234,570}, +{20700,2559,1690,2047,2240,570}, +{20800,2559,1685,2047,2248,570}, +{20900,2559,1678,2047,2257,570}, +{21000,2559,1672,2047,2266,570}, +{21100,2559,1664,2047,2277,570}, +{21200,2559,1656,2047,2288,570}, +{21300,2559,1648,2047,2300,570}, +{21400,2559,1639,2047,2313,570}, +{21500,2559,1630,2047,2326,570}, +{21600,2559,1620,2047,2340,570}, +{21700,2559,1610,2047,2355,570}, +{21800,2559,1599,2047,2370,570}, +{21900,2559,1588,2047,2386,570}, +{22000,2559,1577,2047,2402,570}, +{22100,2559,1566,2047,2418,570}, +{22200,2559,1554,2047,2434,570}, +{22300,2559,1542,2047,2451,570}, +{22400,2559,1531,2047,2468,570}, +{22500,2559,1519,2047,2485,570}, +{22600,2559,1507,2047,2502,570}, +{22700,2559,1495,2047,2519,570}, +{22800,2559,1483,2047,2536,570}, +{22900,2559,1471,2047,2553,570}, +{23000,2559,1459,2047,2570,570}, +{23100,2559,1448,2047,2586,570}, +{23200,2559,1436,2047,2602,570}, +{23300,2559,1425,2047,2618,570}, +{23400,2559,1414,2047,2634,570}, +{23500,2559,1404,2047,2649,570}, +{23600,2559,1394,2047,2664,570}, +{23700,2559,1384,2047,2678,570}, +{23800,2559,1374,2047,2691,570}, +{23900,2559,1365,2047,2704,570}, +{24000,2559,1357,2047,2716,570}, +{24100,2559,1349,2047,2727,570}, +{24200,2559,1342,2047,2738,570}, +{24300,2559,1335,2047,2747,570}, +{24400,2559,1329,2047,2756,570}, +{24500,2559,1323,2047,2764,570}, +{24600,2559,1319,2047,2770,570}, +{24700,2559,1315,2047,2776,570}, +{24800,2559,1312,2047,2780,570}, +{24900,2559,1309,2047,2784,570}, +{25000,2559,1308,2047,2786,570}, +{25100,2559,1308,2047,2786,570}, +{25200,2558,1308,2047,2786,570}, +{25300,2554,1308,2047,2786,570}, +{25400,2548,1308,2047,2786,570}, +{25500,2540,1308,2047,2786,570}, +{25600,2530,1308,2047,2786,570}, +{25700,2518,1308,2047,2786,570}, +{25800,2504,1308,2047,2786,570}, +{25900,2489,1308,2047,2786,570}, +{26000,2471,1308,2047,2786,570}, +{26100,2452,1308,2047,2786,570}, +{26200,2432,1308,2047,2786,570}, +{26300,2410,1308,2047,2786,570}, +{26400,2387,1308,2047,2786,570}, +{26500,2363,1308,2047,2786,570}, +{26600,2338,1308,2047,2786,570}, +{26700,2311,1308,2047,2786,570}, +{26800,2284,1308,2047,2786,570}, +{26900,2256,1308,2047,2786,570}, +{27000,2228,1308,2047,2786,570}, +{27100,2199,1308,2047,2786,570}, +{27200,2169,1308,2047,2786,570}, +{27300,2139,1308,2047,2786,570}, +{27400,2108,1308,2047,2786,570}, +{27500,2078,1308,2047,2786,570}, +{27600,2047,1308,2047,2786,570}, +{27700,2016,1308,2047,2786,570}, +{27800,1986,1308,2047,2786,570}, +{27900,1955,1308,2047,2786,570}, +{28000,1925,1308,2047,2786,570}, +{28100,1895,1308,2047,2786,570}, +{28200,1866,1308,2047,2786,570}, +{28300,1838,1308,2047,2786,570}, +{28400,1810,1308,2047,2786,570}, +{28500,1783,1308,2047,2786,570}, +{28600,1756,1308,2047,2786,570}, +{28700,1731,1308,2047,2786,570}, +{28800,1707,1308,2047,2786,570}, +{28900,1684,1308,2047,2786,570}, +{29000,1662,1308,2047,2786,570}, +{29100,1642,1308,2047,2786,570}, +{29200,1623,1308,2047,2786,570}, +{29300,1605,1308,2047,2786,570}, +{29400,1590,1308,2047,2786,570}, +{29500,1576,1308,2047,2786,570}, +{29600,1564,1308,2047,2786,570}, +{29700,1554,1308,2047,2786,570}, +{29800,1546,1308,2047,2786,570}, +{29900,1540,1308,2047,2786,570}, +{30000,1536,1308,2047,2786,570}, +{30100,1535,1308,2047,2786,570}, +{30200,1535,1308,2047,2786,570}, +{30300,1535,1309,2047,2784,570}, +{30400,1535,1312,2047,2782,570}, +{30500,1535,1315,2047,2778,570}, +{30600,1535,1319,2047,2774,570}, +{30700,1535,1323,2047,2768,570}, +{30800,1535,1329,2047,2762,570}, +{30900,1535,1335,2047,2755,570}, +{31000,1535,1342,2047,2747,570}, +{31100,1535,1349,2047,2739,570}, +{31200,1535,1357,2047,2730,570}, +{31300,1535,1365,2047,2720,570}, +{31400,1535,1374,2047,2710,570}, +{31500,1535,1384,2047,2699,570}, +{31600,1535,1394,2047,2688,570}, +{31700,1535,1404,2047,2676,570}, +{31800,1535,1414,2047,2664,570}, +{31900,1535,1425,2047,2652,570}, +{32000,1535,1436,2047,2639,570}, +{32100,1535,1448,2047,2626,570}, +{32200,1535,1459,2047,2613,570}, +{32300,1535,1471,2047,2600,570}, +{32400,1535,1483,2047,2586,570}, +{32500,1535,1495,2047,2573,570}, +{32600,1535,1507,2047,2559,570}, +{32700,1535,1519,2047,2545,570}, +{32800,1535,1531,2047,2532,570}, +{32900,1535,1542,2047,2518,570}, +{33000,1535,1554,2047,2505,570}, +{33100,1535,1566,2047,2492,570}, +{33200,1535,1577,2047,2479,570}, +{33300,1535,1588,2047,2466,570}, +{33400,1535,1599,2047,2453,570}, +{33500,1535,1610,2047,2441,570}, +{33600,1535,1620,2047,2430,570}, +{33700,1535,1630,2047,2418,570}, +{33800,1535,1639,2047,2408,570}, +{33900,1535,1648,2047,2397,570}, +{34000,1535,1656,2047,2388,570}, +{34100,1535,1664,2047,2379,570}, +{34200,1535,1672,2047,2370,570}, +{34300,1535,1678,2047,2363,570}, +{34400,1535,1685,2047,2356,570}, +{34500,1535,1690,2047,2349,570}, +{34600,1535,1695,2047,2344,570}, +{34700,1535,1699,2047,2340,570}, +{34800,1535,1702,2047,2336,570}, +{34900,1535,1704,2047,2334,570}, +{35000,1535,1705,2047,2332,570}, +{35100,1535,1706,2047,2331,570}, +{35200,1535,1706,2047,2331,571}, +{35300,1535,1706,2047,2331,576}, +{35400,1535,1706,2047,2331,585}, +{35500,1535,1706,2047,2331,596}, +{35600,1535,1706,2047,2331,611}, +{35700,1535,1706,2047,2331,628}, +{35800,1535,1706,2047,2331,648}, +{35900,1535,1706,2047,2331,670}, +{36000,1535,1706,2047,2331,695}, +{36100,1535,1706,2047,2331,722}, +{36200,1535,1706,2047,2331,752}, +{36300,1535,1706,2047,2331,783}, +{36400,1535,1706,2047,2331,816}, +{36500,1535,1706,2047,2331,851}, +{36600,1535,1706,2047,2331,887}, +{36700,1535,1706,2047,2331,925}, +{36800,1535,1706,2047,2331,964}, +{36900,1535,1706,2047,2331,1004}, +{37000,1535,1706,2047,2331,1045}, +{37100,1535,1706,2047,2331,1087}, +{37200,1535,1706,2047,2331,1130}, +{37300,1535,1706,2047,2331,1173}, +{37400,1535,1706,2047,2331,1216}, +{37500,1535,1706,2047,2331,1260}, +{37600,1535,1706,2047,2331,1305}, +{37700,1535,1706,2047,2331,1349}, +{37800,1535,1706,2047,2331,1393}, +{37900,1535,1706,2047,2331,1436}, +{38000,1535,1706,2047,2331,1479}, +{38100,1535,1706,2047,2331,1522}, +{38200,1535,1706,2047,2331,1564}, +{38300,1535,1706,2047,2331,1605}, +{38400,1535,1706,2047,2331,1645}, +{38500,1535,1706,2047,2331,1684}, +{38600,1535,1706,2047,2331,1722}, +{38700,1535,1706,2047,2331,1758}, +{38800,1535,1706,2047,2331,1793}, +{38900,1535,1706,2047,2331,1826}, +{39000,1535,1706,2047,2331,1857}, +{39100,1535,1706,2047,2331,1887}, +{39200,1535,1706,2047,2331,1914}, +{39300,1535,1706,2047,2331,1939}, +{39400,1535,1706,2047,2331,1961}, +{39500,1535,1706,2047,2331,1981}, +{39600,1535,1706,2047,2331,1998}, +{39700,1535,1706,2047,2331,2013}, +{39800,1535,1706,2047,2331,2024}, +{39900,1535,1706,2047,2331,2033}, +{40000,1535,1706,2047,2331,2038}, +{40100,1535,1706,2047,2331,2040}, +{40200,1536,1705,2047,2331,2040}, +{40300,1537,1704,2048,2331,2040}, +{40400,1540,1702,2050,2331,2040}, +{40500,1544,1699,2052,2331,2040}, +{40600,1549,1695,2055,2331,2039}, +{40700,1555,1691,2058,2331,2039}, +{40800,1561,1685,2062,2331,2039}, +{40900,1569,1679,2066,2331,2039}, +{41000,1577,1673,2071,2331,2039}, +{41100,1586,1666,2077,2331,2039}, +{41200,1596,1658,2083,2331,2039}, +{41300,1607,1650,2089,2331,2039}, +{41400,1618,1641,2096,2330,2039}, +{41500,1630,1632,2104,2330,2039}, +{41600,1642,1623,2111,2330,2039}, +{41700,1655,1613,2120,2330,2039}, +{41800,1668,1603,2128,2329,2039}, +{41900,1681,1592,2137,2329,2039}, +{42000,1695,1581,2147,2328,2039}, +{42100,1710,1570,2157,2328,2039}, +{42200,1724,1559,2167,2327,2039}, +{42300,1739,1547,2177,2327,2039}, +{42400,1754,1536,2188,2326,2039}, +{42500,1769,1524,2199,2325,2039}, +{42600,1784,1513,2210,2325,2039}, +{42700,1799,1501,2222,2324,2039}, +{42800,1814,1489,2234,2323,2039}, +{42900,1829,1478,2246,2322,2039}, +{43000,1843,1466,2259,2321,2039}, +{43100,1858,1455,2271,2320,2039}, +{43200,1873,1443,2284,2319,2039}, +{43300,1887,1432,2297,2317,2039}, +{43400,1901,1421,2310,2316,2039}, +{43500,1914,1411,2323,2315,2039}, +{43600,1927,1401,2337,2313,2039}, +{43700,1940,1391,2350,2311,2039}, +{43800,1952,1381,2364,2310,2039}, +{43900,1964,1372,2378,2308,2039}, +{44000,1975,1363,2392,2306,2039}, +{44100,1986,1355,2406,2304,2039}, +{44200,1996,1348,2420,2302,2039}, +{44300,2005,1340,2434,2300,2039}, +{44400,2013,1334,2448,2297,2039}, +{44500,2021,1328,2462,2295,2039}, +{44600,2027,1323,2476,2292,2039}, +{44700,2033,1318,2490,2290,2039}, +{44800,2038,1315,2504,2287,2039}, +{44900,2042,1312,2518,2284,2039}, +{45000,2045,1309,2531,2281,2039}, +{45100,2046,1308,2545,2278,2039}, +{45200,2047,1308,2559,2274,2039}, +{45300,2047,1308,2572,2271,2039}, +{45400,2047,1309,2586,2265,2039}, +{45500,2047,1310,2599,2259,2039}, +{45600,2047,1312,2612,2251,2039}, +{45700,2047,1314,2625,2243,2039}, +{45800,2047,1317,2638,2233,2039}, +{45900,2047,1320,2651,2223,2039}, +{46000,2047,1324,2663,2211,2039}, +{46100,2047,1328,2676,2199,2039}, +{46200,2047,1332,2688,2186,2039}, +{46300,2047,1337,2700,2172,2039}, +{46400,2047,1342,2712,2157,2039}, +{46500,2047,1347,2723,2142,2039}, +{46600,2047,1353,2735,2127,2039}, +{46700,2047,1359,2746,2110,2039}, +{46800,2047,1365,2757,2094,2039}, +{46900,2047,1371,2768,2077,2039}, +{47000,2047,1377,2779,2059,2039}, +{47100,2047,1384,2789,2041,2039}, +{47200,2047,1390,2799,2023,2039}, +{47300,2047,1397,2809,2005,2039}, +{47400,2047,1404,2818,1987,2039}, +{47500,2047,1411,2828,1969,2039}, +{47600,2047,1418,2837,1950,2039}, +{47700,2047,1425,2846,1932,2039}, +{47800,2047,1432,2854,1914,2039}, +{47900,2047,1439,2862,1896,2039}, +{48000,2047,1446,2870,1878,2039}, +{48100,2047,1452,2878,1860,2039}, +{48200,2047,1459,2885,1843,2039}, +{48300,2047,1466,2892,1826,2039}, +{48400,2047,1472,2899,1810,2039}, +{48500,2047,1478,2905,1794,2039}, +{48600,2047,1484,2911,1778,2039}, +{48700,2047,1490,2917,1763,2039}, +{48800,2047,1496,2922,1749,2039}, +{48900,2047,1501,2927,1736,2039}, +{49000,2047,1506,2932,1723,2039}, +{49100,2047,1511,2936,1711,2039}, +{49200,2047,1515,2940,1700,2039}, +{49300,2047,1519,2943,1690,2039}, +{49400,2047,1523,2947,1681,2039}, +{49500,2047,1526,2949,1673,2039}, +{49600,2047,1529,2952,1666,2039}, +{49700,2047,1531,2954,1660,2039}, +{49800,2047,1533,2955,1655,2039}, +{49900,2047,1534,2956,1652,2039}, +{50000,2047,1535,2957,1650,2039}, +{50100,2047,1535,2957,1649,2039} +}; + +// The number of key frames. + +const int g_total_frame = sizeof(arr_trajectory) / sizeof(arr_trajectory[0]); + +void ReadPresentPosition(); + +void InitDXL(int time_trajectory){ + + int idx; + int axes = g_total_row - 1; + + DEBUG_SERIAL.begin(115200); + + // Port Baudrate to communicate with connected DYNAMIXELs. If not matched, you will meet communication fail. + dxl.begin(4000000); + + // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); + + // Ping DYNAMIXELs. If failed, check your Baudrate and pysical wiring connection. + for (idx = 0; idx < axes; idx++) + { + dxl.ping(arr_dxl_id[idx]); + if(dxl.ping(arr_dxl_id[idx]) == true){ + DEBUG_SERIAL.println("ping succeeded!"); + }else{ + DEBUG_SERIAL.println("ping failed!"); + } + + delay(20); + dxl.torqueOff(arr_dxl_id[idx]); + dxl.writeControlTableItem(DRIVE_MODE, arr_dxl_id[idx], 4); // Set Drive Mode as Time-based profile. + dxl.writeControlTableItem(RETURN_DELAY_TIME, arr_dxl_id[idx], 0); // Less Return Delay Time enhance the ability of response. + dxl.setOperatingMode(arr_dxl_id[idx], OP_POSITION); + dxl.torqueOn(arr_dxl_id[idx]); + dxl.writeControlTableItem(PROFILE_VELOCITY, arr_dxl_id[idx], time_trajectory); // Time-based should be larger than set trajectory time. + } +} + +//********** Storing packets in SyncRead struct********** +void MakeSrStruct(){ + + int axes = g_total_row - 1; + + int idx; + + sr_infos.packet.p_buf = g_user_pkt_buf; + sr_infos.packet.buf_capacity = g_user_pkt_buf_cap; + sr_infos.packet.is_completed = false; + sr_infos.addr = SR_START_ADDR; + sr_infos.addr_length = SR_ADDR_LEN; + sr_infos.p_xels = arr_info_xels_sr; + sr_infos.xel_count = 0; + + for(idx=0; idx < axes ; idx++){ + arr_info_xels_sr[idx].id = arr_dxl_id[idx]; + arr_info_xels_sr[idx].p_recv_buf = (uint8_t*)&sr_data[idx]; + sr_infos.xel_count++; + } + + sr_infos.is_info_changed = true; + + ReadPresentPosition(); + +} + +void MakeSwStruct(const int arr[][g_total_row]){ + + int axes = g_total_row - 1; + + int idx; + + //********** Storing packets in SyncWrite struct********** + sw_infos.packet.p_buf = nullptr; + sw_infos.packet.is_completed = false; + sw_infos.addr = SW_START_ADDR; + sw_infos.addr_length = SW_ADDR_LEN; + sw_infos.p_xels = arr_info_xels_sw; + sw_infos.xel_count = 0; + + for(idx=0; idx < axes; idx++){ + sw_data[idx].goal_position = arr[0][idx+1]; + } + + for(idx=0; idx < axes; idx++){ + arr_info_xels_sw[idx].id = arr_dxl_id[idx]; + arr_info_xels_sw[idx].p_data = (uint8_t*)&sw_data[idx].goal_position; + sw_infos.xel_count++; + } + + sw_infos.is_info_changed = true; + +} + +void InitPose(const int init_pose[][g_total_row], int time_trajectory) // Init pose, +{ + + int idx; + + int axes = g_total_row - 1; + + int arr_position_margin[axes] = {}; // Store margin between Goal and Present Position. This detemines the DYNAMIXEL where to rotate in either CW or CCW. + + int arr_dxl_ready_state[axes] = {}; + + int init_steps = 1 ; + + int dxl_ready_count; + + int position_tolerance = 5; + + for (idx = 0; idx < axes; idx++){ + + if (idx == 0){ + DEBUG_SERIAL.println("[Present Position] "); + } + + arr_position_margin[idx] = init_pose[0][idx+1] - sr_data[idx].present_position; + + DEBUG_SERIAL.print("[Present Position_Setup] ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id);DEBUG_SERIAL.print(": ");DEBUG_SERIAL.println(sr_data[idx].present_position); + + DEBUG_SERIAL.print("[Goal Position - Present Position] ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id);DEBUG_SERIAL.print(": ");DEBUG_SERIAL.println(arr_position_margin[idx]); + + }DEBUG_SERIAL.println(""); + + while(true){ + + for (idx = 0; idx < axes; idx++){ // Count the number of ready DYNAMIXEL. + if (arr_dxl_ready_state[idx] == 1){ + dxl_ready_count++; + }else{ + dxl_ready_count = 0; + break; + } + } + + if(dxl_ready_count == axes){ + + delay(1000); + + for (idx = 0; idx < axes; idx++){ + sw_data[idx].goal_position = init_pose[0][idx+1]; + dxl.ledOff(arr_dxl_id[idx]); + } + + sw_infos.is_info_changed = true; + + dxl.syncWrite(&sw_infos); + + g_frame_time +=1; + + DEBUG_SERIAL.println("========================[Play Motion]==========================="); + + break; + } + + now = millis(); + + if (now - past >= time_trajectory-10){ + for (idx=0; idx < axes; idx++){ + + if (idx == 0){ + DEBUG_SERIAL.println("[Moving to the init pose...]"); + } + + if (arr_position_margin[idx] >= position_tolerance ){ // If result in Goal Position - Present Position is positive number, movement direction is in CCW. + DEBUG_SERIAL.print("DIR :CCW, "); + DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); + arr_position_margin[idx] = arr_position_margin[idx] - init_steps ; //set position every 20 ms + DEBUG_SERIAL.print("\t Target Pose: "); DEBUG_SERIAL.print(init_pose[0][idx+1]); + DEBUG_SERIAL.print("\t Present Pose: "); DEBUG_SERIAL.println(init_pose[0][idx+1] - arr_position_margin[idx]); + }else if (arr_position_margin[idx] <= -position_tolerance ){ // If result in Goal Position - Present Position is negative number, movement direction is in CW. + DEBUG_SERIAL.print("DIR : CW, "); + DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); + arr_position_margin[idx] = arr_position_margin[idx] + init_steps ; + DEBUG_SERIAL.print("\t Target Pose: "); DEBUG_SERIAL.print(init_pose[0][idx+1]); + DEBUG_SERIAL.print("\t Present Pose: "); DEBUG_SERIAL.println(init_pose[0][idx+1] - arr_position_margin[idx]); + }else{ // + arr_dxl_ready_state[idx] = 1; + dxl.ledOn(arr_dxl_id[idx]); + DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); + DEBUG_SERIAL.println(" is ready to go."); + } + + sw_data[idx].goal_position = init_pose[0][idx+1] - arr_position_margin[idx]; + + } + } + + if (now - past >= time_trajectory){ + + sw_infos.is_info_changed = true; + + dxl.syncWrite(&sw_infos); + DEBUG_SERIAL.println(" SyncWrite."); + past = now; + } + } +} + + +void setup() { + + int profile_time = (arr_trajectory[2][0] - arr_trajectory[1][0])*2; // 0; + + InitDXL(profile_time); // Initialize the DYNAMIXEL + + MakeSrStruct(); + + MakeSwStruct(arr_trajectory); + + delay(2000); + + InitPose(arr_trajectory, profile_time); // Pass a pose, +} + +void loop() { + + now = millis(); + + PlayMotion(arr_trajectory); // Pass the trajectory information to a motion generator (PlayMotion). + +} + +void PlayMotion(const int arr[][g_total_row]){ + + int axes = g_total_row - 1; + + if(now - past >= arr[g_frame_time+1][0] - arr[g_frame_time][0]){ // Time difference between the past and current time. + +// DisplayTime(); +// ReadPresentPosition(); + + int idx; + + for (idx = 0; idx < axes; idx++) { + + if (idx == 0){ + DEBUG_SERIAL.print("[Goal Position]\t "); + } + + DEBUG_SERIAL.print("ID");DEBUG_SERIAL.print(arr_dxl_id[idx]); + sw_data[idx].goal_position = arr[g_frame_time+1][idx+1]; + DEBUG_SERIAL.print(": ");DEBUG_SERIAL.print(sw_data[idx].goal_position);DEBUG_SERIAL.print(" "); + + }DEBUG_SERIAL.println(""); + + sw_infos.is_info_changed = true; + + dxl.syncWrite(&sw_infos); + + past = now; + + g_frame_time +=1; + + if (g_frame_time >= g_total_frame -1 ){ // Trajectory Point at 0 excluded. + + DEBUG_SERIAL.println("============================================================================="); + DEBUG_SERIAL.println("++++++++++++++++++++++++++ Motion Complete ++++++++++++++++++++++++++++++++++"); + DEBUG_SERIAL.print("Total Frames = ");DEBUG_SERIAL.println(g_frame_time + 1); + DEBUG_SERIAL.println("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"); + DEBUG_SERIAL.println("============================================================================="); + + g_frame_time = 1; + + loop_counts--; + + } + } +} + +void DisplayTime(){ // Display current time, past time, and elapsed time in two trajectory points. If not needed, comment out. + + DEBUG_SERIAL.println("========================[Keyframe is ongoing]==========================="); + DEBUG_SERIAL.print("Now: ");DEBUG_SERIAL.print(now);DEBUG_SERIAL.print(" ms"); + DEBUG_SERIAL.print("\t Past: ");DEBUG_SERIAL.print(past);DEBUG_SERIAL.print(" ms"); + DEBUG_SERIAL.print("\t Elapsed Time:");DEBUG_SERIAL.print(now - past);DEBUG_SERIAL.println(" ms"); + DEBUG_SERIAL.print("[Trajectory Point] :");DEBUG_SERIAL.println(arr_trajectory[g_frame_time+1][0]); +} + +void ReadPresentPosition(){ + + int idx; + + int axes = g_total_row - 1; + + for (idx = 0; idx < axes; idx++){ + + if (idx ==0){ + DEBUG_SERIAL.print("[Present Position] "); + } + + DEBUG_SERIAL.print(" ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); + DEBUG_SERIAL.print(": ");DEBUG_SERIAL.print(sr_data[idx].present_position); + + }DEBUG_SERIAL.println(""); + + dxl.syncRead(&sr_infos); + +} From 180895cef5eab0f359619d085851680ae2e4b86e Mon Sep 17 00:00:00 2001 From: "david.park" Date: Fri, 9 Jul 2021 19:27:19 +0900 Subject: [PATCH 02/10] version_change --- ...ion_Trajectory_v4.1.ino => Blender_Motion_Trajectory_v1.1.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/blender/Blender_Motion_v1.1/{Blender_Motion_Trajectory_v4.1.ino => Blender_Motion_Trajectory_v1.1.ino} (100%) diff --git a/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino b/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino similarity index 100% rename from examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v4.1.ino rename to examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino From 168bfb62e96e5f984f80790dce5cf2f7e83d217a Mon Sep 17 00:00:00 2001 From: Will Son Date: Tue, 13 Jul 2021 14:43:24 +0900 Subject: [PATCH 03/10] example review rev1 --- .../openmanipulator_x.ino} | 548 +++++++++--------- 1 file changed, 261 insertions(+), 287 deletions(-) rename examples/blender/{Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino => openmanipulator_x/openmanipulator_x.ino} (66%) diff --git a/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino b/examples/blender/openmanipulator_x/openmanipulator_x.ino similarity index 66% rename from examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino rename to examples/blender/openmanipulator_x/openmanipulator_x.ino index 13f993b..672e131 100644 --- a/examples/blender/Blender_Motion_v1.1/Blender_Motion_Trajectory_v1.1.ino +++ b/examples/blender/openmanipulator_x/openmanipulator_x.ino @@ -50,56 +50,59 @@ Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); -//This namespace is required to use Control table item names +// This namespace is required to use Control table item names using namespace ControlTableItem; -// Data struct for SyncRead -typedef struct sr_data{ - int32_t present_position; -} __attribute__((packed)) sr_data_t; - -// Data struct for SyncWrite -typedef struct sw_data{ - int32_t goal_position; -} __attribute__((packed)) sw_data_t; +// DYNAMIXEL Configuration +const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; +// OpenMANIPULATOR has ID of 11 ~ 15 by default +const uint8_t DYNAMIXEL_ID[] = {1, 2, 3, 4, 5}; +const uint8_t NUMBER_OF_JOINT = sizeof(DYNAMIXEL_ID)/sizeof(uint8_t); -//********** SyncWrite********* -sw_data_t sw_data[5]; -DYNAMIXEL::InfoSyncWriteInst_t sw_infos; -DYNAMIXEL::XELInfoSyncWrite_t arr_info_xels_sw[5]; -//********** SyncRead********** -sr_data_t sr_data[5]; -DYNAMIXEL::InfoSyncReadInst_t sr_infos; -DYNAMIXEL::XELInfoSyncRead_t arr_info_xels_sr[5]; +// Data struct for SyncRead +typedef struct sync_read_data { + int32_t present_position; +} __attribute__((packed)) sync_read_data_t; -const float DXL_PROTOCOL_VERSION = 2.0; +// Sync Read Present Position +const uint16_t ADDRESS_TO_SYNC_READ = 132; +const uint16_t LENGTH_TO_SYNC_READ = 4; -const uint8_t arr_dxl_id[5] = {1,2,3,4,5}; +// Sync Read +sync_read_data_t sync_read_data[NUMBER_OF_JOINT]; +DYNAMIXEL::InfoSyncReadInst_t sync_read_information; +DYNAMIXEL::XELInfoSyncRead_t sync_read_dynamixel_info[NUMBER_OF_JOINT]; -unsigned long now = 0; -unsigned long past = 0; -const uint16_t g_user_pkt_buf_cap = 128; -uint8_t g_user_pkt_buf[g_user_pkt_buf_cap]; +// Data struct for SyncWrite +typedef struct sync_write_data { + int32_t goal_position; +} __attribute__((packed)) sync_write_data_t; -const uint16_t SR_START_ADDR = 132; -const uint16_t SR_ADDR_LEN = 4; +// Sync Write Goal Position +const uint16_t ADDRESS_TO_SYNC_WRITE = 116; +const uint16_t LENGTH_TO_SYNC_WRITE = 4; -//Goal Position -const uint16_t SW_START_ADDR = 116; +// Sync Write +sync_write_data_t sync_write_data[NUMBER_OF_JOINT]; +DYNAMIXEL::InfoSyncWriteInst_t sync_write_information; +DYNAMIXEL::XELInfoSyncWrite_t sync_write_dynamixel_info[NUMBER_OF_JOINT]; -const uint16_t SW_ADDR_LEN = 4; -int loop_counts = 2; // The number of times of loop +const uint16_t MAX_PACKET_BUFFER_LENGTH = 128; +uint8_t packet_buffer[MAX_PACKET_BUFFER_LENGTH]; -int g_frame_time = 0; +int loop_counts = 2; // Number of times to play the motion +int frame_time = 0; +unsigned long current_time = 0; +unsigned long saved_time = 0; -// The number of information in frames -const int g_total_row = 6; +// The number of information in each frame. (Time + Position of each Joint) +const int MAX_MOTION_DATA_COLUMN_INDEX = 1 + NUMBER_OF_JOINT; -// Paste all keyframes. -const int arr_trajectory [][g_total_row]{ +// Paste all keyframes. +const int MOTION_DATA [][MAX_MOTION_DATA_COLUMN_INDEX]{ //Time[ms],ID1,ID2,ID3,ID4,Hand, {100,2047,1535,2957,1649,2039}, {200,2047,1535,2957,1650,2039}, @@ -605,302 +608,273 @@ const int arr_trajectory [][g_total_row]{ }; // The number of key frames. +const int TOTAL_FRAMES = sizeof(MOTION_DATA) / sizeof(MOTION_DATA[0]); -const int g_total_frame = sizeof(arr_trajectory) / sizeof(arr_trajectory[0]); - -void ReadPresentPosition(); - -void InitDXL(int time_trajectory){ - - int idx; - int axes = g_total_row - 1; +void setup() +{ + int time_between_frame = (MOTION_DATA[2][0] - MOTION_DATA[1][0]) * 2; + // Open serial port for debugging DEBUG_SERIAL.begin(115200); // Port Baudrate to communicate with connected DYNAMIXELs. If not matched, you will meet communication fail. dxl.begin(4000000); - - // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. - dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); - + + // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. + dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); + + InitDXL(time_between_frame); // Initialize the DYNAMIXEL + InitSyncRead(); + InitSyncWrite(); + delay(2000); + InitPose(MOTION_DATA, time_between_frame); // Pass a pose +} + +void loop() +{ + // Pass the motion data + PlayMotion(MOTION_DATA); +} + +void InitDXL(int frame_time) +{ + uint8_t index = 0; + // Ping DYNAMIXELs. If failed, check your Baudrate and pysical wiring connection. - for (idx = 0; idx < axes; idx++) + for (index = 0; index < NUMBER_OF_JOINT; index++) { - dxl.ping(arr_dxl_id[idx]); - if(dxl.ping(arr_dxl_id[idx]) == true){ - DEBUG_SERIAL.println("ping succeeded!"); - }else{ - DEBUG_SERIAL.println("ping failed!"); + if (!dxl.ping(DYNAMIXEL_ID[index])) { + DEBUG_SERIAL.print("[ERROR] DYNAMIXEL ID "); + DEBUG_SERIAL.print(index); + DEBUG_SERIAL.println(" is not detected."); + } else { + dxl.torqueOff(DYNAMIXEL_ID[index]); + // Set the Drive Mode as Time-based mode. + dxl.writeControlTableItem(DRIVE_MODE, DYNAMIXEL_ID[index], 4); + // Lower Return Delay Time enhances the responsiveness. + dxl.writeControlTableItem(RETURN_DELAY_TIME, DYNAMIXEL_ID[index], 0); + dxl.setOperatingMode(DYNAMIXEL_ID[index], OP_POSITION); + dxl.torqueOn(DYNAMIXEL_ID[index]); + // Time-based should be larger than frame time. + dxl.writeControlTableItem(PROFILE_VELOCITY, DYNAMIXEL_ID[index], frame_time); } - - delay(20); - dxl.torqueOff(arr_dxl_id[idx]); - dxl.writeControlTableItem(DRIVE_MODE, arr_dxl_id[idx], 4); // Set Drive Mode as Time-based profile. - dxl.writeControlTableItem(RETURN_DELAY_TIME, arr_dxl_id[idx], 0); // Less Return Delay Time enhance the ability of response. - dxl.setOperatingMode(arr_dxl_id[idx], OP_POSITION); - dxl.torqueOn(arr_dxl_id[idx]); - dxl.writeControlTableItem(PROFILE_VELOCITY, arr_dxl_id[idx], time_trajectory); // Time-based should be larger than set trajectory time. } } -//********** Storing packets in SyncRead struct********** -void MakeSrStruct(){ - - int axes = g_total_row - 1; - - int idx; - - sr_infos.packet.p_buf = g_user_pkt_buf; - sr_infos.packet.buf_capacity = g_user_pkt_buf_cap; - sr_infos.packet.is_completed = false; - sr_infos.addr = SR_START_ADDR; - sr_infos.addr_length = SR_ADDR_LEN; - sr_infos.p_xels = arr_info_xels_sr; - sr_infos.xel_count = 0; - - for(idx=0; idx < axes ; idx++){ - arr_info_xels_sr[idx].id = arr_dxl_id[idx]; - arr_info_xels_sr[idx].p_recv_buf = (uint8_t*)&sr_data[idx]; - sr_infos.xel_count++; +// Initialize SyncRead packet in the structure +void InitSyncRead() +{ + uint8_t index = 0; + + sync_read_information.packet.p_buf = packet_buffer; + sync_read_information.packet.buf_capacity = MAX_PACKET_BUFFER_LENGTH; + sync_read_information.packet.is_completed = false; + sync_read_information.addr = ADDRESS_TO_SYNC_READ; + sync_read_information.addr_length = LENGTH_TO_SYNC_READ; + sync_read_information.p_xels = sync_read_dynamixel_info; + sync_read_information.xel_count = 0; + + for (index = 0; index < NUMBER_OF_JOINT ; index++) { + sync_read_dynamixel_info[index].id = DYNAMIXEL_ID[index]; + sync_read_dynamixel_info[index].p_recv_buf = (uint8_t*)&sync_read_data[index]; + sync_read_information.xel_count++; } - - sr_infos.is_info_changed = true; - - ReadPresentPosition(); + sync_read_information.is_info_changed = true; + ReadPresentPosition(); } -void MakeSwStruct(const int arr[][g_total_row]){ - - int axes = g_total_row - 1; - - int idx; - - //********** Storing packets in SyncWrite struct********** - sw_infos.packet.p_buf = nullptr; - sw_infos.packet.is_completed = false; - sw_infos.addr = SW_START_ADDR; - sw_infos.addr_length = SW_ADDR_LEN; - sw_infos.p_xels = arr_info_xels_sw; - sw_infos.xel_count = 0; - - for(idx=0; idx < axes; idx++){ - sw_data[idx].goal_position = arr[0][idx+1]; - } - - for(idx=0; idx < axes; idx++){ - arr_info_xels_sw[idx].id = arr_dxl_id[idx]; - arr_info_xels_sw[idx].p_data = (uint8_t*)&sw_data[idx].goal_position; - sw_infos.xel_count++; - } +// Initialize SyncWrite packet frame in the structure +void InitSyncWrite() +{ + uint8_t index = 0; - sw_infos.is_info_changed = true; - -} - -void InitPose(const int init_pose[][g_total_row], int time_trajectory) // Init pose, -{ - - int idx; + sync_write_information.packet.p_buf = nullptr; + sync_write_information.packet.is_completed = false; + sync_write_information.addr = ADDRESS_TO_SYNC_WRITE; + sync_write_information.addr_length = LENGTH_TO_SYNC_WRITE; + sync_write_information.p_xels = sync_write_dynamixel_info; + sync_write_information.xel_count = 0; + + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_dynamixel_info[index].id = DYNAMIXEL_ID[index]; + sync_write_dynamixel_info[index].p_data = (uint8_t*)&sync_write_data[index].goal_position; + sync_write_information.xel_count++; + } +} - int axes = g_total_row - 1; - - int arr_position_margin[axes] = {}; // Store margin between Goal and Present Position. This detemines the DYNAMIXEL where to rotate in either CW or CCW. - - int arr_dxl_ready_state[axes] = {}; +void InitPose(const int init_pose[][MAX_MOTION_DATA_COLUMN_INDEX], int frame_time) +{ + uint8_t index = 0; + // Store margin between Goal and Present Position. + // This determines the DYNAMIXEL where to rotate in either CW or CCW. + int arr_position_margin[NUMBER_OF_JOINT] = {}; + int arr_dxl_ready_state[NUMBER_OF_JOINT] = {}; + int init_steps = 1; + int dxl_ready_count = 0; + int position_tolerance = 5; + + for (index = 0; index < NUMBER_OF_JOINT; index++){ + if (index == 0) { + DEBUG_SERIAL.println("[Present Position] "); + } - int init_steps = 1 ; + arr_position_margin[index] = init_pose[0][index+1] - sync_read_data[index].present_position; + DEBUG_SERIAL.print("[Present Position_Setup] ID"); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.print(": "); + DEBUG_SERIAL.println(sync_read_data[index].present_position); + DEBUG_SERIAL.print("[Goal Position - Present Position] ID"); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.print(": "); + DEBUG_SERIAL.println(arr_position_margin[index]); + } - int dxl_ready_count; + DEBUG_SERIAL.println(""); - int position_tolerance = 5; + while(true) { + // Count the number of ready DYNAMIXEL. + for (index = 0; index < NUMBER_OF_JOINT; index++) { + if (arr_dxl_ready_state[index] == 1) { + dxl_ready_count++; + } else { + dxl_ready_count = 0; + break; + } + } - for (idx = 0; idx < axes; idx++){ + if(dxl_ready_count == NUMBER_OF_JOINT) { + delay(1000); - if (idx == 0){ - DEBUG_SERIAL.println("[Present Position] "); + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_data[index].goal_position = init_pose[0][index+1]; + dxl.ledOff(DYNAMIXEL_ID[index]); } - - arr_position_margin[idx] = init_pose[0][idx+1] - sr_data[idx].present_position; - - DEBUG_SERIAL.print("[Present Position_Setup] ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id);DEBUG_SERIAL.print(": ");DEBUG_SERIAL.println(sr_data[idx].present_position); - - DEBUG_SERIAL.print("[Goal Position - Present Position] ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id);DEBUG_SERIAL.print(": ");DEBUG_SERIAL.println(arr_position_margin[idx]); - - }DEBUG_SERIAL.println(""); - while(true){ + sync_write_information.is_info_changed = true; + dxl.syncWrite(&sync_write_information); + frame_time +=1; + DEBUG_SERIAL.println("=== [Play Motion] ==="); + break; + } - for (idx = 0; idx < axes; idx++){ // Count the number of ready DYNAMIXEL. - if (arr_dxl_ready_state[idx] == 1){ - dxl_ready_count++; - }else{ - dxl_ready_count = 0; - break; + if ( ElapsedTime() >= (unsigned long)(frame_time - 10)) { + for (index=0; index < NUMBER_OF_JOINT; index++) { + if (index == 0) { + DEBUG_SERIAL.println("[Moving to the init pose...]"); } - } - - if(dxl_ready_count == axes){ - - delay(1000); - - for (idx = 0; idx < axes; idx++){ - sw_data[idx].goal_position = init_pose[0][idx+1]; - dxl.ledOff(arr_dxl_id[idx]); - } - - sw_infos.is_info_changed = true; - - dxl.syncWrite(&sw_infos); - - g_frame_time +=1; - - DEBUG_SERIAL.println("========================[Play Motion]==========================="); - - break; - } - now = millis(); - - if (now - past >= time_trajectory-10){ - for (idx=0; idx < axes; idx++){ - - if (idx == 0){ - DEBUG_SERIAL.println("[Moving to the init pose...]"); - } - - if (arr_position_margin[idx] >= position_tolerance ){ // If result in Goal Position - Present Position is positive number, movement direction is in CCW. - DEBUG_SERIAL.print("DIR :CCW, "); - DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); - arr_position_margin[idx] = arr_position_margin[idx] - init_steps ; //set position every 20 ms - DEBUG_SERIAL.print("\t Target Pose: "); DEBUG_SERIAL.print(init_pose[0][idx+1]); - DEBUG_SERIAL.print("\t Present Pose: "); DEBUG_SERIAL.println(init_pose[0][idx+1] - arr_position_margin[idx]); - }else if (arr_position_margin[idx] <= -position_tolerance ){ // If result in Goal Position - Present Position is negative number, movement direction is in CW. - DEBUG_SERIAL.print("DIR : CW, "); - DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); - arr_position_margin[idx] = arr_position_margin[idx] + init_steps ; - DEBUG_SERIAL.print("\t Target Pose: "); DEBUG_SERIAL.print(init_pose[0][idx+1]); - DEBUG_SERIAL.print("\t Present Pose: "); DEBUG_SERIAL.println(init_pose[0][idx+1] - arr_position_margin[idx]); - }else{ // - arr_dxl_ready_state[idx] = 1; - dxl.ledOn(arr_dxl_id[idx]); - DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); - DEBUG_SERIAL.println(" is ready to go."); - } - - sw_data[idx].goal_position = init_pose[0][idx+1] - arr_position_margin[idx]; - - } + // If result in Goal Position - Present Position is positive number, movement direction is in CCW. + if (arr_position_margin[index] >= position_tolerance) { + DEBUG_SERIAL.print("DIR :CCW, "); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + //set position every 20 ms + arr_position_margin[index] = arr_position_margin[index] - init_steps ; + DEBUG_SERIAL.print("\t Target Pose: "); + DEBUG_SERIAL.print(init_pose[0][index+1]); + DEBUG_SERIAL.print("\t Present Pose: "); + DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); + } else if (arr_position_margin[index] <= -position_tolerance ) { + // If result in Goal Position - Present Position is negative number, movement direction is in CW. + DEBUG_SERIAL.print("DIR : CW, "); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + arr_position_margin[index] = arr_position_margin[index] + init_steps ; + DEBUG_SERIAL.print("\t Target Pose: "); + DEBUG_SERIAL.print(init_pose[0][index+1]); + DEBUG_SERIAL.print("\t Present Pose: "); + DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); + } else { + arr_dxl_ready_state[index] = 1; + dxl.ledOn(DYNAMIXEL_ID[index]); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.println(" is ready to go."); } - if (now - past >= time_trajectory){ - - sw_infos.is_info_changed = true; - - dxl.syncWrite(&sw_infos); - DEBUG_SERIAL.println(" SyncWrite."); - past = now; + sync_write_data[index].goal_position = init_pose[0][index+1] - arr_position_margin[index]; + } } - } -} - - -void setup() { - int profile_time = (arr_trajectory[2][0] - arr_trajectory[1][0])*2; // 0; - - InitDXL(profile_time); // Initialize the DYNAMIXEL - - MakeSrStruct(); - - MakeSwStruct(arr_trajectory); - - delay(2000); - - InitPose(arr_trajectory, profile_time); // Pass a pose, + if ( ElapsedTime() >= (unsigned long)(frame_time)) { + sync_write_information.is_info_changed = true; + dxl.syncWrite(&sync_write_information); + DEBUG_SERIAL.println(" SyncWrite."); + saved_time = current_time; + } + } } -void loop() { - - now = millis(); +void PlayMotion(const int MOTION_DATA[][MAX_MOTION_DATA_COLUMN_INDEX]) +{ + // Time difference between the saved_time and current time. + if ( ElapsedTime() >= (unsigned long)(MOTION_DATA[frame_time + 1][0] - MOTION_DATA[frame_time][0]) ) { + // DisplayTime(); + // ReadPresentPosition(); - PlayMotion(arr_trajectory); // Pass the trajectory information to a motion generator (PlayMotion). - -} + uint8_t index = 0; -void PlayMotion(const int arr[][g_total_row]){ - - int axes = g_total_row - 1; + DEBUG_SERIAL.print("[Goal Position] : "); + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_data[index].goal_position = MOTION_DATA[frame_time + 1][index + 1]; + DEBUG_SERIAL.print(sync_write_data[index].goal_position); + DEBUG_SERIAL.print("\t"); + } + DEBUG_SERIAL.println(""); - if(now - past >= arr[g_frame_time+1][0] - arr[g_frame_time][0]){ // Time difference between the past and current time. - -// DisplayTime(); -// ReadPresentPosition(); - - int idx; - - for (idx = 0; idx < axes; idx++) { + sync_write_information.is_info_changed = true; - if (idx == 0){ - DEBUG_SERIAL.print("[Goal Position]\t "); + if(!dxl.syncWrite(&sync_write_information)) { + DEBUG_SERIAL.println("[ERROR] Failed to Sync Write."); } - - DEBUG_SERIAL.print("ID");DEBUG_SERIAL.print(arr_dxl_id[idx]); - sw_data[idx].goal_position = arr[g_frame_time+1][idx+1]; - DEBUG_SERIAL.print(": ");DEBUG_SERIAL.print(sw_data[idx].goal_position);DEBUG_SERIAL.print(" "); - - }DEBUG_SERIAL.println(""); - - sw_infos.is_info_changed = true; - - dxl.syncWrite(&sw_infos); - - past = now; - - g_frame_time +=1; - - if (g_frame_time >= g_total_frame -1 ){ // Trajectory Point at 0 excluded. - - DEBUG_SERIAL.println("============================================================================="); - DEBUG_SERIAL.println("++++++++++++++++++++++++++ Motion Complete ++++++++++++++++++++++++++++++++++"); - DEBUG_SERIAL.print("Total Frames = ");DEBUG_SERIAL.println(g_frame_time + 1); - DEBUG_SERIAL.println("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"); - DEBUG_SERIAL.println("============================================================================="); - - g_frame_time = 1; - - loop_counts--; - - } - } -} -void DisplayTime(){ // Display current time, past time, and elapsed time in two trajectory points. If not needed, comment out. + saved_time = current_time; + frame_time += 1; - DEBUG_SERIAL.println("========================[Keyframe is ongoing]==========================="); - DEBUG_SERIAL.print("Now: ");DEBUG_SERIAL.print(now);DEBUG_SERIAL.print(" ms"); - DEBUG_SERIAL.print("\t Past: ");DEBUG_SERIAL.print(past);DEBUG_SERIAL.print(" ms"); - DEBUG_SERIAL.print("\t Elapsed Time:");DEBUG_SERIAL.print(now - past);DEBUG_SERIAL.println(" ms"); - DEBUG_SERIAL.print("[Trajectory Point] :");DEBUG_SERIAL.println(arr_trajectory[g_frame_time+1][0]); + // When reached at the end of the motion frame array + if (frame_time >= TOTAL_FRAMES -1 ) { + DEBUG_SERIAL.println("=== [Motion Play Completed] ==="); + DEBUG_SERIAL.print("Total Played Frames = "); + DEBUG_SERIAL.println(frame_time + 1); + frame_time = 1; + loop_counts--; + } + } } -void ReadPresentPosition(){ +void DisplayTime() +{ + // Display current time, saved_time time, and elapsed time in two trajectory points. If not needed, comment out. + DEBUG_SERIAL.println("=== [Print Time (ms)] ==="); + DEBUG_SERIAL.print("Current: "); + DEBUG_SERIAL.print(current_time); + DEBUG_SERIAL.print("\t Saved: "); + DEBUG_SERIAL.print(saved_time); + DEBUG_SERIAL.print("\t Elapsed:"); + DEBUG_SERIAL.print(current_time - saved_time); + DEBUG_SERIAL.print("\t Motion Frame:"); + DEBUG_SERIAL.println(MOTION_DATA[frame_time+1][0]); +} - int idx; +void ReadPresentPosition() +{ + uint8_t index = 0; - int axes = g_total_row - 1; - - for (idx = 0; idx < axes; idx++){ - - if (idx ==0){ - DEBUG_SERIAL.print("[Present Position] "); + if(dxl.syncRead(&sync_read_information) > 0) { + DEBUG_SERIAL.print("[Present Position] : "); + for (index = 0; index < NUMBER_OF_JOINT; index++) { + DEBUG_SERIAL.print(sync_read_data[index].present_position); + DEBUG_SERIAL.print("\t"); } - - DEBUG_SERIAL.print(" ID");DEBUG_SERIAL.print(sr_infos.p_xels[idx].id); - DEBUG_SERIAL.print(": ");DEBUG_SERIAL.print(sr_data[idx].present_position); - - }DEBUG_SERIAL.println(""); - - dxl.syncRead(&sr_infos); - + DEBUG_SERIAL.println(""); + } else { + DEBUG_SERIAL.println("[ERROR] Sync Read Failed"); + } } + +unsigned long ElapsedTime() +{ + current_time = millis(); + return (current_time - saved_time); +} \ No newline at end of file From 28b1f13f6bf6a26cb16938dc26f0c14f324a6c79 Mon Sep 17 00:00:00 2001 From: Will Son Date: Tue, 13 Jul 2021 15:24:07 +0900 Subject: [PATCH 04/10] apply delay for bootup --- examples/blender/openmanipulator_x/openmanipulator_x.ino | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/blender/openmanipulator_x/openmanipulator_x.ino b/examples/blender/openmanipulator_x/openmanipulator_x.ino index 672e131..48120fe 100644 --- a/examples/blender/openmanipulator_x/openmanipulator_x.ino +++ b/examples/blender/openmanipulator_x/openmanipulator_x.ino @@ -616,9 +616,12 @@ void setup() // Open serial port for debugging DEBUG_SERIAL.begin(115200); + while(!DEBUG_SERIAL); // Port Baudrate to communicate with connected DYNAMIXELs. If not matched, you will meet communication fail. dxl.begin(4000000); + + delay(100); // Allow time for DYNAMIXEL to finish boot up // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); @@ -644,9 +647,8 @@ void InitDXL(int frame_time) for (index = 0; index < NUMBER_OF_JOINT; index++) { if (!dxl.ping(DYNAMIXEL_ID[index])) { - DEBUG_SERIAL.print("[ERROR] DYNAMIXEL ID "); - DEBUG_SERIAL.print(index); - DEBUG_SERIAL.println(" is not detected."); + DEBUG_SERIAL.print("[ERROR] Failed to connect DYNAMIXEL "); + DEBUG_SERIAL.println(DYNAMIXEL_ID[index]); } else { dxl.torqueOff(DYNAMIXEL_ID[index]); // Set the Drive Mode as Time-based mode. From bd2e8f381af1e68dbc308affcaf3362f680bd9f9 Mon Sep 17 00:00:00 2001 From: Will Son Date: Thu, 29 Jul 2021 10:11:14 +0900 Subject: [PATCH 05/10] update todo #73 --- src/Dynamixel2Arduino.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 9396f7b..187d152 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -22,6 +22,8 @@ #include "utility/slave.h" #include "actuator.h" +// TODO: Match the OP Mode to the actual value +// https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/issues/73 enum OperatingMode{ OP_POSITION = 0, OP_EXTENDED_POSITION, @@ -443,6 +445,9 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master bool setFeedForwardGain(uint8_t id, uint16_t fisrt_gain, uint16_t second_gain); uint8_t getHardwareError(uint8_t id); + + // https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/issues/73 + uint8_t getOperatingMode(uint8_t id); #endif private: From 6086fb32e363798ec44290ee0d0657ee63ccbcd5 Mon Sep 17 00:00:00 2001 From: Will Son Date: Wed, 11 Aug 2021 16:59:07 +0900 Subject: [PATCH 06/10] update the example Signed-off-by: Will Son --- .../openmanipulator_x/openmanipulator_x.ino | 53 +++++++++++-------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/examples/blender/openmanipulator_x/openmanipulator_x.ino b/examples/blender/openmanipulator_x/openmanipulator_x.ino index 48120fe..1316034 100644 --- a/examples/blender/openmanipulator_x/openmanipulator_x.ino +++ b/examples/blender/openmanipulator_x/openmanipulator_x.ino @@ -1,18 +1,28 @@ -/******************************************************************************* -* Copyright 2016 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ +// Copyright 2021 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Example Environment +// +// - DYNAMIXEL: X series (except XL-320) +// ID = 11, 12, 13, 14, 15, Baudrate = 4000000bps, Protocol 2.0 +// - Controller: OpenCR 1.0 +// - Library: DYNAMIXEL2Arduino +// - Software: Blender 2.93.1 +// Arduino IDE +// - https://emanual.robotis.com/docs/en/parts/interface/mkr_shield/#examples +// +// Author: David Park #include @@ -66,7 +76,7 @@ typedef struct sync_read_data { } __attribute__((packed)) sync_read_data_t; // Sync Read Present Position -const uint16_t ADDRESS_TO_SYNC_READ = 132; +const uint16_t PRESENT_POSITION_ADDRESS = 132; const uint16_t LENGTH_TO_SYNC_READ = 4; // Sync Read @@ -81,7 +91,7 @@ typedef struct sync_write_data { } __attribute__((packed)) sync_write_data_t; // Sync Write Goal Position -const uint16_t ADDRESS_TO_SYNC_WRITE = 116; +const uint16_t GOAL_POSITION_ADDRESS = 116; const uint16_t LENGTH_TO_SYNC_WRITE = 4; // Sync Write @@ -642,6 +652,7 @@ void loop() void InitDXL(int frame_time) { uint8_t index = 0; + dxl.torqueOff(DXL_BROADCAST_ID); // Ping DYNAMIXELs. If failed, check your Baudrate and pysical wiring connection. for (index = 0; index < NUMBER_OF_JOINT; index++) @@ -650,17 +661,17 @@ void InitDXL(int frame_time) DEBUG_SERIAL.print("[ERROR] Failed to connect DYNAMIXEL "); DEBUG_SERIAL.println(DYNAMIXEL_ID[index]); } else { - dxl.torqueOff(DYNAMIXEL_ID[index]); // Set the Drive Mode as Time-based mode. dxl.writeControlTableItem(DRIVE_MODE, DYNAMIXEL_ID[index], 4); // Lower Return Delay Time enhances the responsiveness. dxl.writeControlTableItem(RETURN_DELAY_TIME, DYNAMIXEL_ID[index], 0); dxl.setOperatingMode(DYNAMIXEL_ID[index], OP_POSITION); - dxl.torqueOn(DYNAMIXEL_ID[index]); // Time-based should be larger than frame time. dxl.writeControlTableItem(PROFILE_VELOCITY, DYNAMIXEL_ID[index], frame_time); } } + dxl.torqueOn(DXL_BROADCAST_ID); + delay(100); } // Initialize SyncRead packet in the structure @@ -671,7 +682,7 @@ void InitSyncRead() sync_read_information.packet.p_buf = packet_buffer; sync_read_information.packet.buf_capacity = MAX_PACKET_BUFFER_LENGTH; sync_read_information.packet.is_completed = false; - sync_read_information.addr = ADDRESS_TO_SYNC_READ; + sync_read_information.addr = PRESENT_POSITION_ADDRESS; sync_read_information.addr_length = LENGTH_TO_SYNC_READ; sync_read_information.p_xels = sync_read_dynamixel_info; sync_read_information.xel_count = 0; @@ -693,7 +704,7 @@ void InitSyncWrite() sync_write_information.packet.p_buf = nullptr; sync_write_information.packet.is_completed = false; - sync_write_information.addr = ADDRESS_TO_SYNC_WRITE; + sync_write_information.addr = GOAL_POSITION_ADDRESS; sync_write_information.addr_length = LENGTH_TO_SYNC_WRITE; sync_write_information.p_xels = sync_write_dynamixel_info; sync_write_information.xel_count = 0; From c0ab4b2143a5ede404991722fa02db7cd7732037 Mon Sep 17 00:00:00 2001 From: ROBOTIS-Will Date: Wed, 11 Aug 2021 17:51:55 +0900 Subject: [PATCH 07/10] skip mega&uno --- examples/blender/openmanipulator_x/.mega2560.test.skip | 0 examples/blender/openmanipulator_x/.uno.test.skip | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 examples/blender/openmanipulator_x/.mega2560.test.skip create mode 100644 examples/blender/openmanipulator_x/.uno.test.skip diff --git a/examples/blender/openmanipulator_x/.mega2560.test.skip b/examples/blender/openmanipulator_x/.mega2560.test.skip new file mode 100644 index 0000000..e69de29 diff --git a/examples/blender/openmanipulator_x/.uno.test.skip b/examples/blender/openmanipulator_x/.uno.test.skip new file mode 100644 index 0000000..e69de29 From 61864bd1b5064554907f38e5a3d2bbc027f88376 Mon Sep 17 00:00:00 2001 From: Will Son Date: Thu, 12 Aug 2021 09:56:14 +0900 Subject: [PATCH 08/10] add new example, fix #73 Signed-off-by: Will Son --- src/Dynamixel2Arduino.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 187d152..29a6b52 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -25,14 +25,12 @@ // TODO: Match the OP Mode to the actual value // https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/issues/73 enum OperatingMode{ - OP_POSITION = 0, - OP_EXTENDED_POSITION, - OP_CURRENT_BASED_POSITION, - OP_VELOCITY, - OP_PWM, - OP_CURRENT, - - UNKNOWN_OP + OP_CURRENT = 0, + OP_VELOCITY = 1, + OP_POSITION = 3, + OP_EXTENDED_POSITION = 4, + OP_CURRENT_BASED_POSITION = 5, + OP_PWM = 16, }; enum ParamUnit{ From 39d02546af34992a0ecc4f0be3ee5b85b41f9b8d Mon Sep 17 00:00:00 2001 From: Will Son Date: Thu, 12 Aug 2021 09:57:42 +0900 Subject: [PATCH 09/10] prepare release 0.4.9 Signed-off-by: Will Son --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index e8384b3..2536911 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Dynamixel2Arduino -version=0.4.8 +version=0.4.9 author=ROBOTIS license=Apache-2.0 maintainer=Will Son(willson@robotis.com) From c4125a3cd115040cca51f11b3c3a74d0f622fcf0 Mon Sep 17 00:00:00 2001 From: Will Son Date: Thu, 12 Aug 2021 09:59:09 +0900 Subject: [PATCH 10/10] remove wrong info Signed-off-by: Will Son --- examples/blender/openmanipulator_x/openmanipulator_x.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/blender/openmanipulator_x/openmanipulator_x.ino b/examples/blender/openmanipulator_x/openmanipulator_x.ino index 1316034..cd009c6 100644 --- a/examples/blender/openmanipulator_x/openmanipulator_x.ino +++ b/examples/blender/openmanipulator_x/openmanipulator_x.ino @@ -20,7 +20,6 @@ // - Library: DYNAMIXEL2Arduino // - Software: Blender 2.93.1 // Arduino IDE -// - https://emanual.robotis.com/docs/en/parts/interface/mkr_shield/#examples // // Author: David Park