Rhino Reimburse / Mbed 2 deprecated Dynamixel_XL320

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of XL320_Test by Rhino Reimburse

XL320_MFA/XL320_MFA.cpp

Committer:
mfajaradityo
Date:
2016-02-24
Revision:
0:2071f8e463dd
Child:
1:acad5f85d4f5

File content as of revision 0:2071f8e463dd:

#include "XL320_MFA.h"

XL320::XL320(uint32_t baudrate, PinName tx, PinName rx, PinName dirpin): sXL320(tx, rx), dPin(dirpin)
{   
    dPin=0;
    sXL320.baud(baudrate);
    pc.baud(115200);
    
    //Initialize header
	iPacket[0] = 0xff; //header
    iPacket[1] = 0xff; //header
    iPacket[2] = 0xfd; //header
    iPacket[3] = 0x00; //reserved
}

////// EEPROM ACCESS METHODS //////

/***** XL320 Network Parameter *****/
void XL320::SetID(uint8_t id,uint8_t idnew)
{
	pc.printf("Set ID\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = idnew;
	
	iWrite(1, XL_ID, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
	
	wait_ms(restTime);
}

uint8_t XL320::GetID(uint8_t id)
{
	pc.printf("Get ID\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, XL_ID);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9]; //return tergeted servo ID
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetBaudRate(uint8_t id,uint8_t baudrate)
{
	pc.printf("Set BaudRate\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = baudrate;
	
	iWrite(1, BAUD_RATE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetBaudRate(uint8_t id)
{
	pc.printf("Get BaudRate\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, BAUD_RATE);
	sendIPacket();
	
	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9]; //return tergeted servo Baud Rate
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}	
}

void XL320::SetRetDelTime(uint8_t id,uint8_t time)
{
	pc.printf("Set Return Delay Time\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = time;
	
	iWrite(1, RETURN_DELAY_TIME, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetRetDelTime(uint8_t id)
{
	pc.printf("Get Return Delay Time\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, RETURN_DELAY_TIME);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9]; //return tergeted servo Baud Rate
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

/***** XL320 Motor Setting *****/
void XL320::SetCWAngLim(uint8_t id,uint16_t angle)
{
	pc.printf("Set CW Angle Limit\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(angle);
	parameter[1] = SHIFT_TO_MSB(angle);
	
	iWrite(2, CW_ANGLE_LIMIT, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint16_t XL320::GetCWAngLim(uint8_t id)
{
	pc.printf("Get CW Angle Limit\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, CW_ANGLE_LIMIT);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

void XL320::SetCCWAngLim(uint8_t id,uint16_t angle)
{
	pc.printf("Set CCW Angle Limit\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(angle);
	parameter[1] = SHIFT_TO_MSB(angle);
	
	iWrite(2, CCW_ANGLE_LIMIT, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint16_t XL320::GetCCWAngLim(uint8_t id)
{
	pc.printf("Get CCW Angle Limit\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, CW_ANGLE_LIMIT);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

void XL320::SetContMode(uint8_t id,uint8_t mode)
{
	pc.printf("Set Control Mode \n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = mode;
	
	iWrite(1, CONTROL_MODE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetContMode(uint8_t id)
{
	pc.printf("Get Control Mode\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, CONTROL_MODE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetTempLim(uint8_t id,uint8_t temp)
{
	pc.printf("Set Temperature Limit\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = temp;
	
	iWrite(1, LIMIT_TEMPERATURE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetTempLim(uint8_t id)
{
	pc.printf("Get Temperature Limit\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, LIMIT_TEMPERATURE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetLowVoltLim(uint8_t id,uint8_t volt)
{
	pc.printf("Set Lower Voltage Limit\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = volt;
	
	iWrite(1, LOWER_LIMIT_VOLTAGE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetLowVoltLim(uint8_t id)
{
	pc.printf("Get Lower Voltage Limit\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, LOWER_LIMIT_VOLTAGE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetUpVoltLim(uint8_t id,uint8_t volt)
{
	pc.printf("Set Upper Voltage Limit\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = volt;
	
	iWrite(1, UPPER_LIMIT_VOLTAGE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetUpVoltLim(uint8_t id)
{
	pc.printf("Get Upper Voltage Limit\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, UPPER_LIMIT_VOLTAGE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}


void XL320::SetMaxTorq(uint8_t id,uint16_t torque)
{
	pc.printf("Set Maximum Torque\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(torque);
	parameter[1] = SHIFT_TO_MSB(torque);
	
	iWrite(2, MAX_TORQUE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint16_t XL320::GetMaxTorq(uint8_t id)
{
	pc.printf("Get Maximum Torque\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, MAX_TORQUE);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

void XL320::SetRetLev(uint8_t id,uint8_t level)
{
	pc.printf("Set Return Level\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = level;
	
	iWrite(1, RETURN_LEVEL, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetRetLev(uint8_t id)
{
	pc.printf("Get Return Level\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, RETURN_LEVEL);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetAlarmShut(uint8_t id,uint8_t alarm)
{
	pc.printf("Set Alarm Shutdown\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = alarm;
	
	iWrite(1, ALARM_SHUTDOWN, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetAlarmShut(uint8_t id)
{
	pc.printf("Get Alarm Shutdown\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, ALARM_SHUTDOWN);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

////// RAM ACCESS METHODS //////
/***** XL320 On/Off *****/
void XL320::SetTorqueEn(uint8_t id,uint8_t enable)
{
	pc.printf("Set Torque Enable\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = enable;
	
	iWrite(1, TORQUE_ENABLE, parameter);
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

uint8_t XL320::GetTorqueEn(uint8_t id)
{
	pc.printf("Get Torque Enable\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, TORQUE_ENABLE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::TurnOnLED(uint8_t id,uint8_t led)
{
	pc.printf("Turn On LED\n\r");
	targetID = id; //XL320 ID
	targetInst = WRITE_DATA;
	parameter[0] = led;	//led to turn on
	
	iWrite(1, LED, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime);
}

uint8_t XL320::GetStatusLED(uint8_t id)
{
	pc.printf("Get Status LED\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, LED);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

/***** XL320 Motor Control *****/
void XL320::SetDGain(uint8_t id,uint8_t d_cons)
{
	pc.printf("Set D-Gain\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = d_cons;
	
	iWrite(1, D_GAIN, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime);
}

uint8_t XL320::GetDGain(uint8_t id)
{
	pc.printf("Get D-Gain\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, D_GAIN);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetIGain(uint8_t id,uint8_t i_cons)
{
	pc.printf("Set I-Gain\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = i_cons;
	
	iWrite(1, I_GAIN, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime);
}

uint8_t XL320::GetIGain(uint8_t id)
{
	pc.printf("Get I-Gain\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, I_GAIN);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetPGain(uint8_t id,uint8_t p_cons)
{
	pc.printf("Set P-Gain\n\r");
	targetID = id;
	targetInst = WRITE_DATA;
	parameter[0] = p_cons;
	
	iWrite(1, P_GAIN, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime);
}

uint8_t XL320::GetPGain(uint8_t id)
{
	pc.printf("Get P-Gain\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, P_GAIN);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetGoalPos(uint8_t id,uint16_t position)
{
	pc.printf("Set Goal Position\n\r");
	targetID = id; //XL320 ID
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(position);	//position goal LSB
	parameter[1] = SHIFT_TO_MSB(position);	//position goal MSB
	
	iWrite(2, GOAL_POSITION, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
}

uint16_t XL320::GetGoalPos(uint8_t id)
{
	pc.printf("Get Goal Position\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, GOAL_POSITION);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

void XL320::SetGoalVel(uint8_t id,uint16_t velocity)
{
	pc.printf("Set Goal Velocity\n\r");
	targetID = id; //XL320 ID
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(velocity);
	parameter[1] = SHIFT_TO_MSB(velocity);
	
	iWrite(2, GOAL_SPEED, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
}

uint16_t XL320::GetGoalVel(uint8_t id)
{
	pc.printf("Get Goal Velocity\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, GOAL_SPEED);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

void XL320::SetGoalTorq(uint8_t id,uint16_t torque)
{
	pc.printf("Set Goal Torque\n\r");
	targetID = id; //XL320 ID
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(torque);
	parameter[1] = SHIFT_TO_MSB(torque);
	
	iWrite(2, GOAL_TORQUE, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
}

uint16_t XL320::GetGoalTorq(uint8_t id)
{
	pc.printf("Get Goal Torque\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, GOAL_TORQUE);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

/***** XL320 Feedback *****/
uint16_t XL320::GetPresentPos(uint8_t id)
{
	pc.printf("Get Present Position\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, PRESENT_POSITION);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

uint16_t XL320::GetPresentSpeed(uint8_t id)
{
	pc.printf("Get Present Speed\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, PRESENT_SPEED);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

uint16_t XL320::GetPresentLoad(uint8_t id)
{
	pc.printf("Get Present Load\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, PRESENT_LOAD);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}

uint8_t XL320::GetPresentVolt(uint8_t id)
{
	pc.printf("Get Present Volt\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, PRESENT_VOLTAGE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

uint8_t XL320::GetPresentTemp(uint8_t id)
{
	pc.printf("Get Present Temperature\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, PRESENT_TEMPERATURE);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

/***** XL320 Status *****/
uint8_t XL320::GetRegInst(uint8_t id)
{
	pc.printf("Get Registered Instruction\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, REGISTERED_INSTRUCTION);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

uint8_t XL320::GetMoving(uint8_t id)
{
	pc.printf("Get Moving\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, MOVING);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

uint8_t XL320::GetHWErr(uint8_t id)
{
	pc.printf("Get Hardware Error Status\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(1, HARDWARE_ERROR_STATUS);
	sendIPacket();

	rPacketLength = 12;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return rPacket[9];
						
		default :	//error occurred
			return rPacket[8]; //return error status
	}
}

void XL320::SetPunch(uint8_t id,uint16_t punch)
{
	pc.printf("Set Punch\n\r");
	targetID = id; //XL320 ID
	targetInst = WRITE_DATA;
	parameter[0] = SHIFT_TO_LSB(punch);
	parameter[1] = SHIFT_TO_MSB(punch);
	
	iWrite(2, PUNCH, parameter);
	sendIPacket();
	
    rPacketLength = 11;
    getRPacket();
    
    wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
}

uint16_t XL320::GetPunch(uint8_t id)
{
	pc.printf("Get Punch\n\r");
	targetID = id;
	targetInst = READ_DATA;
	
	iRead(2, PUNCH);
	sendIPacket();

	rPacketLength = 13;
	getRPacket();
	
	wait_ms(restTime);
	
	switch(rPacket[8])
	{
		case 0x00 :	//no error
			return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
						
		default :	//error occurred
			return (uint16_t)rPacket[8]; //return error status
	}
}


/***** XL320 Instruction Method *****/  
void XL320::iFactoryReset(uint8_t id,uint8_t option)
{
	pc.printf("Factory Reset!\n\r");
	targetID = id;
	targetInst = FACTORY_RESET;
	
	iPacket[4] = targetID;
    iPacket[5] = 0x04; //[intruction][reset_option][CRC_LSB][CRC_MSB]
    iPacket[6] = 0x00;
    iPacket[7] = targetInst; //instruction
    iPacket[8] = option;	//Intruction Packet's parameter field 0 (reset option)
    
    CRC = update_crc(0, iPacket, 9);	//All Instruction Packet minus CRC
    iPacket[9] = SHIFT_TO_LSB(CRC);
    iPacket[10] = SHIFT_TO_MSB(CRC);
    
    iPacketLength = 11;
	sendIPacket();
	
	rPacketLength = 11;
	getRPacket();
}

void XL320::iPing(uint8_t id)
{
	pc.printf("PING!\n\r");
	targetID = id;
	targetInst = PING;
	
	iPacket[4] = targetID;
    iPacket[5] = 0x03; //[instruction][CRC_LSB][CRC_MSB]
    iPacket[6] = 0x00;
    iPacket[7] = targetInst; //instruction
    
    CRC = update_crc(0, iPacket, 9);	//All Instruction Packet minus CRC
    iPacket[8] = SHIFT_TO_LSB(CRC);
    iPacket[9] = SHIFT_TO_MSB(CRC);
    
    iPacketLength = 10;
	sendIPacket();
	
	rPacketLength = 14;
	getRPacket();
}

/***** Debugging *****/  
void XL320::printLastPackets()
{
	int i;
    
    //print to screen
    pc.printf("Instruction Packet : [");
    for(i = 0; i < iPacketLength; i++)
    {
        pc.printf("'%x' |", iPacket[i]);
    }
    pc.printf("]\n");
    pc.printf("\r");
    
    pc.printf("Return Packet : [");
    for(i = 0; i < rPacketLength; i++)
    {                  
        pc.printf("'%x' |",rPacket[i]);
    }   
    pc.printf("]\n");
    pc.printf("\r");
}

void XL320::translateLastRPacket()
{
	//check Return Packet from Write instruction
	if(rPacket[8] != 0x00)	//rPacket[8] is Error Byte
	{
		switch(rPacket[8])
		{				
			case RESULT_FAIL :
				pc.printf("Writing Intruction Failed! :(\n\r");
				break;
				
			case INS_ERR :
				pc.printf("Undefined Instruction ~.~\n\r");
				break;
				
			case CRC_ERR :
				pc.printf("CRC Value Error ~.~\n\r");
				break;
				
			case DATA_RANGE_ERR :
				pc.printf("Data Range Error ~.~\n\r");
				break;
				
			case DATA_LENGTH_ERR :
				pc.printf("Data too short! -__-\n\r");
				break;
				
			case DATA_LIMIT_ERR :
				pc.printf("Data too long! -__-\n\r");
				break;
				
			case ACCESS_ERR :
				pc.printf("Not Allowed! >:(\n\r");
				break;
			
			default :
				pc.printf("Something is wrong, don't know what ;(\n\r");
				break;
		}
	}
	else
	{
		if(targetInst == WRITE_DATA)
		{
			pc.printf("write Instruction Success!\n\r");
		}
		else
		{
			if(targetInst == READ_DATA)
			{
				switch(targetAddr)
				{
					case BAUD_RATE:
						switch(rPacket[9])
						{
							case 0x00:
								pc.printf("BaudRate : 9600\n\r");
								break;
							case 0x01:
								pc.printf("BaudRate : 57600\n\r");
								break;
							case 0x02:
								pc.printf("BaudRate : 115200\n\r");
								break;
							case 0x04:
								pc.printf("BaudRate : 1000000\n\r");
								break;
							default:
								pc.printf("BaudRate : ??\n\r");
								break;
						}
						break;
					
					case RETURN_DELAY_TIME:
						pc.printf("Return Delay Time : '%d'us\n\r", rPacket[9]*2);
						break;
					
					case GOAL_POSITION:
						pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
						break;
						
					case GOAL_SPEED:
						pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
						break;
					
					default:
						pc.printf("Value : '%d'\n\r", rPacket[9]);
						break;
				}
			}
		}
	}
	pc.printf("\n\r");
}

/***** XL320 Instruction Method *****/	
void XL320::iRead(uint16_t length, uint16_t address)
{
    targetAddr = address;
    	
    iPacket[4] = targetID;
    iPacket[5] = 0x07; //[intruction][address_LSB][address_MSB][DataLength_LSB][DataLength_MSB][CRC_LSB][CRC_MSB]
    iPacket[6] = 0x00;
    iPacket[7] = targetInst; //instruction
    iPacket[8] = SHIFT_TO_LSB(address);	//Intruction Packet's parameter field 0 (start address_LSB)
    iPacket[9] = SHIFT_TO_MSB(address); //Instruction Packet's parameter field 1 (start address_MSB)
	iPacket[10] = SHIFT_TO_LSB(length);	//Instruction Packet's parameter field 2 (read data length_MSB)
    iPacket[11] = SHIFT_TO_MSB(length);	//Instruction Packet's parameter field 3 (read data length_LSB)
    
    CRC = update_crc(0, iPacket, 12);	//All Instruction Packet minus CRC
    iPacket[12] = SHIFT_TO_LSB(CRC);
    iPacket[13] = SHIFT_TO_MSB(CRC);
    
    iPacketLength = 14;
}

void XL320::iWrite(uint8_t length, uint16_t address, uint8_t param[])
{
	int i;
	targetAddr = address;
	
    iPacket[4] = targetID;
    iPacket[5] = (5+length); //[intruction][address_LSB][address_MSB][parameter(length)][CRC_LSB][CRC_MSB]
    iPacket[6] = 0x00;
    iPacket[7] = targetInst; //instruction
    iPacket[8] = SHIFT_TO_LSB(address);	//Intruction Packet's parameter field 0 (start address_LSB)
    iPacket[9] = SHIFT_TO_MSB(address); //Instruction Packet's parameter field 1 (start address_MSB)
    for(i = 10; i < 10+length ; i++)
    {
    	iPacket[i] = param[i-10]; //Instruction Packet's parameter field 2 and so on (write data n-th byte)
    }
    
    CRC = update_crc(0, iPacket, 10+length);	//All Instruction Packet minus CRC
    iPacket[i] = SHIFT_TO_LSB(CRC);
    iPacket[i+1] = SHIFT_TO_MSB(CRC);
    
    iPacketLength = i+2;
}

/***** Dynamixel Protocol 2.0 Methods *****/
void XL320::sendIPacket()
{
	int i;
    
    //initialize rPacket
    i = 0;
    for(i = 0; i < nBytes; i++)
    {
        rPacket[i] = 0x99;
    }
    
    //send Instruction Packet
    dPin = 1;
    wait_ms(1);     //wait for 74HC126 enable pin transition delay
    
    for(i = 0; i < iPacketLength; i++)
    {
        sXL320.putc(iPacket[i]);
    }   
}

void XL320::getRPacket()
{
	int i;
	const unsigned int wTimeOut = 500;
	bool wait = true;
	
	//read Return Packet
    wait_us(180);	//strange..default response time delay should be 500..
    dPin = 0;
    
    //wait until Return Packet ready to be read
    i = 0;
    while(!sXL320.readable() && wait)
    {
    	wait_us(1); //
    	i++;
    	
    	if(i == wTimeOut)
    	{
    		wait = false;
    		pc.printf("No Return Packet Detected!, ");
    	}
    }
    //pc.printf("has wait Return Packet for :'%d' us\n\r", i);
    
    i = 0;
    while(sXL320.readable() && i < rPacketLength) 
    {
        rPacket[i] = sXL320.getc();
        i++;
    }
    if(i == 0)
    {
    	pc.printf("F*ck! missed the return packet!@#$%^&* \n\r");
    }
    
    //printLastPackets();
}

/***** Calculating CRC Method *****/
unsigned short XL320::update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size)
{
    unsigned short i, j;
    unsigned short crc_table[256] = {
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
    };
 
    for(j = 0; j < data_blk_size; j++)
    {
        i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
        crc_accum = (crc_accum << 8) ^ crc_table[i];
    }
 
    return crc_accum;
}


XL320::~XL320(){};