--- robotserver.c.orig	2017-09-05 14:52:40.132575160 +0200
+++ robotserver.c	2017-09-05 14:51:26.906666815 +0200
@@ -28,6 +28,7 @@
 void SendStatus(void);
 void DecodeRobotReply(void);
 void InitPositions(void);
+int InterpolatePosition(int nb, int *args);
 char *NextPosition(int nb, char *buf, int *args, float zoff);
 char *ParkPosition(int nb, char *buf, int *args);
 char *TesterPosition(int nb,char *buf, int stay);
@@ -45,16 +46,19 @@
 void Push(void);
 
 /* --------------- Type definitions -----------------------------------------*/
-typedef struct Trays {
-	float x;
-	float y;
-	float z;
-	float dx;
-	float dy;
-	float xoff;
-	float yoff;
-	int   columns;
-	int   rows;
+typedef struct Trays { /* x,y,z positions for each of the 4 corners */
+  float xFirstRowFirstCol;
+  float yFirstRowFirstCol;
+  float zFirstRowFirstCol;
+  float xFirstRowLastCol;
+  float yFirstRowLastCol;
+  float zFirstRowLastCol;
+  float xLastRowFirstCol;
+  float yLastRowFirstCol;
+  float zLastRowFirstCol;
+  float xLastRowLastCol;
+  float yLastRowLastCol;
+  float zLastRowLastCol;
 } TRAYS;
 
 typedef struct Testers {
@@ -139,6 +143,10 @@
 /* Trays and testers that we have */
 #define TRAY_FIRST       1
 #define TRAY_LAST        8
+#define ROW_FIRST        1
+#define ROW_LAST        18
+#define COL_FIRST        1
+#define COL_LAST         7
 #define TESTER_FIRST     1
 #define TESTER_LAST      4
 
@@ -175,6 +183,7 @@
 int  statusRobot = STATUS_IDLE;
 int  commandRemote = 0;
 int  moveArguments[7];
+float moveCoords[3];
 int  fetchArguments[5];
 int  AllowedHost(char *ip);
 
@@ -581,50 +590,86 @@
 	ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
 }
 
+int InterpolatePosition(int nb, int *args)
+{
+  int   traynb, row, col;
+  float mult;
+  float x, y, z;
+
+  traynb = args[nb];
+  col = args[nb+1];
+  row = args[nb+2];
+
+  if ((traynb >= TRAY_FIRST) && (traynb <= TRAY_LAST)) {
+    traynb--;
+    if ((row >= ROW_FIRST) && (row <= ROW_LAST) &&
+        (col >= COL_FIRST) && (col <= COL_LAST)) {
+      mult = 1.0 / ( (COL_LAST - COL_FIRST) * (ROW_LAST - ROW_FIRST) );
+      
+      x = ( tray[traynb].xFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
+            tray[traynb].xFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
+            tray[traynb].xLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
+            tray[traynb].xLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;
+
+      y = ( tray[traynb].yFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
+            tray[traynb].yFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
+            tray[traynb].yLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
+            tray[traynb].yLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;
+
+      z = ( tray[traynb].zFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
+            tray[traynb].zFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
+            tray[traynb].zLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
+            tray[traynb].zLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;
+
+      moveCoords[0] = x;
+      moveCoords[1] = y;
+      moveCoords[2] = z;
+
+      return(1); // OK                                                                                   
+    }
+  }
+
+  return(0); // not OK                                                                                   
+}
+
 char *NextPosition(int nb, char *buf, int *args, float zoff)
 {
-int traynb,row,col;
-float x, y, z, rot;
-	traynb = args[nb];
-	col = args[nb+1];
-	row = args[nb+2];
-	if ((traynb >= TRAY_FIRST) && (traynb <= TRAY_LAST)) {
-		traynb--;
-		if ((row >= 1) && (row <= tray[traynb].rows) &&
-			(col >= 1) && (col <= tray[traynb].columns)) {
-			x = tray[traynb].x + tray[traynb].xoff + tray[traynb].dx*(col-1);
-			y = tray[traynb].y - tray[traynb].yoff - tray[traynb].dy*(row-1);
-			z = tray[traynb].z + zoff;
-			rot = 0.0;
-			sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
-			"@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
-			return(buf);
-		}
-	}
-	return(NULL);
+  int result;
+  float x, y, z, rot;
+  
+  result = InterpolatePosition(nb, args);
+  if (result) {
+    x = moveCoords[0];
+    y = moveCoords[1];
+    z = moveCoords[2];
+    
+    rot = 0.0;
+    sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
+	    "@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
+    return(buf);
+  }
+  return(NULL);
 }
 
 char *ParkPosition(int nb, char *buf, int *args)
 {
-int   traynb, row, col;
-float x, y, z, rot;
-	traynb = args[nb];
-	col = args[nb+1];
-	row = args[nb+2];
-	if ((traynb >= TRAY_FIRST) && (traynb <= TRAY_LAST)) {
-		traynb--;
-		if ((row >= 1) && (row <= tray[traynb].rows) &&
-			(col >= 1) && (col <= tray[traynb].columns)) {
-			x = tray[traynb].x + tray[traynb].xoff + tray[traynb].dx*(col-1);
-			y = tray[traynb].y - tray[traynb].yoff - tray[traynb].dy*(row-1);
-			z = Z0;
-			rot = 0.0;
-			sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
-			"@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
-			return(buf);
-		}
-	}
-	return(NULL);
+  int result;
+  float x, y, z, rot;
+  
+  result = InterpolatePosition(nb, args);
+  if (result) {
+    x = moveCoords[0];
+    y = moveCoords[1];
+    
+    z = Z0;
+    rot = 0.0;
+    
+    sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
+	    "@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
+    return(buf);
+  }
+  
+  return(NULL);
 }
 
 /* Set the hand (lefty or righty) for a position on a tray */
@@ -1543,44 +1588,74 @@
 /* Read tray and tester position information from file */
 void InitPositions(void)
 {
-FILE *fpos;
-char buf[512];
-int traynb,cols,rows;
-float x, y, z,  dx, dy, xoff, yoff;
-float Xcomp = 0.00, Ycomp = 0.00;
-	if (fpos = fopen("positions.txt", "r")) {
-		while(fgets (buf, 510, fpos)) {
-			if (buf[0] != '#') {
-				if (sscanf(buf,"%d %f %f %f %d %d  %f %f %f %f",
-					&traynb, &x, &y, &z, &cols, &rows, &dx, &dy, &xoff, &yoff) == 10) {
-					
-					if (traynb < 10) {
-						traynb--;
-						tray[traynb].x = x + Xcomp;
-						tray[traynb].y = y + Ycomp;
-						tray[traynb].z = z;
-						tray[traynb].dx = dx;
-						tray[traynb].dy = dy;
-						tray[traynb].columns = cols;
-						tray[traynb].rows = rows;
-						tray[traynb].xoff = xoff;
-						tray[traynb].yoff = yoff;
-					}
-					else if ((traynb >= 10) &&  (traynb <= 13)) {
-						traynb -= 10;
-						tester[traynb].x = x + Xcomp;
-						tester[traynb].y = y + Ycomp;
-						tester[traynb].z = z;
-						tester[traynb].rot = cols;
-						tester[traynb].z1 = dx;
-						tester[traynb].z2 = dy;
-						tester[traynb].z3 = xoff;
-					}
-				}
-			}
-		}
-		fclose(fpos);
-	}
+  FILE *fpos;
+  char buf[512];
+  int traynb,cols,rows;
+  float x, y, z;
+  int testernb;
+  float rot, z1, z2, z3;
+
+  fpos = fopen("positions_trays.txt", "r");
+  if (fpos) {
+    while(fgets (buf, 510, fpos)) {
+      if (buf[0] != '#') {
+        if (sscanf(buf,"%d %d %d %f %f %f",
+                   &traynb, &rows, &cols, &x, &y, &z) == 6) {
+
+          if (traynb < 10) {
+            traynb--;
+
+            if ( (cols == COL_FIRST) && (rows == ROW_FIRST) ) {
+              tray[traynb].xFirstRowFirstCol = x;
+              tray[traynb].yFirstRowFirstCol = y;
+              tray[traynb].zFirstRowFirstCol = z;
+            }
+            else if ( (cols == COL_LAST) && (rows == ROW_FIRST) ) {
+              tray[traynb].xFirstRowLastCol = x;
+              tray[traynb].yFirstRowLastCol = y;
+              tray[traynb].zFirstRowLastCol = z;
+            }
+            else if ( (cols == COL_FIRST) && (rows == ROW_LAST) ) {
+              tray[traynb].xLastRowFirstCol = x;
+              tray[traynb].yLastRowFirstCol = y;
+              tray[traynb].zLastRowFirstCol = z;
+            }
+            else if ( (cols == COL_LAST) && (rows == ROW_LAST) ) {
+              tray[traynb].xLastRowLastCol = x;
+              tray[traynb].yLastRowLastCol = y;
+              tray[traynb].zLastRowLastCol = z;
+            }
+
+          }
+        }
+      }
+    }
+    fclose(fpos);
+  }
+
+  fpos = fopen("positions_tester.txt", "r");
+  if (fpos) {
+    while(fgets (buf, 510, fpos)) {
+      if (buf[0] != '#') {
+        if (sscanf(buf,"%d %f %f %f %f %f %f %f",
+                   &testernb, &x, &y, &z, &rot, &z1, &z2, &z3) == 8) {
+
+          if ((testernb >= 10) &&  (testernb <= 13)) {
+            testernb -= 10;
+            tester[testernb].x = x;
+            tester[testernb].y = y;
+            tester[testernb].z = z;
+            tester[testernb].rot = rot;
+            tester[testernb].z1 = z1;
+            tester[testernb].z2 = z2;
+            tester[testernb].z3 = z3;
+          }
+        }
+      }
+    }
+    fclose(fpos);
+  }
+  return;
 }	/* End of InitPositions */
 
 void InitPanel(void)