Commits

Trammell Hudson  committed f7d87ae

add linear movement. not great, but a start

  • Participants
  • Parent commits ec96604

Comments (0)

Files changed (1)

 int verbose = 0;
 FILE * logfile;
 
+static double last_position[3];
+
 controller_t *
 controller_create(
 	const char * device
 		robot_move(robot, i+1, commands[i], 1);
 	}
 
+	// Copy the xyz into the position vector for later iteration
+	for (int i = 0 ; i < 3 ; i++)
+		last_position[i] = xyz[i];
 #endif
 
 	return 0;
 }
 
 
+int
+robot_linear(
+	robot_t * const robot,
+	const double x2,
+	const double y2,
+	const double z2,
+	const int steps,
+	const int delay
+)
+{
+	const double x1 = last_position[0];
+	const double y1 = last_position[1];
+	const double z1 = last_position[2];
+
+	if (x1 == 0 && y1 == 0 && y2 == 0)
+		return robot_test(robot, x2, y2, z2);
+
+	for (int i = 0 ; i < steps ; i++)
+	{
+		const double x = x1 + i * (x2 - x1) / steps;
+		const double y = y1 + i * (y2 - y1) / steps;
+		const double z = z1 + i * (z2 - z1) / steps;
+
+		robot_ik_test(robot, x, y, z);
+		usleep(delay);
+	}
+
+	return 0;
+}
+
+
 typedef struct
 {
 	robot_t * robot;
 			continue;
 		}
 
+		if (strcmp(cmd, "l") == 0)
+		{
+			robot_linear(robot, x, y, z, 100, 50000);
+			continue;
+		}
+
 		if (strcmp(cmd, "box") == 0)
 		{
 			robot_test_box(robot);