Diff: XL320_MFA/XL320_MFA.cpp

Revision:

0:2071f8e463dd

Child:

1:acad5f85d4f5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XL320_MFA/XL320_MFA.cpp	Wed Feb 24 10:25:39 2016 +0000
@@ -0,0 +1,1360 @@
+#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(){};
\ No newline at end of file