mx106을 openRB-150에 연결하여 아두이노로 코드를 작성하고 있습니다.
시리얼 통신으로 특정 신호를 보내면 지정된 각도로 이동하게 하려고 코드를 작성하고 있습니다.
아두이노의 write_ax_mx 예제를 활용해서 작성하는데 goalposition을 3개 이상 지정해 줘도 상관이 없나요?
아두이노 코드로 실행할때마다 setup 위치값이 이상하게 변경되거나 문제가 있어서 다이나 믹셀 위자드로 이동하면 모터의 cw, ccw limit값이 제가 원하지 않는 방향으로 이동하는데 어떻게 해야 할까요?
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = 1;
#define CW_ANGLE_LIMIT_ADDR 6 //시계 방향 한계 각도 주소
#define CCW_ANGLE_LIMIT_ADDR 8 //반시계 방향 한계 각도 주소
#define ANGLE_LIMIT_ADDR_LEN 2 //
#define OPERATING_MODE_ADDR_LEN 2 //
#define TORQUE_ENABLE_ADDR 24 //토크 주소
#define TORQUE_ENABLE_ADDR_LEN 1 //토크 주소 길이
#define LED_ADDR 25 //led 주소
#define LED_ADDR_LEN 1 //led 주소 길이
#define GOAL_POSITION_ADDR 30 //목표 위치 주소
#define GOAL_POSITION_ADDR_LEN 2 //목표 위치 주소 길이
#define PRESENT_POSITION_ADDR 36 //현재 위치 주소
#define PRESENT_POSITION_ADDR_LEN 2 //현재 위치 주소 길이
#define TIMEOUT 10
const uint8_t DXL_ID_1 = 1, DXL_ID_2 = 2, DXL_ID_3 = 3;
const float DXL_PROTOCOL_VERSION = 1.0;
uint16_t goalPosition1 = 2048;
uint16_t goalPosition2 = 1024;//1536;
uint16_t goalPosition3 = 512;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
using namespace ControlTableItem;
char var;
void setup()
{
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
dxl.setGoalPosition(DXL_ID_1, 2048);
dxl.setGoalVelocity(DXL_ID_1, 50);
dxl.set_Angle_Limit
dxl.setGoalPosition(DXL_ID_2, 2560);
dxl.setGoalVelocity(DXL_ID_2, 30);
dxl.setGoalPosition(DXL_ID_3, 2560);
dxl.setGoalVelocity(DXL_ID_3, 30);
// Set to Joint Mode
if(dxl.write(DXL_ID_1, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)
&& dxl.write(DXL_ID_1, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Set operating mode");
else
DEBUG_SERIAL.println("Error: Set operating mode failed");
if(dxl.write(DXL_ID_2, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)
&& dxl.write(DXL_ID_2, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Set operating mode");
else
DEBUG_SERIAL.println("Error: Set operating mode failed");
// Get DYNAMIXEL information
// dxl.ping(DXL_ID_1, DXL_ID_2, DXL_ID_3);
}
void loop()
{
var = Serial.read();
if(Serial.available())
{
if(var == 'q')
{
DEBUG_SERIAL.print(dxl.getPresentPosition(1));
DEBUG_SERIAL.print("ID1 Goal Position : ");
DEBUG_SERIAL.println(goalPosition1);
dxl.write(DXL_ID_1, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 'w')
{
DEBUG_SERIAL.print("ID1 Goal Position : ");
DEBUG_SERIAL.println(goalPosition2);
dxl.write(DXL_ID_1, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 'e')
{
DEBUG_SERIAL.print("ID1 Goal Position : ");
DEBUG_SERIAL.println(goalPosition3);
dxl.write(DXL_ID_1, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition3, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 'a')
{
DEBUG_SERIAL.print("ID2 Goal Position : ");
DEBUG_SERIAL.println(goalPosition1);
dxl.write(DXL_ID_2, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 's')
{
DEBUG_SERIAL.print("ID2 Goal Position : ");
DEBUG_SERIAL.println(goalPosition2);
dxl.write(DXL_ID_2, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 'z')
{
DEBUG_SERIAL.print("ID3 Goal Position : ");
DEBUG_SERIAL.println(goalPosition1);
dxl.write(DXL_ID_3, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else if(var == 'x')
{
DEBUG_SERIAL.print("ID3 Goal Position : ");
DEBUG_SERIAL.println(goalPosition2);
dxl.write(DXL_ID_3, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(1000);
}
else{
DEBUG_SERIAL.println(var);
}
}
}