XM540-W270-T 3개와 2XL430-W250-T 1개를 사용하여 아두이노에서 위치 제어를 하려고 합니다.
XM540-W270-T를 id 1,2,3으로 설정하였고 2XL430-W250-T를 id 4번으로 설정하였습니다.
아두이노 코드에서 수식을 입력한 후 시리얼 창에서 확인하면 값이 변함을 확인할 수 있는데 다이나믹셀 id 3번이 동작하지 않습니다. 다른 코드로 다이나믹셀을 제어하면 동작이 되어 회로나 모터 자체의 문제는 아닌듯 합니다. 어떻게 해결해야 하나요?
#include <Dynamixel2Arduino.h>
#include <math.h>
#define DXL_SERIAL Serial
#define DEBUG_SERIAL Serial3
const int DXL_DIR_PIN = 2;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
using namespace ControlTableItem;
const float DXL_PROTOCOL_VERSION = 2.0;
const uint8_t DXL_ID_1 = 1;
const uint8_t DXL_ID_2 = 2;
const uint8_t DXL_ID_3 = 3;
const uint8_t DXL_ID_4 = 4;
// 링크 길이 (단위: cm)
const float L1 = 23;
const float L2 = 33;
const float L3 = 30;
const float L4 = 12.15;
const float base_distance = 16.9;
const float blank_length = 1.65;
const float board_height = 1.3;
const float square_length = 2.05;
float arr[4] = {0};
void setup() {
DEBUG_SERIAL.begin(115200);
while (!Serial);
dxl.begin(57600);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID_3, 0);
dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID_3, 4095);
dxl.torqueOff(DXL_ID_1);
dxl.torqueOff(DXL_ID_2);
dxl.torqueOff(DXL_ID_3);
dxl.torqueOff(DXL_ID_4);
dxl.setOperatingMode(DXL_ID_1, OP_POSITION);
dxl.setOperatingMode(DXL_ID_2, OP_POSITION);
dxl.setOperatingMode(DXL_ID_3, OP_POSITION);
dxl.setOperatingMode(DXL_ID_4, OP_POSITION);
// 속도 제한
const int VELOCITY_UNIT = 200;
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_1, VELOCITY_UNIT);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_2, VELOCITY_UNIT);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_3, VELOCITY_UNIT);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_4, VELOCITY_UNIT);
dxl.torqueOn(DXL_ID_1);
dxl.torqueOn(DXL_ID_2);
dxl.torqueOn(DXL_ID_3);
dxl.torqueOn(DXL_ID_4);
float x_init = 0;
float y_init = 2.05 * 10 + base_distance;
float z_init = 30;
// 역기구학
float d = sqrt(x_init * x_init + y_init * y_init + pow(z_init - L1, 2));
float D = (d * d - L2 * L2 - L3 * L3) / (2 * L2 * L3);
D = constrain(D, -1.0, 1.0);
float theta3 = atan2(-sqrt(1 - D * D), D);
float alpha = atan2(z_init - L1, sqrt(x_init * x_init + y_init * y_init));
float E = (L2 * L2 + d * d - L3 * L3) / (2 * L2 * d);
E = constrain(E, -1.0, 1.0);
float beta = atan2(sqrt(1 - E * E), E);
float theta2 = alpha + beta;
float theta1 = atan2(y_init, x_init);
float theta4 = radians(180) + theta3 - theta2;
// 라디안을 도로 변환
arr[0] = degrees(theta1);
arr[1] = degrees(theta2);
arr[2] = degrees(theta3);
arr[3] = degrees(theta4);
dxl.setGoalPosition(DXL_ID_1, arr[0], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_2, arr[1], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_3, arr[2], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_4, arr[3], UNIT_DEGREE);
DEBUG_SERIAL.println(“x y z를 입력하세요. 띄어쓰기 필수 (단위: cm), (z=1.3으로 입력)”);
}
void loop() {
if (DEBUG_SERIAL.available()) {
String input = DEBUG_SERIAL.readStringUntil(‘\n’);
input.trim();
// x,y,z 값 추출
float x, y, z;
int s1 = input.indexOf(’ ‘);
int s2 = input.indexOf(’ ', s1 + 1);
if (s1 == -1 || s2 == -1) {
DEBUG_SERIAL.println(“형식 오류”);
return;
}
x = input.substring(0, s1).toFloat();
y = input.substring(s1 + 1, s2).toFloat();
z = input.substring(s2 + 1).toFloat();
float x_p = square_length * (x - 10);
float y_p = blank_length + base_distance + square_length * (19 - y);
float z_p = z;
// 역기구학
float d = sqrt(x_p * x_p + y_p * y_p + pow(z_p - L1, 2));
float D = (d * d - L2 * L2 - L3 * L3) / (2 * L2 * L3);
D = constrain(D, -1.0, 1.0);
float theta3 = atan2(-sqrt(1 - D * D), D);
float alpha = atan2(z - L1, sqrt(x_p * x_p + y_p * y_p ));
float E = (L2 * L2 + d * d - L3 * L3) / (2 * L2 * d);
E = constrain(E, -1.0, 1.0);
float beta = atan2(sqrt(1 - E * E), E);
float theta2 = alpha + beta;
float theta1 = atan2(y_p , x_p);
//좌표에 따른 theat4 값
float theta4;
float dx = square_length * fabs(x - 10);
float dy = square_length * fabs(y - 19) + blank_length;
if (x==10)
{
theta4 = PI / 2.0;
}
else if (x<10)
{
theta4 = PI / 2.0 + atan2(dx,dy);
}
else
{
theta4 = PI / 2.0 - atan2(dx,dy);
}
// 라디안을 도로 변환
arr[0] = degrees(theta1);
arr[1] = degrees(theta2);
arr[2] = degrees(theta3);
arr[3] = degrees(theta4);
// 결과 출력
DEBUG_SERIAL.print("theta_1: "); DEBUG_SERIAL.println(arr[0]);
DEBUG_SERIAL.print("theta_2: "); DEBUG_SERIAL.println(arr[1]);
DEBUG_SERIAL.print("theta_3: "); DEBUG_SERIAL.println(arr[2]);
DEBUG_SERIAL.print("theta_4: "); DEBUG_SERIAL.println(arr[3]);
// 다이나믹셀 제어
dxl.setGoalPosition(DXL_ID_1, arr[0], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_2, arr[1], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_3, arr[2], UNIT_DEGREE);
dxl.setGoalPosition(DXL_ID_4, arr[3], UNIT_DEGREE);
}
}