Commits

Trammell Hudson  committed ff7c611

box test "works"

  • Participants
  • Parent commits 099529f

Comments (0)

Files changed (1)

 	double y
 )
 {
-	int steps = 100;
+	const double phi_1 = atan2(y,x);
+	const double r = sqrt(x*x + y*y);
 
+	const int cmd_1 = phi_1 * robot->servos[1].scale;
+
+	printf("0: %.2f deg %d\n",
+		phi_1 * 180 / M_PI,
+		cmd_1
+	);
+
+	robot_move(robot, 1, cmd_1);
+	robot_ik_test(robot, r, 0);
+	
+
+	return 0;
+}
+
+
+int
+robot_line(
+	robot_t * const robot,
+	const double x1,
+	const double y1,
+	const double x2,
+	const double y2,
+	const int steps,
+	const int delay
+)
+{
 	for (int i = 0 ; i < steps ; i++)
 	{
-		const double r = x + i * (y - x) / steps;
-		robot_ik_test(robot, r, 0);
-		usleep(50000);
+		const double x = x1 + i * (x2 - x1) / steps;
+		const double y = y1 + i * (y2 - y1) / steps;
+
+		robot_test(robot, x, y);
+		usleep(delay);
 	}
 
 	return 0;
 }
 
 
+int
+robot_test_box(
+	robot_t * const robot
+)
+{
+	const double x1 = 150;
+	const double y1 = 150;
+	const double x2 = 300;
+	const double y2 = 300;
+
+	robot_line(robot, x1, y1, x1, y2, 100, 50000);
+	robot_line(robot, x1, y2, x2, y2, 100, 50000);
+	robot_line(robot, x2, y2, x2, y1, 100, 50000);
+	robot_line(robot, x2, y1, x1, y1, 100, 50000);
+
+	return 0;
+}
+
 
 int
 main(
 			continue;
 		}
 
+		if (strcmp(cmd, "box") == 0)
+		{
+			robot_test_box(robot);
+			continue;
+		}
+
+
 		printf("?\n");
 	}
 }