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

Diff: XL320_MFA/XL320_MFA.cpp

Revision:
2:0ec2dce7c5f8
Parent:
1:acad5f85d4f5
Child:
3:89bed7a6d852
--- a/XL320_MFA/XL320_MFA.cpp	Wed Feb 24 10:37:29 2016 +0000
+++ b/XL320_MFA/XL320_MFA.cpp	Thu Feb 25 04:19:15 2016 +0000
@@ -18,7 +18,9 @@
 /***** XL320 Network Parameter *****/
 void XL320::SetID(uint8_t id,uint8_t idnew)
 {
-	pc.printf("Set ID\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set ID\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = idnew;
@@ -34,7 +36,9 @@
 
 uint8_t XL320::GetID(uint8_t id)
 {
-	pc.printf("Get ID\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get ID\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -58,7 +62,9 @@
 
 void XL320::SetBaudRate(uint8_t id,uint8_t baudrate)
 {
-	pc.printf("Set BaudRate\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set BaudRate\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = baudrate;
@@ -72,7 +78,9 @@
 
 uint8_t XL320::GetBaudRate(uint8_t id)
 {
-	pc.printf("Get BaudRate\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get BaudRate\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -96,7 +104,9 @@
 
 void XL320::SetRetDelTime(uint8_t id,uint8_t time)
 {
-	pc.printf("Set Return Delay Time\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Return Delay Time\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = time;
@@ -110,7 +120,9 @@
 
 uint8_t XL320::GetRetDelTime(uint8_t id)
 {
-	pc.printf("Get Return Delay Time\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Return Delay Time\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -135,7 +147,9 @@
 /***** XL320 Motor Setting *****/
 void XL320::SetCWAngLim(uint8_t id,uint16_t angle)
 {
-	pc.printf("Set CW Angle Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set CW Angle Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(angle);
@@ -150,7 +164,9 @@
 
 uint16_t XL320::GetCWAngLim(uint8_t id)
 {
-	pc.printf("Get CW Angle Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get CW Angle Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -174,7 +190,9 @@
 
 void XL320::SetCCWAngLim(uint8_t id,uint16_t angle)
 {
-	pc.printf("Set CCW Angle Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set CCW Angle Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(angle);
@@ -189,7 +207,9 @@
 
 uint16_t XL320::GetCCWAngLim(uint8_t id)
 {
-	pc.printf("Get CCW Angle Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get CCW Angle Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -213,7 +233,9 @@
 
 void XL320::SetContMode(uint8_t id,uint8_t mode)
 {
-	pc.printf("Set Control Mode \n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Control Mode \n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = mode;
@@ -227,7 +249,9 @@
 
 uint8_t XL320::GetContMode(uint8_t id)
 {
-	pc.printf("Get Control Mode\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Control Mode\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -251,7 +275,9 @@
 
 void XL320::SetTempLim(uint8_t id,uint8_t temp)
 {
-	pc.printf("Set Temperature Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Temperature Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = temp;
@@ -265,7 +291,9 @@
 
 uint8_t XL320::GetTempLim(uint8_t id)
 {
-	pc.printf("Get Temperature Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Temperature Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -289,7 +317,9 @@
 
 void XL320::SetLowVoltLim(uint8_t id,uint8_t volt)
 {
-	pc.printf("Set Lower Voltage Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Lower Voltage Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = volt;
@@ -303,7 +333,9 @@
 
 uint8_t XL320::GetLowVoltLim(uint8_t id)
 {
-	pc.printf("Get Lower Voltage Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Lower Voltage Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -327,7 +359,9 @@
 
 void XL320::SetUpVoltLim(uint8_t id,uint8_t volt)
 {
-	pc.printf("Set Upper Voltage Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Upper Voltage Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = volt;
@@ -341,7 +375,9 @@
 
 uint8_t XL320::GetUpVoltLim(uint8_t id)
 {
-	pc.printf("Get Upper Voltage Limit\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Upper Voltage Limit\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -366,7 +402,9 @@
 
 void XL320::SetMaxTorq(uint8_t id,uint16_t torque)
 {
-	pc.printf("Set Maximum Torque\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Maximum Torque\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(torque);
@@ -381,7 +419,9 @@
 
 uint16_t XL320::GetMaxTorq(uint8_t id)
 {
-	pc.printf("Get Maximum Torque\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Maximum Torque\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -405,7 +445,9 @@
 
 void XL320::SetRetLev(uint8_t id,uint8_t level)
 {
-	pc.printf("Set Return Level\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Return Level\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = level;
@@ -419,7 +461,9 @@
 
 uint8_t XL320::GetRetLev(uint8_t id)
 {
-	pc.printf("Get Return Level\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Return Level\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -443,7 +487,9 @@
 
 void XL320::SetAlarmShut(uint8_t id,uint8_t alarm)
 {
-	pc.printf("Set Alarm Shutdown\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Alarm Shutdown\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = alarm;
@@ -457,7 +503,9 @@
 
 uint8_t XL320::GetAlarmShut(uint8_t id)
 {
-	pc.printf("Get Alarm Shutdown\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Alarm Shutdown\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -483,7 +531,9 @@
 /***** XL320 On/Off *****/
 void XL320::SetTorqueEn(uint8_t id,uint8_t enable)
 {
-	pc.printf("Set Torque Enable\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Torque Enable\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = enable;
@@ -497,7 +547,9 @@
 
 uint8_t XL320::GetTorqueEn(uint8_t id)
 {
-	pc.printf("Get Torque Enable\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Torque Enable\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -521,7 +573,9 @@
 
 void XL320::TurnOnLED(uint8_t id,uint8_t led)
 {
-	pc.printf("Turn On LED\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Turn On LED\n\r");
+	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
 	parameter[0] = led;	//led to turn on
@@ -537,7 +591,9 @@
 
 uint8_t XL320::GetStatusLED(uint8_t id)
 {
-	pc.printf("Get Status LED\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Status LED\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -562,7 +618,9 @@
 /***** XL320 Motor Control *****/
 void XL320::SetDGain(uint8_t id,uint8_t d_cons)
 {
-	pc.printf("Set D-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set D-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = d_cons;
@@ -578,7 +636,9 @@
 
 uint8_t XL320::GetDGain(uint8_t id)
 {
-	pc.printf("Get D-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get D-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -602,7 +662,9 @@
 
 void XL320::SetIGain(uint8_t id,uint8_t i_cons)
 {
-	pc.printf("Set I-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set I-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = i_cons;
@@ -618,7 +680,9 @@
 
 uint8_t XL320::GetIGain(uint8_t id)
 {
-	pc.printf("Get I-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get I-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -642,7 +706,9 @@
 
 void XL320::SetPGain(uint8_t id,uint8_t p_cons)
 {
-	pc.printf("Set P-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set P-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
 	parameter[0] = p_cons;
@@ -658,7 +724,9 @@
 
 uint8_t XL320::GetPGain(uint8_t id)
 {
-	pc.printf("Get P-Gain\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get P-Gain\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -682,7 +750,9 @@
 
 void XL320::SetGoalPos(uint8_t id,uint16_t position)
 {
-	pc.printf("Set Goal Position\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Goal Position\n\r");
+	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(position);	//position goal LSB
@@ -699,7 +769,9 @@
 
 uint16_t XL320::GetGoalPos(uint8_t id)
 {
-	pc.printf("Get Goal Position\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Goal Position\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -723,7 +795,9 @@
 
 void XL320::SetGoalVel(uint8_t id,uint16_t velocity)
 {
-	pc.printf("Set Goal Velocity\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Goal Velocity\n\r");
+	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(velocity);
@@ -740,7 +814,9 @@
 
 uint16_t XL320::GetGoalVel(uint8_t id)
 {
-	pc.printf("Get Goal Velocity\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Goal Velocity\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -764,7 +840,9 @@
 
 void XL320::SetGoalTorq(uint8_t id,uint16_t torque)
 {
-	pc.printf("Set Goal Torque\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Goal Torque\n\r");
+	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(torque);
@@ -781,7 +859,9 @@
 
 uint16_t XL320::GetGoalTorq(uint8_t id)
 {
-	pc.printf("Get Goal Torque\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Goal Torque\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -806,7 +886,9 @@
 /***** XL320 Feedback *****/
 uint16_t XL320::GetPresentPos(uint8_t id)
 {
-	pc.printf("Get Present Position\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Present Position\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -830,7 +912,9 @@
 
 uint16_t XL320::GetPresentSpeed(uint8_t id)
 {
-	pc.printf("Get Present Speed\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Present Speed\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -854,7 +938,9 @@
 
 uint16_t XL320::GetPresentLoad(uint8_t id)
 {
-	pc.printf("Get Present Load\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Present Load\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -878,7 +964,9 @@
 
 uint8_t XL320::GetPresentVolt(uint8_t id)
 {
-	pc.printf("Get Present Volt\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Present Volt\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -902,7 +990,9 @@
 
 uint8_t XL320::GetPresentTemp(uint8_t id)
 {
-	pc.printf("Get Present Temperature\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Present Temperature\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -927,7 +1017,9 @@
 /***** XL320 Status *****/
 uint8_t XL320::GetRegInst(uint8_t id)
 {
-	pc.printf("Get Registered Instruction\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Registered Instruction\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -951,7 +1043,9 @@
 
 uint8_t XL320::GetMoving(uint8_t id)
 {
-	pc.printf("Get Moving\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Moving\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -975,7 +1069,9 @@
 
 uint8_t XL320::GetHWErr(uint8_t id)
 {
-	pc.printf("Get Hardware Error Status\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Hardware Error Status\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -999,7 +1095,9 @@
 
 void XL320::SetPunch(uint8_t id,uint16_t punch)
 {
-	pc.printf("Set Punch\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Set Punch\n\r");
+	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
 	parameter[0] = SHIFT_TO_LSB(punch);
@@ -1016,7 +1114,9 @@
 
 uint16_t XL320::GetPunch(uint8_t id)
 {
-	pc.printf("Get Punch\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Get Punch\n\r");
+	#endif
 	targetID = id;
 	targetInst = READ_DATA;
 	
@@ -1042,7 +1142,9 @@
 /***** XL320 Instruction Method *****/  
 void XL320::iFactoryReset(uint8_t id,uint8_t option)
 {
-	pc.printf("Factory Reset!\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("Factory Reset!\n\r");
+	#endif
 	targetID = id;
 	targetInst = FACTORY_RESET;
 	
@@ -1065,7 +1167,9 @@
 
 void XL320::iPing(uint8_t id)
 {
-	pc.printf("PING!\n\r");
+	#ifdef DATA_DEBUG
+		pc.printf("PING!\n\r");
+	#endif
 	targetID = id;
 	targetInst = PING;
 	
@@ -1289,10 +1393,17 @@
     	if(i == wTimeOut)
     	{
     		wait = false;
-    		pc.printf("No Return Packet Detected!, ");
+    		#ifdef DATA_DEBUG
+    			pc.printf("No Return Packet Detected!, ");
+    		#endif
     	}
     }
-    pc.printf("has wait Return Packet for :'%d' us\n\r", i);
+    
+    #ifdef DATA_DEBUG
+    	pc.printf("has wait Return Packet for :'%d' us\n\r", i);
+    #else
+    	wait_ms(4);
+    #endif
     
     i = 0;
     while(sXL320.readable() && i < rPacketLength) 
@@ -1302,7 +1413,9 @@
     }
     if(i == 0)
     {
-    	pc.printf("F*ck! missed the return packet!@#$%^&* \n\r");
+    	#ifdef DATA_DEBUG
+    		pc.printf("F*ck! missed the return packet!@#$%^&* \n\r");
+    	#endif
     }
     
     //printLastPackets();