real_final_arm_cpp.txt

#if defined(__linux__) || defined(__APPLE__)
#include <fcntl.h>
#include <termios.h>
#define STDIN_FILENO 0
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif

#pragma comment(lib, "ws2_32.lib")
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <cstdlib>
#include <string>
#include <winsock2.h>
#include <Windows.h>
#include <cstring>
#include "dynamixel_sdk.h"
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#define PORT 10000
#define PI 3.1415

using namespace std;

// Uses Dynamixel SDK library

////////////////////////////////////////////
// Control table address
////////////////////////////////////////////

// Control table address for Dynamixel MX
#define ADDR_MX_TORQUE_ENABLE           24                  // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION           30
#define ADDR_MX_PRESENT_POSITION        36
#define ADDR_MX_GOAL_ACCELERATION       73
#define ADDR_MX_MOVING_SPEED            32

//  Control table address for Dynamixel XM430
#define ADDR_XM430_TORQUE_ENABLE          64
#define ADDR_XM430_GOAL_POSITION          116
#define ADDR_XM430_PRESENT_POSITION       132
#define ADDR_XM430_PROFILE_ACCELERATION   108
#define ADDR_XM430_PROFILE_VELOCITY       112


////////////////////////////////////////////
// 1. Acceleration, Velocity
////////////////////////////////////////////

#define DXL1_ACCELERATION  200
#define DXL2_ACCELERATION  200
#define DXL3_ACCELERATION  200
#define DXL4_ACCELERATION  2000





////////////////////////////////////////////
// Protocol version
////////////////////////////////////////////

#define PROTOCOL_VERSION1               1.0                 // See which protocol version is used in the Dynamixel
#define PROTOCOL_VERSION2               2.0

////////////////////////////////////////////
// Default setting 
////////////////////////////////////////////

#define DXL1_ID                         12                 // Dynamixel#1 ID: 12
#define DXL2_ID                         14                 // Dynamixel#2 ID: 14
#define DXL3_ID                         3                   // Dynamixel#3 ID: 3
#define DXL4_ID                         13                 // Dynamixel#4 ID: 13
#define BAUDRATE                        57600
#define DEVICENAME                      "COM4"      // Check which port is being used on your controller
															// ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
#define DXL1_MINIMUM_POSITION_VALUE     0                 
#define DXL1_MAXIMUM_POSITION_VALUE     4095                
#define DXL2_MINIMUM_POSITION_VALUE     0
#define DXL2_MAXIMUM_POSITION_VALUE     4095
#define DXL3_MINIMUM_POSITION_VALUE     0
#define DXL3_MAXIMUM_POSITION_VALUE     4095
#define DXL4_MINIMUM_POSITION_VALUE     0
#define DXL4_MAXIMUM_POSITION_VALUE     4095

#define DXL1_MOVING_STATUS_THRESHOLD    10                  // Dynamixel MX moving status threshold
#define DXL2_MOVING_STATUS_THRESHOLD    10                  // Dynamixel XM430 moving status threshold
#define DXL3_MOVING_STATUS_THRESHOLD    10                  // Dynamixel XM430 moving status threshold
#define DXL4_MOVING_STATUS_THRESHOLD    10                  // Dynamixel XM430 moving status threshold

#define ESC_ASCII_VALUE                 0x1b


void ShowErrorMessage(string message)
{
	cout << "Error: " << message << '\n';
	system("pause");
	exit(1);
}

int getch()
{
#if defined(__linux__) || defined(__APPLE__)
	struct termios oldt, newt;
	int ch;
	tcgetattr(STDIN_FILENO, &oldt);
	newt = oldt;
	newt.c_lflag &= ~(ICANON | ECHO);
	tcsetattr(STDIN_FILENO, TCSANOW, &newt);
	ch = getchar();
	tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
	return ch;
#elif defined(_WIN32) || defined(_WIN64)
	return _getch();
#endif
}

int kbhit(void)
{
#if defined(__linux__) || defined(__APPLE__)
	struct termios oldt, newt;
	int ch;
	int oldf;

	tcgetattr(STDIN_FILENO, &oldt);
	newt = oldt;
	newt.c_lflag &= ~(ICANON | ECHO);
	tcsetattr(STDIN_FILENO, TCSANOW, &newt);
	oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
	fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

	ch = getchar();

	tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
	fcntl(STDIN_FILENO, F_SETFL, oldf);

	if (ch != EOF)
	{
		ungetc(ch, stdin);
		return 1;
	}

	return 0;
#elif defined(_WIN32) || defined(_WIN64)
	return _kbhit();
#endif
}


void dxl_actuator_func(void);
void read_angle(void);
void enable_torque(void);
void write_acceleration(void);
void write_velocity(void);
void disable_torque(void);
void motor_steer(void);

