Commits

Trammell Hudson committed 3b6a612

send commands to the robot based on the jog wheel and speed setting

Comments (0)

Files changed (1)

 
 	return controller_move(axis->controller, axis->channel, pos);
 }
-	
+
+
+int
+robot_position(
+	robot_t * const robot,
+	const int axis_num
+)
+{
+	servo_t * const servo = robot_servo(robot, axis_num);
+	if (!servo)
+		return 0;
+
+	return servo->position;
+}
 
 
 static void *
 	if (new_axis)
 		handheld_axis(hh, new_axis);
 
+	if (jog && hh->axis)
+	{
+		int cur_pos = robot_position(hh->robot, hh->axis);
+		int delta = (jog * rate)/32;
+		printf("%d: %d * %+d = %d + %d\n",
+			hh->axis,
+			rate,
+			jog,
+			delta,
+			cur_pos
+		);
+
+		//robot_move(hh->robot, hh->axis, cur_pos + delta);
+	}
+
 	return 0;
 }
 
 	logfile = fopen("/tmp/robot.log", "w");
 	fprintf(logfile, "p1,a1,s1,p2,a2,s2,p3,a3,s3\n");
 
-	for (int i = 1 ; i < argc ; i++)
+	for (int i = 2 ; i < argc ; i++)
 	{
 		const char * const device = argv[i];
 		controller_t * const controller = controller_create(device);
 		return -1;
 	}
 
-	handheld_t * handheld = handheld_create("/dev/tty.usbmodem82", robot);
+	handheld_t * handheld = handheld_create(argv[1], robot);
 	if (handheld)
 	{
 		pthread_t handheld_id;