switch to sending absolute coordinates to the V+ code

this works around issue #2, since when the serial line fails the V+ routines read in (0, 0, 0) which is now out of the arm's reach.

also, clean up the output and fix sign error for the clockwise/anticlockwise arc.

cmd/gdmux/dmux.go

@@ -50,7 +50,7 @@ func (c *Cmd) AddOp(code gcode.Code) {
 		// TODO(s): I don't like how this is done, need to rethink this package...
 		c.ops = append(c.ops, func(c *Cmd) {
 			weblog(fmt.Sprintf("Move %8.2f %8.2f %8.2f", c.env['X'], c.env['Y'], c.env['Z']))
-			err := arm.Move(c.env['X'], c.env['Y'], c.env['Z'])
+			err := arm.Move(c.env['X']+c.zero.x, c.env['Y']+c.zero.y, c.env['Z']+c.zero.z)
 			if err != nil {
 				weblog(fmt.Sprintf(" → %s\n", err))
 				return
@@ -60,7 +60,7 @@ func (c *Cmd) AddOp(code gcode.Code) {
 	case "G1":
 		c.ops = append(c.ops, func(c *Cmd) {
 			weblog(fmt.Sprintf("Line %8.2f %8.2f %8.2f", c.env['X'], c.env['Y'], c.env['Z']))
-			err := arm.MoveStraight(c.env['X'], c.env['Y'], c.env['Z'])
+			err := arm.MoveStraight(c.env['X']+c.zero.x, c.env['Y']+c.zero.y, c.env['Z']+c.zero.z)
 			if err != nil {
 				weblog(fmt.Sprintf(" → %s\n", err))
 				return
@@ -79,7 +79,8 @@ func (c *Cmd) AddOp(code gcode.Code) {
 		c.ops = append(c.ops, func(c *Cmd) {
 			weblog(fmt.Sprintf("Clockwise Arc to %8.2f %8.2f %8.2f, around %8.2f %8.2f %8.2f", c.env['X'], c.env['Y'], c.env['Z'], c.env['I'], c.env['J'], c.env['K']))
 			// TODO add a step argument here and use negative to go anti-clockwise.
-			err := arm.ArcCenter(c.env['X'], c.env['Y'], c.env['Z'], c.env['I'], c.env['J'], c.env['K'], staubli.Clockwise)
+			err := arm.ArcCenter(c.env['X']+c.zero.x, c.env['Y']+c.zero.y, c.env['Z']+c.zero.z,
+				c.env['I']+c.zero.x, c.env['J']+c.zero.y, c.env['K']+c.zero.z, staubli.Clockwise)
 			if err != nil {
 				weblog(fmt.Sprintf(" → %s\n", err))
 				return
@@ -91,7 +92,8 @@ func (c *Cmd) AddOp(code gcode.Code) {
 		c.ops = append(c.ops, func(c *Cmd) {
 			weblog(fmt.Sprintf("Anti-clockwise Arc to %8.2f %8.2f %8.2f, around %8.2f %8.2f %8.2f", c.env['X'], c.env['Y'], c.env['Z'], c.env['I'], c.env['J'], c.env['K']))
 			// TODO add a step argument here and use negative to go anti-clockwise.
-			err := arm.ArcCenter(c.env['X'], c.env['Y'], c.env['Z'], c.env['I'], c.env['J'], c.env['K'], staubli.Anticlockwise)
+			err := arm.ArcCenter(c.env['X']+c.zero.x, c.env['Y']+c.zero.y, c.env['Z']+c.zero.z,
+				c.env['I']+c.zero.x, c.env['J']+c.zero.y, c.env['K']+c.zero.z, staubli.Anticlockwise)
 			if err != nil {
 				weblog(fmt.Sprintf(" → %s\n", err))
 				return
@@ -111,7 +113,10 @@ func (c *Cmd) AddOp(code gcode.Code) {
 
 func dmux(read io.Reader) {
 	r := gcode.NewParser(read)
-	cmd := Cmd{}
+	cmd := Cmd{
+		env:  make(map[byte]float64),
+		zero: point{x: 500, y: 0, z: -100},
+	}
 	n := 1
 	for {
 		var err error

pkg/staubli/V+/gcode.pg

@@ -16,14 +16,14 @@ SET zeroloc = TRANS(500,0,0,0,90,0)
 ABOVE
 IF INRANGE(zeroloc) == 0 THEN
 	MOVE zeroloc
+	WRITE (slun) "Ready"
 ELSE
 	READY
 	DRIVE 2, 45, 100
+	WRITE (slun) "Ready"
 END
 ABOVE
 
-cpon
-
 WHILE TRUE DO
 	READ (slun) op, a, b, c, d, e, f, g
 	IF IOSTAT(slun) < 0 THEN
@@ -35,9 +35,9 @@ WHILE TRUE DO
 	CASE op OF
 	VALUE 0:
 		; quick move
-		x = a+500
+		x = a
 		y = b
-		z = c-131
+		z = c
 		SET loc = TRANS(x,y,z,0,90,0)
 
 		TYPE "move ", x, ",", y, ",", z
@@ -51,9 +51,9 @@ WHILE TRUE DO
 		END
 	VALUE 1:
 		; straight line
-		x = a+500
+		x = a
 		y = b
-		z = c-131
+		z = c
 		SET loc = TRANS(x,y,z,0,90,0)
 
 		TYPE "line ", x, ",", y, ",", z
@@ -67,13 +67,13 @@ WHILE TRUE DO
 		END
 	VALUE 2:
 		; arc
-		target.x = a+500
+		target.x = a
 		target.y = b
-		target.z = c-131
+		target.z = c
 
-		center.x = d+500
+		center.x = d
 		center.y = e
-		center.z = f-131
+		center.z = f
 
 		starta = atan2(y - center.y,x - center.x)
 

pkg/staubli/staubli.go

@@ -24,7 +24,7 @@ type Staubli struct {
 // Move the arm to the point (x,y,z), without guaranteeing a staight line.
 func (s *Staubli) Move(x, y, z float64) error {
 	// we probably need a lock here...
-	_, err := fmt.Fprintf(s.rw, "0 %f %f %f\r\n", x, y, z)
+	_, err := fmt.Fprintf(s.rw, "0 %.3f %.3f %.3f\r\n", x, y, z)
 	if err != nil {
 		return fmt.Errorf("error sending coordinates to arm: %s", err)
 	}
@@ -37,7 +37,7 @@ func (s *Staubli) Move(x, y, z float64) error {
 
 // Move the arm to the point (x,y,z) in a straight line.
 func (s *Staubli) MoveStraight(x, y, z float64) error {
-	_, err := fmt.Fprintf(s.rw, "1 %f %f %f\r\n", x, y, z)
+	_, err := fmt.Fprintf(s.rw, "1 %.3f %.3f %.3f\r\n", x, y, z)
 	if err != nil {
 		return fmt.Errorf("error sending coordinates to arm: %s", err)
 	}
@@ -49,16 +49,16 @@ func (s *Staubli) MoveStraight(x, y, z float64) error {
 }
 
 const (
-	Clockwise     = 0.5
-	Anticlockwise = -0.5
+	Clockwise     = -1
+	Anticlockwise = 1
 )
 
 // Move the arm to the point (x,y,z) following the path of an arc whose centre is at (i,j,k).
 //
 // The distance between the current position and (i,j,k) must equal that between (x,y,z) and (i,j,k).
 // This method is likely to be removed.
 func (s *Staubli) ArcCenter(x, y, z, i, j, k, direction float64) error {
-	_, err := fmt.Fprintf(s.rw, "2 %f %f %f %f %f %f %f\r\n", x, y, z, i, j, k, direction)
+	_, err := fmt.Fprintf(s.rw, "2 %.3f %.3f %.3f %.3f %.3f %.3f %.3f\r\n", x, y, z, i, j, k, direction)
 	if err != nil {
 		return fmt.Errorf("error sending coordinates to arm: %s", err)
 	}