// Initialize PortHandler instance
	// Set the port path
	// Get methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler* portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler* packetHandler1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION2);
dynamixel::PacketHandler* packetHandler2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION2);
dynamixel::PacketHandler* packetHandler3 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION1);
dynamixel::PacketHandler* packetHandler4 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION2);

int dxl_comm_result = COMM_TX_FAIL;       // Communication result

//index
int index = 0;

////////////////////////////////////////////
//2. to set the control position value
////////////////////////////////////////////

//front position and back position

int dxl_1_front_pos_deg = 180;  //id 12
int dxl_1_back_pos_deg = 180;
int dxl_2_front_pos_deg = 210;  //id 14
int dxl_2_back_pos_deg = 246;
int dxl_3_front_pos_deg = 140; //id 3 (mx)
int dxl_3_back_pos_deg = 108;
int dxl_4_front_pos_deg = 205; //id  13
int dxl_4_back_pos_deg = 185;

int dxl_1_front_pos = int(dxl_1_front_pos_deg * 11.38);
int dxl_1_back_pos = int(dxl_1_back_pos_deg * 11.38);
int dxl_2_front_pos = int(dxl_2_front_pos_deg * 11.38);
int dxl_2_back_pos = int(dxl_2_back_pos_deg * 11.38);
int dxl_3_front_pos = int(dxl_3_front_pos_deg * 11.38);
int dxl_3_back_pos = int(dxl_3_back_pos_deg * 11.38);
int dxl_4_front_pos = int(dxl_4_front_pos_deg * 11.38);
int dxl_4_back_pos = int(dxl_4_back_pos_deg * 11.38);



////////////////////////////////////////////
//Goal position allocating
////////////////////////////////////////////

int dxl1_goal_position[2] = { dxl_1_back_pos, dxl_1_front_pos };     // Goal position of Dynamixel 1
int dxl2_goal_position[2] = { dxl_2_back_pos, dxl_2_front_pos };     // Goal position of Dynamixel 2
int dxl3_goal_position[2] = { dxl_3_back_pos, dxl_3_front_pos };     // Goal position of Dynamixel 3
int dxl4_goal_position[2] = { dxl_4_back_pos, dxl_4_front_pos };     // Goal position of Dynamixel 4


////////////////////////////////////////////
//VELOCITY
////////////////////////////////////////////

int DXL1_VELOCITY[2] = { 250,250 };
int DXL2_VELOCITY[2] = { 100,200 };
int DXL3_MOVING_SPEED[2] = { 100,400 };
int DXL4_VELOCITY[2] = { 100, 200 };




uint8_t dxl_error = 0;                    // Dynamixel error
int32_t dxl1_present_position = 0;       // Present position of Dynamixel 1
int32_t dxl2_present_position = 0;        // Present position of Dynamixel 2
uint16_t dxl3_present_position = 0;        // Present position of Dynamixel 3
int32_t dxl4_present_position = 0;        // Present position of Dynamixel 4



