Commits

Trammell Hudson  committed 64c29d9

Add go home and sample position commands

  • Participants
  • Parent commits 040a772

Comments (0)

Files changed (1)

 	usleep(50000);
 	controller_send(controller, "!s 2 4000\r\n");
 	usleep(50000);
-	controller_send(controller, "!ac 1 50000\r\n");
+	controller_send(controller, "!ac 1 40000\r\n");
 	usleep(50000);
-	controller_send(controller, "!dc 1 50000\r\n");
+	controller_send(controller, "!dc 1 40000\r\n");
 	usleep(50000);
-	controller_send(controller, "!ac 2 50000\r\n");
+	controller_send(controller, "!ac 2 40000\r\n");
 	usleep(50000);
-	controller_send(controller, "!dc 2 50000\r\n");
+	controller_send(controller, "!dc 2 40000\r\n");
 	usleep(50000);
 	controller_send(controller, "# C\r\n");
 	usleep(100000);
 				robot_axis_t * const axis = &robot->servos[i+1];
 				if (!axis->controller)
 				{
-					printf(" ?????@????");
+					printf(" ?????");
 					continue;
 				}
 
 				servo_t * const servo = &axis->controller->servo[axis->channel - 1];
-				printf(" %+5d@%+4d",
-					servo->position,
-					servo->speed
+				printf(" %+5d",
+					servo->position
 				);
 			}
 
 			continue;
 		}
 
+		if (line[0] == 's')
+		{
+			// sample the current positions and command them
+			printf("new cmd:");
+			for (int i = 0 ; i < 6; i++)
+			{
+				robot_axis_t * const axis = &robot->servos[i+1];
+				if (!axis->controller)
+				{
+					printf(" ?????");
+					continue;
+				}
+
+				servo_t * const servo = &axis->controller->servo[axis->channel - 1];
+				printf(" %+5d",
+					servo->position
+				);
+
+				robot_move(robot, i+1, servo->position, 1);
+			}
+
+			printf("\n");
+
+			continue;
+		}
+
+		if (line[0] == 'h')
+		{
+			// go home
+			printf("HOME\n");
+			for (int i = 0 ; i < 6; i++)
+				robot_move(robot, i+1, 0, 1);
+
+			continue;
+		}
+
 		int x = 0;
 		int y = 0;
 		int z = 0;