diff --git a/Firmware/DexRun.c b/Firmware/DexRun.c
index 83d56e7..04e0e33 100644
--- a/Firmware/DexRun.c
+++ b/Firmware/DexRun.c
@@ -1276,8 +1276,7 @@ void pos_ori_mat_to_string(struct pos_ori_mat A, char *result){
 	printf("done with pos_ori_mat_to_string\n");
 };
 
-struct pos_ori_mat J_angles_to_pos_ori_mat(struct J_angles angles) {
-	
+struct pos_ori_mat J_angles_to_pos_ori_mat(struct J_angles angles, unsigned char end_point) {	
 	
 	
 	//Code from Forward Kinematics:
@@ -1375,6 +1374,12 @@ struct pos_ori_mat J_angles_to_pos_ori_mat(struct J_angles angles) {
 	result.r1.c3 = U4.y;
 	result.r2.c3 = U4.z;
 	
+	if (5 == end_point) {
+		result.r0.c3 = U5.x;
+		result.r1.c3 = U5.y;
+		result.r2.c3 = U5.z;
+		}
+	
 	//printf("\nResult 3 complete\n");
 	
 	result.r3.c0 = 0;
@@ -2989,7 +2994,9 @@ bool ProcessServerSendDataDDE(char *sendBuff,char *recBuff)
 						(float)(getNormalizedInput(ANGLE_MEASURED_ANGLE)),
 						(float)(getNormalizedInput(ROT_MEASURED_ANGLE))
 					);
-					struct pos_ori_mat A = J_angles_to_pos_ori_mat(measured_angles);
+					unsigned char end_point = 4;
+					token = strtok(NULL, delimiters); if (token) end_point = token[0]-'0'; //which point? Single digit 4 or 5
+					struct pos_ori_mat A = J_angles_to_pos_ori_mat(measured_angles, end_point);
 
 					//pos_ori_mat_to_string(measured_pos_ori_mat, mat_string);