Optionally allow #POM to return XYZ from end of Link 5

#POM and #XYZ look at the point above the J5 axis (point 4) and NOT the tool tip position (point 5). This change adds an optional extra parameter which will select which point to use. Valid values are '4' or '5' with the default, if not specified or if any other value is used, remaining as '4'. e.g. to get the position orientation matrix from point 5, use 
r 0 #POM 5;

See also:
https://github.com/HaddingtonDynamics/Dexter/wiki/read-from-robot#keywords

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);