Revision 4:2df002cf5f6c, committed 2016-02-28

Comitter:

mfajaradityo

Date:

Sun Feb 28 14:48:03 2016 +0000

Parent:

3:89bed7a6d852

Commit message:

Benerin deskripsi repository yang sebelumnya

Changed in this revision

--- a/XL320_MFA/XL320_MFA.cpp	Thu Feb 25 04:28:14 2016 +0000
+++ b/XL320_MFA/XL320_MFA.cpp	Sun Feb 28 14:48:03 2016 +0000
@@ -19,7 +19,7 @@
 void XL320::SetID(uint8_t id,uint8_t idnew)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set ID\n\r");
+		pc.printf("Set ID (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -37,7 +37,7 @@
 uint8_t XL320::GetID(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get ID\n\r");
+		pc.printf("Get ID (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -63,7 +63,7 @@
 void XL320::SetBaudRate(uint8_t id,uint8_t baudrate)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set BaudRate\n\r");
+		pc.printf("Set BaudRate (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -79,7 +79,7 @@
 uint8_t XL320::GetBaudRate(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get BaudRate\n\r");
+		pc.printf("Get BaudRate (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -105,7 +105,7 @@
 void XL320::SetRetDelTime(uint8_t id,uint8_t time)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Return Delay Time\n\r");
+		pc.printf("Set Return Delay Time (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -121,7 +121,7 @@
 uint8_t XL320::GetRetDelTime(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Return Delay Time\n\r");
+		pc.printf("Get Return Delay Time (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -148,7 +148,7 @@
 void XL320::SetCWAngLim(uint8_t id,uint16_t angle)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set CW Angle Limit\n\r");
+		pc.printf("Set CW Angle Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -165,7 +165,7 @@
 uint16_t XL320::GetCWAngLim(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get CW Angle Limit\n\r");
+		pc.printf("Get CW Angle Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -191,7 +191,7 @@
 void XL320::SetCCWAngLim(uint8_t id,uint16_t angle)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set CCW Angle Limit\n\r");
+		pc.printf("Set CCW Angle Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -208,7 +208,7 @@
 uint16_t XL320::GetCCWAngLim(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get CCW Angle Limit\n\r");
+		pc.printf("Get CCW Angle Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -234,7 +234,7 @@
 void XL320::SetContMode(uint8_t id,uint8_t mode)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Control Mode \n\r");
+		pc.printf("Set Control Mode (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -250,7 +250,7 @@
 uint8_t XL320::GetContMode(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Control Mode\n\r");
+		pc.printf("Get Control Mode (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -276,7 +276,7 @@
 void XL320::SetTempLim(uint8_t id,uint8_t temp)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Temperature Limit\n\r");
+		pc.printf("Set Temperature Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -292,7 +292,7 @@
 uint8_t XL320::GetTempLim(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Temperature Limit\n\r");
+		pc.printf("Get Temperature Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -318,7 +318,7 @@
 void XL320::SetLowVoltLim(uint8_t id,uint8_t volt)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Lower Voltage Limit\n\r");
+		pc.printf("Set Lower Voltage Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -334,7 +334,7 @@
 uint8_t XL320::GetLowVoltLim(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Lower Voltage Limit\n\r");
+		pc.printf("Get Lower Voltage Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -360,7 +360,7 @@
 void XL320::SetUpVoltLim(uint8_t id,uint8_t volt)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Upper Voltage Limit\n\r");
+		pc.printf("Set Upper Voltage Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -376,7 +376,7 @@
 uint8_t XL320::GetUpVoltLim(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Upper Voltage Limit\n\r");
+		pc.printf("Get Upper Voltage Limit (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -403,7 +403,7 @@
 void XL320::SetMaxTorq(uint8_t id,uint16_t torque)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Maximum Torque\n\r");
+		pc.printf("Set Maximum Torque (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -420,7 +420,7 @@
 uint16_t XL320::GetMaxTorq(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Maximum Torque\n\r");
+		pc.printf("Get Maximum Torque (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -446,7 +446,7 @@
 void XL320::SetRetLev(uint8_t id,uint8_t level)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Return Level\n\r");
+		pc.printf("Set Return Level (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -462,7 +462,7 @@
 uint8_t XL320::GetRetLev(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Return Level\n\r");
+		pc.printf("Get Return Level (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -488,7 +488,7 @@
 void XL320::SetAlarmShut(uint8_t id,uint8_t alarm)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Alarm Shutdown\n\r");
+		pc.printf("Set Alarm Shutdown (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -504,7 +504,7 @@
 uint8_t XL320::GetAlarmShut(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Alarm Shutdown\n\r");
+		pc.printf("Get Alarm Shutdown (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -532,7 +532,7 @@
 void XL320::SetTorqueEn(uint8_t id,uint8_t enable)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Torque Enable\n\r");
+		pc.printf("Set Torque Enable (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -548,7 +548,7 @@
 uint8_t XL320::GetTorqueEn(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Torque Enable\n\r");
+		pc.printf("Get Torque Enable (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -574,7 +574,7 @@
 void XL320::TurnOnLED(uint8_t id,uint8_t led)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Turn On LED\n\r");
+		pc.printf("Turn On LED (Servo'%d')\n\r", id);
 	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
@@ -592,7 +592,7 @@
 uint8_t XL320::GetStatusLED(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Status LED\n\r");
+		pc.printf("Get Status LED (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -619,7 +619,7 @@
 void XL320::SetDGain(uint8_t id,uint8_t d_cons)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set D-Gain\n\r");
+		pc.printf("Set D-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -637,7 +637,7 @@
 uint8_t XL320::GetDGain(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get D-Gain\n\r");
+		pc.printf("Get D-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -663,7 +663,7 @@
 void XL320::SetIGain(uint8_t id,uint8_t i_cons)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set I-Gain\n\r");
+		pc.printf("Set I-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -681,7 +681,7 @@
 uint8_t XL320::GetIGain(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get I-Gain\n\r");
+		pc.printf("Get I-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -707,7 +707,7 @@
 void XL320::SetPGain(uint8_t id,uint8_t p_cons)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set P-Gain\n\r");
+		pc.printf("Set P-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = WRITE_DATA;
@@ -725,7 +725,7 @@
 uint8_t XL320::GetPGain(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get P-Gain\n\r");
+		pc.printf("Get P-Gain (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -751,7 +751,7 @@
 void XL320::SetGoalPos(uint8_t id,uint16_t position)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Goal Position\n\r");
+		pc.printf("Set Goal Position (Servo'%d')\n\r", id);
 	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
@@ -770,7 +770,7 @@
 uint16_t XL320::GetGoalPos(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Goal Position\n\r");
+		pc.printf("Get Goal Position (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -796,7 +796,7 @@
 void XL320::SetGoalVel(uint8_t id,uint16_t velocity)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Goal Velocity\n\r");
+		pc.printf("Set Goal Velocity (Servo'%d')\n\r", id);
 	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
@@ -815,7 +815,7 @@
 uint16_t XL320::GetGoalVel(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Goal Velocity\n\r");
+		pc.printf("Get Goal Velocity (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -841,7 +841,7 @@
 void XL320::SetGoalTorq(uint8_t id,uint16_t torque)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Goal Torque\n\r");
+		pc.printf("Set Goal Torque (Servo'%d')\n\r", id);
 	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
@@ -860,7 +860,7 @@
 uint16_t XL320::GetGoalTorq(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Goal Torque\n\r");
+		pc.printf("Get Goal Torque (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -887,7 +887,7 @@
 uint16_t XL320::GetPresentPos(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Present Position\n\r");
+		pc.printf("Get Present Position (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -913,7 +913,7 @@
 uint16_t XL320::GetPresentSpeed(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Present Speed\n\r");
+		pc.printf("Get Present Speed (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -939,7 +939,7 @@
 uint16_t XL320::GetPresentLoad(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Present Load\n\r");
+		pc.printf("Get Present Load (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -965,7 +965,7 @@
 uint8_t XL320::GetPresentVolt(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Present Volt\n\r");
+		pc.printf("Get Present Volt (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -991,7 +991,7 @@
 uint8_t XL320::GetPresentTemp(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Present Temperature\n\r");
+		pc.printf("Get Present Temperature (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -1018,7 +1018,7 @@
 uint8_t XL320::GetRegInst(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Registered Instruction\n\r");
+		pc.printf("Get Registered Instruction (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -1044,7 +1044,7 @@
 uint8_t XL320::GetMoving(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Moving\n\r");
+		pc.printf("Get Moving (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -1070,7 +1070,7 @@
 uint8_t XL320::GetHWErr(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Hardware Error Status\n\r");
+		pc.printf("Get Hardware Error Status (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -1096,7 +1096,7 @@
 void XL320::SetPunch(uint8_t id,uint16_t punch)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Set Punch\n\r");
+		pc.printf("Set Punch (Servo'%d')\n\r", id);
 	#endif
 	targetID = id; //XL320 ID
 	targetInst = WRITE_DATA;
@@ -1115,7 +1115,7 @@
 uint16_t XL320::GetPunch(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Get Punch\n\r");
+		pc.printf("Get Punch (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = READ_DATA;
@@ -1143,7 +1143,7 @@
 void XL320::iFactoryReset(uint8_t id,uint8_t option)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("Factory Reset!\n\r");
+		pc.printf("Factory Reset! (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = FACTORY_RESET;
@@ -1168,7 +1168,7 @@
 void XL320::iPing(uint8_t id)
 {
 	#ifdef DATA_DEBUG
-		pc.printf("PING!\n\r");
+		pc.printf("PING! (Servo'%d')\n\r", id);
 	#endif
 	targetID = id;
 	targetInst = PING;
@@ -1288,13 +1288,40 @@
 					case RETURN_DELAY_TIME:
 						pc.printf("Return Delay Time : '%d'us\n\r", rPacket[9]*2);
 						break;
+						
+					case CW_ANGLE_LIMIT:
+						pc.printf("CW Angle Limit : '%f' degree\n\r", ((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))*0.29);
+						break;
+						
+					case CCW_ANGLE_LIMIT:
+						pc.printf("CCW Angle Limit : '%f' degree\n\r", ((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))*0.29);
+						break;
+						
+					case CONTROL_MODE:
+						if(rPacket[9] == 2)
+						{
+							pc.printf("Control Mode : Join-Mode\n\r");
+						}
+						else
+						{
+							pc.printf("Control Mode : Wheel-Mode\n\r");
+						}
+						break;
 					
 					case GOAL_POSITION:
-						pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
+						pc.printf("Goal Position : '%f' Degree\n\r", ((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))*0.29);
 						break;
 						
 					case GOAL_SPEED:
-						pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
+						pc.printf("Speed in Join-Mode : '%f' rpm\n\r", ((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))*0.111);
+						if(((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00)) < 1024)
+						{
+							pc.printf("Speed in Wheel-Mode : '%f' CW\n\r", ((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))*0.1);
+						}
+						else
+						{
+							pc.printf("Speed in Wheel-Mode : '%f' CCW\n\r", (((uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00))-1024)*0.1);
+						}
 						break;
 					
 					default:
@@ -1398,12 +1425,7 @@
     		#endif
     	}
     }
-    
-    #ifdef DATA_DEBUG
-    	pc.printf("has wait Return Packet for :'%d' us\n\r", i);
-    #else
-    	wait_ms(4);
-    #endif
+    wait_ms(4);	//somehow has to wait 4ms after sXL320 is readable 
     
     i = 0;
     while(sXL320.readable() && i < rPacketLength) 

--- a/XL320_MFA/XL320_MFA.h	Thu Feb 25 04:28:14 2016 +0000
+++ b/XL320_MFA/XL320_MFA.h	Sun Feb 28 14:48:03 2016 +0000
@@ -8,7 +8,7 @@
 
 static Serial pc(USBTX,USBRX);
 ////////// DEBUG ///////////
-//#define DATA_DEBUG				//Command this line to disable some of pc.printf in XL320_MFA.cpp
+#define DATA_DEBUG				//Command this line to disable some of pc.printf in XL320_MFA.cpp
 
 ////////// SHIFT BYTES ///////////
 #define SHIFT_TO_LSB(w)			(uint8_t((uint16_t(w)) & 0x00ff))

--- a/main.cpp	Thu Feb 25 04:28:14 2016 +0000
+++ b/main.cpp	Sun Feb 28 14:48:03 2016 +0000
@@ -10,33 +10,55 @@
     while(1)
     {        
         wait(1);
-                
+               
         servoTest.TurnOnLED(1, 1);
+        servoTest.TurnOnLED(2, 7);
+        
         servoTest.TurnOnLED(1, 2);
+        servoTest.TurnOnLED(2, 6);
+        
         servoTest.TurnOnLED(1, 3);
+        servoTest.TurnOnLED(2, 5);
+        
         servoTest.TurnOnLED(1, 4);
+        servoTest.TurnOnLED(2, 4);
+        
         servoTest.TurnOnLED(1, 5);
+        servoTest.TurnOnLED(2, 3);
+        
         servoTest.TurnOnLED(1, 6);
-        servoTest.TurnOnLED(1, 7); 
-        servoTest.TurnOnLED(1, 8); // should return error message
+        servoTest.TurnOnLED(2, 2);
+        
+        servoTest.TurnOnLED(1, 7);
+        servoTest.TurnOnLED(2, 1);
+        
+        servoTest.TurnOnLED(2, 8);  // should return error message
+        servoTest.translateLastRPacket(); 
+        servoTest.TurnOnLED(1, 8);  // should return error message
         servoTest.translateLastRPacket();
         
         value = servoTest.GetID(1);
-        servoTest.translateLastRPacket();        
+        servoTest.translateLastRPacket();       
+        value = servoTest.GetID(2);
+        servoTest.translateLastRPacket();
         
         value = servoTest.GetBaudRate(1);
         servoTest.translateLastRPacket();
+        value = servoTest.GetBaudRate(2);
+        servoTest.translateLastRPacket();
         
-        value = servoTest.GetRetDelTime(1);
+        servoTest.SetTorqueEn(1, 0);
+        servoTest.translateLastRPacket();
+        servoTest.SetTorqueEn(2, 0);
         servoTest.translateLastRPacket();
         
-        value = servoTest.GetCWAngLim(1);
+        servoTest.SetContMode(1, 2);    
         servoTest.translateLastRPacket();
-        
-        value = servoTest.GetCCWAngLim(1);
+        servoTest.SetContMode(2, 2);
+        servoTest.translateLastRPacket(); 
+        value = servoTest.GetContMode(1);
         servoTest.translateLastRPacket();
-        
-        value = servoTest.GetContMode(1);
+        value = servoTest.GetContMode(2);
         servoTest.translateLastRPacket();
         
         servoTest.SetGoalVel(1, 1000);
@@ -44,21 +66,42 @@
         value = servoTest.GetGoalVel(1);
         servoTest.translateLastRPacket();
         
+        servoTest.SetGoalVel(2, 200);
+        servoTest.translateLastRPacket();
+        value = servoTest.GetGoalVel(2);
+        servoTest.translateLastRPacket();
+        
         servoTest.SetGoalPos(1, 0);
         servoTest.translateLastRPacket();
         value = servoTest.GetGoalPos(1);
         servoTest.translateLastRPacket();
         
+        servoTest.SetGoalPos(2, 1000);
+        servoTest.translateLastRPacket();
+        value = servoTest.GetGoalPos(2);
+        servoTest.translateLastRPacket();
+        
         wait(1);
         servoTest.SetGoalVel(1, 200);
         servoTest.translateLastRPacket();
         value = servoTest.GetGoalVel(1);
         servoTest.translateLastRPacket();
         
-        servoTest.SetGoalPos(1, 1000);
+        servoTest.SetGoalVel(2, 1000);
+        servoTest.translateLastRPacket();
+        value = servoTest.GetGoalVel(2);
         servoTest.translateLastRPacket();
+        
+        servoTest.SetGoalPos(1, 2000);
+        servoTest.translateLastRPacket();   //Should return error
         value = servoTest.GetGoalPos(1);
         servoTest.translateLastRPacket();
-        wait(1);            
+        
+        servoTest.SetGoalPos(2, 0);
+        servoTest.translateLastRPacket();
+        value = servoTest.GetGoalPos(2);
+        servoTest.translateLastRPacket();
+        
+        wait(1);          
     }    
 }
\ No newline at end of file