int main()
{
	// Initiate communication at first.
	WSADATA wsaData;
	SOCKET clientSocket;
	SOCKADDR_IN serverAddress;
	char received[256], * seg;
	int recv_arr, ang_deg;
	double recv_ang, abs_recv_ang;
	string sent;
	if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) // Winsock   ʱ ȭ մϴ .
		ShowErrorMessage("WSAStartup()");
	clientSocket = socket(PF_INET, SOCK_STREAM, 0); // TCP            մϴ .
	if (clientSocket == INVALID_SOCKET)
		ShowErrorMessage("socket()");
	memset(&serverAddress, 0, sizeof(serverAddress));
	serverAddress.sin_family = AF_INET;
	serverAddress.sin_addr.s_addr = inet_addr("127.0.0.1"); //    ڿ IP    Ʈ  ũ    Ʈ        
	serverAddress.sin_port = htons(PORT); // 2    Ʈ        Ʈ  ũ    Ʈ        

	if (connect(clientSocket, (SOCKADDR*)&serverAddress, sizeof(serverAddress)) == SOCKET_ERROR)
		ShowErrorMessage("connect()");
	cout << "[Status] connect()\n";
	send(clientSocket, "r", 1, 0);

	// Open port
	if (portHandler->openPort())
	{
		printf("Succeeded to open the port!\n");
	}
	else
	{
		printf("Failed to open the port!\n");
		printf("Press any key to terminate...\n");
		getch();
		return 0;
	}

	// Set port baudrate
	if (portHandler->setBaudRate(BAUDRATE))
	{
		printf("Succeeded to change the baudrate!\n");
	}
	else
	{
		printf("Failed to change the baudrate!\n");
		printf("Press any key to terminate...\n");
		getch();
		return 0;
	}

	////////////////////////////////////////////
	// Enable Torque
	////////////////////////////////////////////

	enable_torque();
	index = 0;
	write_acceleration();
	write_velocity();
	dxl_actuator_func();
	motor_steer();

	////////////////////////////////////////////
	//3. repeat while true --> input              ۵ 
	////////////////////////////////////////////
	while (1) {

		printf("waiting for input froim python server..\n");
		//     κ          Է¹޴´ .
		int length = recv(clientSocket, received, sizeof(received), 0);
		if (!length || length >> 6) continue;
		received[length] = '\0';
		cout << "Message: " << received << '\n';
		seg = strtok(received, ",");
		if (seg == NULL)    continue;
		//recv_arr, recv_ang: two input

		recv_arr = atoi(seg), seg = strtok(NULL, ","); //integer
		if (seg == NULL)    continue;
		recv_ang = atof(seg), seg = strtok(NULL, ","); //double
		cout << "arrival time: " << recv_arr << " angle: " << recv_ang << endl;
		// Angular distance, ang_deg  == goal_posiion of motor No.12

		if (recv_ang >= 0)
		{
			if (recv_ang < 1)	ang_deg = 1536;
			else if (recv_ang < 20)	ang_deg = 1024 + floor(atan(recv_ang) * 2048 / PI);
			else
			{
				ang_deg = 2048;
			}
		}
		else
		{
			if (recv_ang > -1)	ang_deg = 2560;
			else if (recv_ang > -20)	ang_deg = 3092 + floor(atan(recv_ang) * 2048 / PI);
			else
			{
				ang_deg = 2048;
			}
		}


		dxl1_goal_position[1] = ang_deg;

		bool input;
		if (recv_arr > 0)
		{
			index = 1;
			write_velocity();
			motor_steer();
			//Zzzzzzzzzzzzz
			Sleep(recv_arr - 50);
			//Zzzzzzzzzzzzz
			input = 1;
		}
		else {

			index = 0;
			input = 0;
		}


		if (input == 0) {
			////////////////////////////////////////////
			//Write reference position for motors
			////////////////////////////////////////////
			index = 0;
			write_velocity();
			dxl_actuator_func();
			motor_steer();
		}
		else {
			for (int i = 0; i < 2; i++) {
				////////////////////////////////////////////
				//Write front and back position for motors
				////////////////////////////////////////////
				if (i == 0) {
					index = 1;
				}
				else {
					index = 0;
				}

				dxl_actuator_func();
				motor_steer();

				do {
					read_angle();
				} while ((abs(dxl1_goal_position[index] - dxl1_present_position) > DXL1_MOVING_STATUS_THRESHOLD) || (abs(dxl2_goal_position[index] - dxl2_present_position) > DXL2_MOVING_STATUS_THRESHOLD+20)
					|| (abs(dxl3_goal_position[index] - dxl3_present_position) > DXL3_MOVING_STATUS_THRESHOLD) || (abs(dxl4_goal_position[index] - dxl4_present_position) > DXL4_MOVING_STATUS_THRESHOLD));

			}
		}
	}

	////////////////////////////////////////////
	//loop end
	////////////////////////////////////////////

	void disable_torque(void);

	// Close port
	portHandler->closePort();

	// Terminate Communication.
	closesocket(clientSocket);
	WSACleanup();
	system("pause");
	return 0;
}


void dxl_actuator_func(void) {

	// Write Dynamixel#2 goal position
	dxl_comm_result = packetHandler2->write4ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_GOAL_POSITION, dxl2_goal_position[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}

	// Write Dynamixel#3 goal position
	dxl_comm_result = packetHandler3->write2ByteTxRx(portHandler, DXL3_ID, ADDR_MX_GOAL_POSITION, dxl3_goal_position[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}

	// Write Dynamixel#4 goal position
	dxl_comm_result = packetHandler4->write4ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_GOAL_POSITION, dxl4_goal_position[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
}

void motor_steer(void) {
	// Write Dynamixel#1 goal position
	dxl_comm_result = packetHandler1->write4ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_GOAL_POSITION, dxl1_goal_position[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}
}
void read_angle(void) {
	// Read Dynamixel#1 present position
	dxl_comm_result = packetHandler1->read4ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_PRESENT_POSITION, (uint32_t*)&dxl1_present_position, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}

	// Read Dynamixel#2 present position
	dxl_comm_result = packetHandler2->read4ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_PRESENT_POSITION, (uint32_t*)&dxl2_present_position, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}

	// Read Dynamixel#3 present position
	dxl_comm_result = packetHandler3->read2ByteTxRx(portHandler, DXL3_ID, ADDR_MX_PRESENT_POSITION, &dxl3_present_position, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}

	// Read Dynamixel#4 present position
	dxl_comm_result = packetHandler4->read4ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_PRESENT_POSITION, (uint32_t*)&dxl4_present_position, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
	printf("[ID:%03d] GoalPos:%03d  PresPos:%03d [ID:%03d] GoalPos:%03d\nPresPos:%03d [ID:%03d] GoalPos:%03d  PresPos:%03d [ID:%03d] GoalPos:%03d  PresPos:%03d\n",
		DXL1_ID, dxl1_goal_position[index], dxl1_present_position, DXL2_ID, dxl2_goal_position[index], dxl2_present_position, DXL3_ID, dxl3_goal_position[index], dxl3_present_position, DXL4_ID, dxl4_goal_position[index], dxl4_present_position);

}

