Commits

Trammell Hudson committed 7121758

two axis ik works

  • Participants
  • Parent commits c117509

Comments (0)

Files changed (1)

 #include <errno.h>
 #include <ctype.h>
 #include <pthread.h>
+#include <math.h>
 
 typedef struct _servo_t servo_t;
 typedef struct _controller_t controller_t;
 typedef struct _robot_t robot_t;
 
 
+int verbose = 0;
+
 controller_t *
 controller_create(
 	const char * device
 typedef struct {
 	controller_t * controller;
 	int channel;
+	double scale; // counts/radians
+	double length; // mm of bone
+	double shift; // mm of offset from previous joint
 } robot_axis_t;
 
+
 struct _robot_t
 {
 	int controller_count;
 		return 0;
 	}
 
-	//printf("type='%s' x=%d y=%d\n", type, x, y);
+	if (verbose)
+		printf("%s: type='%s' x=%d y=%d\n", controller->device, type, x, y);
 
 	if (strcmp(type, "ACTR") == 0)
 	{
 	controller_t * const controller
 )
 {
+	controller_send(controller, "~ACTR\r\n");
+	controller_send(controller, "^CLERD 1 0\r\n");
+	controller_send(controller, "^CLERD 2 0\r\n");
 	controller_send(controller, "# C\r\n");
-	controller_send(controller, "~ACTR\r\n");
 	controller_send(controller, "?C\r\n");
 	controller_send(controller, "?M\r\n");
 	controller_send(controller, "?A\r\n");
 }
 
 
+int
+robot_test(
+	robot_t * const robot,
+	double x,
+	double y
+)
+{
+	// test moving the arm to position (x,y) using the first three axis
+	// but first just do the x position, assuming facing forwards
+	// This is solving a side-side-side triangle with the three
+	// sides the given x position and the length of joint 2 and 3.
+	// angle for joint 2 is: r_3^2 = r_2^2 + x^2 - 2 r_2 x cos (90 - Phi_2)
+	// angle for joint 3 is: x^2 = r_2^2 + 3^2 - 2 r_2 r_3 cos (180 - Phi_3)
+
+	const double r_1 = robot->servos[1].length;
+	const double r_2 = robot->servos[2].length;
+	const double r_3 = robot->servos[3].length;
+
+	// if x is > r_2 + r_3, then we can't reach that point
+	if (r_2 + r_3 < x
+	||  r_2 + x < r_3
+	||  r_3 + x < r_2)
+	{
+		printf("%.2f is unreachable\n", x);
+		return - 1;
+	}
+
+	double phi_2 = acos((r_2*r_2 + x*x - r_3*r_3) / (2 * r_2 * x));
+	double phi_3 = acos((r_2*r_2 + r_3*r_3 - x*x) / (2 * r_2 * r_3));
+
+	// Adjust for joint orientations
+	phi_2 = M_PI/2 - phi_2;
+	phi_3 = M_PI - phi_3;
+
+	// translate to a command
+	const int cmd_2 = phi_2 * robot->servos[2].scale;
+	const int cmd_3 = phi_3 * robot->servos[3].scale;
+
+	printf("angles %.2f %.2f: %d %d\n",
+		phi_2 * 180 / M_PI,
+		phi_3 * 180 / M_PI,
+		cmd_2,
+		cmd_3
+	);
+
+	robot_move(robot, 2, cmd_2);
+	robot_move(robot, 3, cmd_3);
+
+	return 0;
+}
+
 
 
 int
 {
 	robot_t * const robot = robot_create();
 
+	robot->servos[1].scale = (12000 - -11200) / M_PI;
+	robot->servos[1].length = 300;
+
+	robot->servos[2].scale = (16500 - -18000) / M_PI;
+	robot->servos[2].length = 210;
+
+	robot->servos[3].scale = -(10500 - -11200) / M_PI;
+	robot->servos[3].length = 260;
+
 	for (int i = 1 ; i < argc ; i++)
 	{
 		const char * const device = argv[i];
 					continue;
 				}
 
-				servo_t * const servo = &axis->controller->servo[axis->channel];
+				servo_t * const servo = &axis->controller->servo[axis->channel - 1];
 				printf(" %+5d@%+4d",
 					servo->position,
 					servo->speed
 			continue;
 		}
 
-		int axis = 0;
-		int pos = 0;
-		if (sscanf(line, "%d %d", &axis, &pos) == 2)
+		int x = 0;
+		int y = 0;
+		char cmd[32];
+
+		if (sscanf(line, "%16[^ ] %d %d", cmd, &x, &y) != 3)
 		{
-			robot_move(robot, axis, pos);
+			printf("?\n");
+			continue;
+		}
+
+		if (strcmp(cmd, "v") == 0)
+		{
+			verbose = !verbose;
+			continue;
+		}
+
+		if (strcmp(cmd, "p") == 0)
+		{
+			robot_move(robot, x, y);
+			continue;
+		}
+
+		if (strcmp(cmd, "r") == 0)
+		{
+			robot_test(robot, x, y);
 			continue;
 		}