Commits

Trammell Hudson  committed 63c4c85

limit positions out of ik algorithm

  • Participants
  • Parent commits 5b931ef

Comments (0)

Files changed (1)

 	}
 
 	// Axis limits and offset in the home position
-	const double limits[][3] = {
-		{ +12000, -11200, 0 },
-		{ -18000, +16500, -90 },
-		{ +11200, -10500, +90 },
+	const double limits[][5] = {
+		{ +12000, -11200, 0,  -16000, 16000 },
+		{ -18000, +16500, -90, -24000, 24000 },
+		{ +11200, -10500, +90, -18500, 18500 },
 	};
 
+	double commands[3];
+
 	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 (command < lim[3] || command > lim[4])
+		{
+			fprintf(stderr, "%d: %f => %d out of limits! (%f,%f,%f)\n",
+				i,
+				theta[i],
+				(int) command,
+				x,
+				y,
+				z
+			);
+			return 0;
+		}
+
+		commands[i] = command;
+	}
+
+	for (int i = 0 ; i < 3 ; i++)
+	{
 		if (0)
 		printf("command: %d %f => %d\n",
 			i,
 			theta[i] * 180/M_PI,
-			(int) command
+			(int) commands[i]
 		);
 
-		robot_move(robot, i+1, command, 1);
+		robot_move(robot, i+1, commands[i], 1);
 	}
 
 #endif
 	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);
+	robot_line(robot, x1, y1, x1, y2, 250, 50000);
+	robot_line(robot, x1, y2, x2, y2, 500, 50000);
+	robot_line(robot, x2, y2, x2, y1, 250, 50000);
+	robot_line(robot, x2, y1, x1, y1, 500, 50000);
 
 	printf("box done!\n");