Commits

Trammell Hudson committed 5b931ef

direct ik works!

Comments (0)

Files changed (4)

 
 all: servo
 
-servo: servo.o
+servo: servo.o ik.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
 fk: fk.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
-ik: ik.o
+ik: ik-main.o ik.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
 servo.o: servo.c
 130mm total height
 
 
-Axis 1:
--11200 == -90 deg
-+12000 == 90  deg
+Axis 1: (positive rotation ccw viewed from above)
+-11200 == +90 deg
++12000 == -90  deg
 = 128.8 ticks/deg
 
 Axis 2:
 	const double r = sqrt(px*px + py*py - d2*d2);
 	const double R = sqrt(px*px + py*py + pz*pz - d2*d2);
 
-	printf("r=%f R=%f\n", r, R);
+	//printf("r=%f R=%f\n", r, R);
 
 	// theta[0] defined in equation 26
 	theta[0] = atan2(
 		-right * py * r - px * d2,
-		-right * pz * r + py * d2
+		-right * px * r + py * d2
 	);
 
 	// theta[1] is equations 28 - 35.
 }
 
 
+#if 0
 int main(int argc, char **argv)
 {
 	if (argc != 4)
 
 	return 0;
 }
+#endif
 #include <ctype.h>
 #include <pthread.h>
 #include <math.h>
+#include "ik.h"
 
 typedef struct _servo_t servo_t;
 typedef struct _controller_t controller_t;
 	if (len > sizeof(buf))
 		len = sizeof(buf);
 
-	fprintf(stdout, "sending: '%s'\n", buf);
+	//fprintf(stdout, "sending: '%s'\n", buf);
 
 	size_t offset = 0;
 	while (offset < len)
 	servo_t * const servo = &controller->servo[channel];
 	servo->transiting = 1;
 
-	printf("%s: channel %d pos %d\n", controller->device, channel, pos);
+	//printf("%s: channel %d pos %d\n", controller->device, channel, pos);
 	controller_send(controller,
 		"!P%s %d %d\r\n",
 		absolute ? "" : "R",
 robot_ik_test(
 	robot_t * const robot,
 	double x,
-	double y
+	double y,
+	double z
 )
 {
+#if 0
 	// 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
 
 	robot_move(robot, 2, cmd_2, 1);
 	robot_move(robot, 3, cmd_3, 1);
+#else
+	const double xyz[] = { x, y, z };
+	double theta[6];
+
+	if (!ik_first(theta, xyz, 1, 1))
+	{
+		printf("unreachable\n");
+		return 0;
+	}
+
+	// Axis limits and offset in the home position
+	const double limits[][3] = {
+		{ +12000, -11200, 0 },
+		{ -18000, +16500, -90 },
+		{ +11200, -10500, +90 },
+	};
+
+	for (int i = 0 ; i < 3 ; i++)
+	{
+		const double * const lim = limits[i];
+		double command = theta[i] - lim[2] * M_PI/180;
+		command = command * (lim[1] - lim[0]) / M_PI;
+
+		if (0)
+		printf("command: %d %f => %d\n",
+			i,
+			theta[i] * 180/M_PI,
+			(int) command
+		);
+
+		robot_move(robot, i+1, command, 1);
+	}
+
+#endif
 
 	return 0;
 }
 	double y
 )
 {
+#if 0
 	const double phi_1 = atan2(y,x);
 	const double r = sqrt(x*x + y*y);
 
 
 	robot_move(robot, 1, cmd_1, 1);
 	robot_ik_test(robot, r, 0);
-	
-
+#endif
+	robot_ik_test(robot, x, y, 0);
 	return 0;
 }
 
 		const double x = x1 + i * (x2 - x1) / steps;
 		const double y = y1 + i * (y2 - y1) / steps;
 
-		robot_test(robot, x, y);
+		robot_ik_test(robot, x, y, 0);
 		usleep(delay);
 	}
 
 	robot_t * const robot
 )
 {
-	const double x1 = 150;
-	const double y1 = 150;
-	const double x2 = 250;
-	const double y2 = 250;
+	const double x1 = -200;
+	const double x2 =  200;
 
-	robot_line(robot, x1, y1, x1, y2, 250, 10000);
-	robot_line(robot, x1, y2, x2, y2, 250, 10000);
-	robot_line(robot, x2, y2, x2, y1, 250, 10000);
-	robot_line(robot, x2, y1, x1, y1, 250, 10000);
+	const double y1 =  200;
+	const double y2 =  300;
+
+	robot_line(robot, x1, y1, x1, y2, 250, 20000);
+	robot_line(robot, x1, y2, x2, y2, 500, 20000);
+	robot_line(robot, x2, y2, x2, y1, 250, 20000);
+	robot_line(robot, x2, y1, x1, y1, 500, 20000);
+
+	printf("box done!\n");
 
 	return 0;
 }