Esempio n. 1
0
int main()
{

  printf( "\n===== Read/Write Tutorial for DARwIn =====\n\n");
  
  //////////////////// Framework Initialize ////////////////////////////
  // LinuxCM730 linux_cm730("/dev/ttyUSB0");
  // CM730 cm730(&linux_cm730);
  if(cm730.Connect() == false)
  {
    printf("Fail to connect CM-730!\n");
    return 0;
  }
  
  /////////////////////////////////////////////////////////////////////
  
  cm730.WriteWord(JointData::ID_R_SHOULDER_PITCH, MX28::P_TORQUE_ENABLE, 0, 0);
  cm730.WriteWord(JointData::ID_R_SHOULDER_ROLL,  MX28::P_TORQUE_ENABLE, 0, 0);
  cm730.WriteWord(JointData::ID_R_ELBOW,          MX28::P_TORQUE_ENABLE, 0, 0);
  
  cm730.WriteWord(JointData::ID_L_SHOULDER_PITCH, MX28::P_TORQUE_ENABLE, 0, 0);
  cm730.WriteWord(JointData::ID_L_SHOULDER_ROLL,  MX28::P_TORQUE_ENABLE, 0, 0);
  cm730.WriteWord(JointData::ID_L_ELBOW,          MX28::P_TORQUE_ENABLE, 0, 0);

  cm730.WriteWord(JointData::ID_R_HIP_ROLL,  MX28::P_TORQUE_ENABLE, 0, 0);
  cm730.WriteWord(JointData::ID_L_HIP_ROLL,     MX28::P_TORQUE_ENABLE, 0, 0);
  
  int i;
  
  // Read current positions of upper body
  int body[8] = {0,0,0,0,0,0,0,0};

  getCurrentRightArmPos(body);
  getCurrentLeftArmPos(body + 3);
  getCurrentPitchPos(body + 6);
  usleep(SIM_TIME_U);
  gotoInitPos(body,100);

  /*
  for (i = 0; i<6; i++)
    printf("ID %d - %d\n", i, arms[i]);

  int len = (sizeof(arms) / sizeof(*arms));
  printf("%d\n", len);
  */

  getchar();

  hoseHandling();

  usleep(20000);
}
Esempio n. 2
0
int main()
{
	signal(SIGABRT, &sighandler);
    signal(SIGTERM, &sighandler);
	signal(SIGQUIT, &sighandler);
	signal(SIGINT, &sighandler);

	char input[MAXNUM_INPUTCHAR];
	char *token;
	int input_len;
	char cmd[80];
	char param[20][30];
	int num_param;



	printf( "\n[Dynamixel Monitor for DARwIn %s]\n", PROGRAM_VERSION);

	if(cm730.Connect() == true)
	{
		while(1)
		{
			gets(input);
			fflush(stdin);
			input_len = strlen(input);
			if(input_len == 0)
				continue;
			token = strtok( input, " " );
			if(token == 0)
				continue;
			strcpy( cmd, token );
			token = strtok( 0, " " );
			num_param = 0;
			while(token != 0)
			{
				strcpy( param[num_param++], token );
				token = strtok( 0, " " );
			}
			if(strcmp(cmd, "d") == 0)
				compass_Gyro(&cm730, gID);
		}
	}
	printf("\nTerminated DXL Manager.\n");
	return 0;
}
Esempio n. 3
0
int main()
{
	signal(SIGABRT, &sighandler);
	signal(SIGTERM, &sighandler);
	signal(SIGQUIT, &sighandler);
	signal(SIGINT, &sighandler);

	char input[MAXNUM_INPUTCHAR];
	char *token;
	int input_len;
	char cmd[80];
	char param[20][30];
	int num_param;
	int iparam[20];	

	printf( "\n[Dynamixel Monitor for DARwIn %s]\n", PROGRAM_VERSION);

	if(cm730.Connect() == true)
	{
		Scan(&cm730);

		while(1)
		{
			Prompt(gID);
			gets(input);
			fflush(stdin);
			input_len = strlen(input);
			if(input_len == 0)
				continue;

			token = strtok( input, " " );
			if(token == 0)
				continue;

			strcpy( cmd, token );
			token = strtok( 0, " " );
			num_param = 0;
			while(token != 0)
			{
				strcpy( param[num_param++], token );
				token = strtok( 0, " " );
			}

			if(strcmp(cmd, "exit") == 0)
				break;
			else if(strcmp(cmd, "scan") == 0)
				Scan(&cm730);
			else if(strcmp(cmd, "help") == 0)
				Help();
			else if(strcmp(cmd, "id") == 0)
			{
				if(num_param != 1)
				{
					printf(" Invalid parameter!\n");
					continue;
				}
				
				iparam[0] = atoi(param[0]);
				if(cm730.Ping(iparam[0], 0) == CM730::SUCCESS)
				{
					gID = iparam[0];
				}
				else
				{
					printf(" Invalid ID(%d)!\n", iparam[0]);
					continue;
				}
			}
			else if(strcmp(cmd, "on") == 0)
			{
				if(num_param == 0)
				{
					cm730.WriteByte(gID, MX28::P_TORQUE_ENABLE, 1, 0);
					if(gID == CM730::ID_CM)
						printf(" Dynamixel power on\n");
				}
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							cm730.WriteByte(i, MX28::P_TORQUE_ENABLE, 1, 0);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "off") == 0)
			{
				if(num_param == 0)
				{
					cm730.WriteByte(gID, MX28::P_TORQUE_ENABLE, 0, 0);
					if(gID == CM730::ID_CM)
						printf(" Dynamixel power off\n");
				}
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							cm730.WriteByte(i, MX28::P_TORQUE_ENABLE, 0, 0);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "d") == 0)
				Dump(&cm730, gID);
			else if(strcmp(cmd, "reset") == 0)
			{
				int firm_ver = 0;
				if(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0)  != CM730::SUCCESS)
				{
					fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN);
					exit(0);
				}

				if(0 < firm_ver && firm_ver < 27)
				{
					fprintf(stderr, "\n MX-28's firmware is not support 4096 resolution!! \n");
					fprintf(stderr, " Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
					continue;
				}

				if(num_param == 0)
					Reset(&cm730, gID);
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							Reset(&cm730, i);

						if(cm730.Ping(FSR::ID_R_FSR, 0) == CM730::SUCCESS)
							Reset(&cm730, FSR::ID_R_FSR);
						if(cm730.Ping(FSR::ID_L_FSR, 0) == CM730::SUCCESS)
							Reset(&cm730, FSR::ID_L_FSR);
						Reset(&cm730, CM730::ID_CM);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "wr") == 0)
			{
				if(num_param == 2)
					Write(&cm730, gID, atoi(param[0]), atoi(param[1]));
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else
				printf(" Bad command! please input 'help'.\n");
		}
	}
	else
		printf("Failed to connect CM-730!");

	printf("\nTerminated DXL Manager.\n");
	return 0;
}