Untitled

#include <Dynamixel2Arduino.h> //подключение библиотеки Dynamixel 
 
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (To use the DXL port on the OpenCM 9.04 board, you must use Serial1 for Serial. And because of the OpenCM 9.04 driver code, you must call Serial1.setDxlMode(true); before dxl.begin();.) 
#define DEBUG_SERIAL Serial //Установка константы, отвечающей за последовательный порт, подключаемый к компьютеру 
const uint8_t DXL_DIR_PIN = 22; //инициализация переменной, отвечащей за номер пина, подключенного к информационному пину приводов манипулятора 
const float DXL_PROTOCOL_VERSION = 1.0; //инициализация переменной, отвечащей за протокол передачи данных от OpenCM9.04 к приводам 
 
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); //инициализация указателя на команды из библиотеки Dynamixel 
 
 
#define jointN 6 //задание константы, определяющей количество приводов 
int n = 1; 
int delta = 4; 
int fi_readpos[jointN+1] = {0, 0, 0, 0, 0, 0, 0}; 
 
void setup() { //структура, команды внутри которой выполняются единожды 
 
DEBUG_SERIAL.begin(57600); //установка скорости обмена данными по последовательному порту компьютера 
dxl.begin(1000000); //установка скорости обмена данными по последовательному порту манипулятора 
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); //выбор протокола обмена данными 
 
for(n=1; n<=jointN; n++) { //цикл с изменением i от 1 до 6 с шагом 1 (для перебора всех приводов 
 dxl.setOperatingMode(n, OP_POSITION); //установка режима работы привода с номером 1 в качестве шарнира 
 fi_readpos[n]=dxl.getPresentPosition(n); 
} 
} 
 
void loop() { 
 
 int mas[20][7]= { 
 {0, 512, 358, 901, 584, 512, 512}, 
 {0, 512, 384, 845, 613, 512, 512}, 
 {0, 512, 402, 822, 619, 512, 512}, 
 {0, 512, 417, 806, 620, 512, 512}, 
 {0, 512, 431, 793, 619, 512, 512}, 
 {0, 512, 443, 784, 616, 512, 512}, 
 {0, 512, 455, 778, 611, 512, 512}, 
 {0, 512, 466, 775, 603, 512, 512}, 
 {0, 512, 475, 776, 592, 512, 512}, 
 {0, 512, 484, 784, 575, 512, 512}, 
 {0, 512, 492, 823, 528, 512, 512}, 
 {0, 512, 496, 878, 613, 512, 512}, 
 {0, 512, 505, 902, 619, 512, 512}, 
 {0, 512, 508, 921, 620, 512, 512}, 
 {0, 512, 506, 935, 619, 512, 512}, 
 {0, 512, 499, 947, 616, 512, 512}, 
 {0, 512, 487, 954, 611, 512, 512}, 
 {0, 512, 472, 958, 603, 512, 512}, 
 {0, 512, 452, 956, 592, 512, 512}, 
 {0, 512, 428, 946, 575, 512, 512}, 
 }; 
 
for (int n = 0; n < 20; n++){ 
 for (int m = 1; m <= 7; m++){ 
 dxl.setGoalVelocity(m, 50); 
 dxl.setGoalPosition(m, mas[n][m]); 
 fi_readpos[m] = dxl.getPresentPosition(m); 
 } 
 Serial.print("["); 
 for(int i=0; i<7; i++) 
 { 
 Serial.print(fi_readpos[i]); 
 Serial.print(", "); 
 } 
 Serial.print("]"); 
 Serial.println(); 
 while ( 
 ( fi_readpos[2] <= mas[n][2]-delta)or(fi_readpos[2] >= mas[n][2]+delta) and 
 ( fi_readpos[3] <= mas[n][3]-delta)or(fi_readpos[3] >= mas[n][3]+delta) and 
 ( fi_readpos[4] <= mas[n][4]-delta)or(fi_readpos[4] >= mas[n][4]+delta)) 
 { 
 delay(100); 
 Serial.print("["); 
 for(int i=0; i<7; i++) 
 { 
 Serial.print(fi_readpos[i]); 
 Serial.print(", "); 
 } 
 Serial.print("]"); 
 Serial.println(); 
 
 Serial.print("ideal["); 
 for(int i=0; i<7; i++) 
 { 
 Serial.print(mas[n][i]); 
 Serial.print(", "); 
 } 
 Serial.print("]"); 
 Serial.println(); 
 for (int m = 1; m <= 7; m++) 
 { 
 fi_readpos[m] = dxl.getPresentPosition(m); 
 } 
 } 
 } 
}