void enable_torque(void) {
	////////////////////////////////////////////
		// Enable Torque (no need to edit)
		////////////////////////////////////////////

		// Enable Dynamixel#1 torque
	dxl_comm_result = packetHandler1->write1ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}
	else {
		printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
	}

	// Enable Dynamixel#2 torque
	dxl_comm_result = packetHandler2->write1ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}
	else {
		printf("Dynamixel#%d has been successfully connected \n", DXL2_ID);
	}

	// Enable Dynamixel#3 torque
	dxl_comm_result = packetHandler3->write1ByteTxRx(portHandler, DXL3_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}
	else {
		printf("Dynamixel#%d has been successfully connected \n", DXL3_ID);
	}

	// Enable Dynamixel#4 torque
	dxl_comm_result = packetHandler4->write1ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
	else {
		printf("Dynamixel#%d has been successfully connected \n", DXL4_ID);
	}
}

void write_acceleration(void) {
	////////////////////////////////////////////
		//Write acceleration for motors
		////////////////////////////////////////////
	dxl_comm_result = packetHandler1->write4ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_PROFILE_ACCELERATION, DXL1_ACCELERATION, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}

	dxl_comm_result = packetHandler2->write4ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_PROFILE_ACCELERATION, DXL2_ACCELERATION, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}

	dxl_comm_result = packetHandler3->write1ByteTxRx(portHandler, DXL3_ID, ADDR_MX_GOAL_ACCELERATION, DXL3_ACCELERATION, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}


	dxl_comm_result = packetHandler4->write4ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_PROFILE_ACCELERATION, DXL4_ACCELERATION, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
}

void write_velocity(void) {
	////////////////////////////////////////////
		//Write velocity for motors
		////////////////////////////////////////////
	dxl_comm_result = packetHandler1->write4ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_PROFILE_VELOCITY, DXL1_VELOCITY[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}

	dxl_comm_result = packetHandler2->write4ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_PROFILE_VELOCITY, DXL2_VELOCITY[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}

	dxl_comm_result = packetHandler3->write2ByteTxRx(portHandler, DXL3_ID, ADDR_MX_MOVING_SPEED, DXL3_MOVING_SPEED[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}


	dxl_comm_result = packetHandler4->write4ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_PROFILE_VELOCITY, DXL4_VELOCITY[index], &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS) {
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0) {
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
}

void disable_torque(void) {
	////////////////////////////////////////////
		// Disable Dynamixel Torque (no need to edit)
		////////////////////////////////////////////

		// Disable Dynamixel#1 Torque
	dxl_comm_result = packetHandler1->write1ByteTxRx(portHandler, DXL1_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS)
	{
		printf("%s\n", packetHandler1->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0)
	{
		printf("%s\n", packetHandler1->getRxPacketError(dxl_error));
	}

	// Disable Dynamixel#2 Torque
	dxl_comm_result = packetHandler2->write1ByteTxRx(portHandler, DXL2_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS)
	{
		printf("%s\n", packetHandler2->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0)
	{
		printf("%s\n", packetHandler2->getRxPacketError(dxl_error));
	}

	// Disable Dynamixel#3 Torque
	dxl_comm_result = packetHandler3->write1ByteTxRx(portHandler, DXL3_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS)
	{
		printf("%s\n", packetHandler3->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0)
	{
		printf("%s\n", packetHandler3->getRxPacketError(dxl_error));
	}

	// Disable Dynamixel#4 Torque
	dxl_comm_result = packetHandler4->write1ByteTxRx(portHandler, DXL4_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
	if (dxl_comm_result != COMM_SUCCESS)
	{
		printf("%s\n", packetHandler4->getTxRxResult(dxl_comm_result));
	}
	else if (dxl_error != 0)
	{
		printf("%s\n", packetHandler4->getRxPacketError(dxl_error));
	}
}