Exemplo n.º 1
0
void getCurrentPitchPos(int pitch[]) {
  int value;
  if (cm730.ReadWord(JointData::ID_R_HIP_ROLL, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
  {
    pitch[0] = value;
  }
  
  if (cm730.ReadWord(JointData::ID_L_HIP_ROLL, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
  {
    pitch[1] = value;
  }
}
Exemplo n.º 2
0
int GetMotorValue(int id) {
	int value;
	cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &value, 0);
	// printf("Value of id %d is %d\n", id, value);
	return value;
	// return Action::GetInstance()->m_Joint.GetValue(id);
	// return MotionStatus::m_CurrentJoints.GetValue(id);
}
Exemplo n.º 3
0
void getCurrentLeftArmPos(int leftArm[])
{
  int value;
  if (cm730.ReadWord(JointData::ID_L_SHOULDER_PITCH, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
  {
    leftArm[0] = value;
  }
  
  if (cm730.ReadWord(JointData::ID_L_SHOULDER_ROLL, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
  {
    leftArm[1] = value;
  }
  
  if (cm730.ReadWord(JointData::ID_L_ELBOW, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
  {
    leftArm[2] = value;
  }
}
int main(int argc, char *argv[])
{
	signal(SIGABRT, &sighandler);
    signal(SIGTERM, &sighandler);
	signal(SIGQUIT, &sighandler);
	signal(SIGINT, &sighandler);

	int ch;
	char filename[128];

    fprintf(stderr, "\n***********************************************************************\n");
    fprintf(stderr,   "*                      RoboPlus Server Program                        *\n");
    fprintf(stderr,   "***********************************************************************\n\n");

	change_current_dir();
	if(argc < 2)
		strcpy(filename, MOTION_FILE_PATH); // Set default motion file path
	else
		strcpy(filename, argv[1]);

	/////////////// Load/Create Action File //////////////////
	if(Action::GetInstance()->LoadFile(filename) == false)
	{
		printf("Can not open %s\n", filename);
		printf("Do you want to make a new action file? (y/n) ");
		ch = _getch();
		if(ch != 'y')
		{
			printf("\n");
			return 0;
		}

		if(Action::GetInstance()->CreateFile(filename) == false)
		{
			printf("Fail to create %s\n", filename);
			return 0;
		}
	}
	////////////////////////////////////////////////////////////


	//////////////////// Framework Initialize ////////////////////////////	
	if(MotionManager::GetInstance()->Initialize(&cm730) == false)
	{
		printf("Fail to initialize Motion Manager!\n");
			return 0;
	}
	MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());	
    LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
    motion_timer->Stop();
	/////////////////////////////////////////////////////////////////////

    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(27 > firm_ver)
    {
        fprintf(stderr, "The RoboPlus(ver 1.0.23.0 or higher) Motion is not supported 1024 resolution..\n\n");
        fprintf(stderr, "Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
        exit(0);
    }

	std::cout << "[Running....]\n";
    try
    {
        // Create the socket
		LinuxServer new_sock;
        LinuxServer server ( TCPIP_PORT );

        while ( true )
        {
            std::cout << "[Waiting..]" << std::endl;            
            server.accept ( new_sock );
            std::cout << "[Accepted..]" << std::endl;

            try
            {
                while ( true )
                {
                    std::string data;
					Action::PAGE page;

                    new_sock >> data;
					std::cout << data << std::endl;

                    std::string* p_str_tok = std::string_split(data, " ");

                    if(p_str_tok[0] == "v")
                    {
                        new_sock << "{[DARwIn:" << VERSION << "]}\n";
                    }
                    else if(p_str_tok[0] == "E")
                    {
                        new_sock << "{[DARwIn:1.000]}";
                        new_sock << "{[PC:TCP/IP][DXL:1000000(BPS)]}";
						new_sock << "{";
						for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
							new_sock << "[" << id << ":029(MX-28)]";
						new_sock << "}";
                        new_sock << "{[DXL:" << JointData::NUMBER_OF_JOINTS << "(PCS)]}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "exit")
                    {
						Action::GetInstance()->Stop();
                    }
                    else if(p_str_tok[0] == "List")
                    {
                        new_sock << "{";
                        for(int i = 0; i < 256; i++)
						{
							Action::GetInstance()->LoadPage(i, &page);
                            new_sock << "[" << i << ":";
							new_sock.send(page.header.name, 14);
							new_sock << ":" << (int)page.header.next << ":" << (int)page.header.exit << ":" << (int)page.header.stepnum << "]";
						}
                        new_sock << "}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "Get" || p_str_tok[0] == "on" || p_str_tok[0] == "off")
                    {
						if(p_str_tok[0] == "on" || p_str_tok[0] == "off")
						{
							int torque;
							if(p_str_tok[0] == "on")
							{
								torque = 1;
								
							}
							else if(p_str_tok[0] == "off")
							{
								torque = 0;
							}

							int i;
							for(i=1; i<ARGUMENT_NAXNUM; i++)
							{
								if(p_str_tok[i].length() > 0)
								{
									cm730.WriteByte(atoi(p_str_tok[i].c_str()), MX28::P_TORQUE_ENABLE, torque, 0);
								}
								else
									break;
							}

							if(i == 1)
							{
								for(int id=1; id<JointData::NUMBER_OF_JOINTS; id++)
									cm730.WriteByte(id, MX28::P_TORQUE_ENABLE, torque, 0);
							}
						}

						int torque, position;
						new_sock << "{";
						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							new_sock << "[";
							std::cout << "[";
							if(id >= 1 && id < JointData::NUMBER_OF_JOINTS)
							{
								if(cm730.ReadByte(id, MX28::P_TORQUE_ENABLE, &torque, 0) == CM730::SUCCESS)
								{
									if(torque == 1)
									{
										if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &position, 0) == CM730::SUCCESS)
										{
											new_sock << position;
											std::cout << position;
										}
										else
										{
											new_sock << "----";
											std::cout << "Fail to read present position ID(" << id << ")";
										}
									}
									else
									{
										new_sock << "????";
										std::cout << "????";
									}
								}
								else
								{
									new_sock << "----";
									std::cout << "Fail to read torque ID(" << id << ")";
								}
							}
							else
							{
								new_sock << "----";
								std::cout << "----";
							}
							new_sock << "]";
							std::cout << "]";
						}
						std::cout << std::endl;
						new_sock << "}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "go")
                    {
						int GoalPosition, StartPosition, Distance;
						
						new_sock << "{";
						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							if(id >= 1 && id < JointData::NUMBER_OF_JOINTS)
							{
								if(cm730.ReadWord(id, MX28::P_PRESENT_POSITION_L, &StartPosition, 0) == CM730::SUCCESS)
								{
									GoalPosition = (int)atoi(p_str_tok[id + 1].c_str());

									if( StartPosition > GoalPosition )
										Distance = StartPosition - GoalPosition;
									else
										Distance = GoalPosition - StartPosition;

									Distance >>= 2;
									if( Distance < 8 )
										Distance = 8;

									cm730.WriteWord(id, MX28::P_MOVING_SPEED_L, Distance, 0);
									if(cm730.WriteWord(id, MX28::P_GOAL_POSITION_L, GoalPosition, 0) == CM730::SUCCESS)
									{
										new_sock << "[" << GoalPosition << "]";
										std::cout << "[" << GoalPosition << "]";
									}
									else
									{
										new_sock << "[----]";
										std::cout << "[----]";
									}
								}
								else
								{
									new_sock << "[----]";
									std::cout << "[----]";
								}
							}
							else
							{
								new_sock << "[----]";
								std::cout << "[----]";
							}
						}
						std::cout << std::endl;
						new_sock << "}";
						new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "set")
                    {
						int id = (int)atoi(p_str_tok[1].c_str());
						int position = (int)atoi(p_str_tok[2].c_str());

						if(cm730.WriteWord(id, MX28::P_GOAL_POSITION_L, position, 0) == CM730::SUCCESS)
						{
							if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &position, 0) == CM730::SUCCESS)
							{
								std::cout << "{[" << position << "]}";
								new_sock << "{[" << position << "]}";
							}
							else
							{
								std::cout << "[Fail to read goal position ID(" << id << ")]";
								new_sock << "{[----]}";
							}
						}
						else
						{
							std::cout << "[Fail to write goal position ID(" << id << ")]";
							new_sock << "{[----]}";
						}
						std::cout << std::endl;
						new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "play" || p_str_tok[0] == "rplay")
                    {						
						int value;

						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							if(id >= JointData::ID_R_SHOULDER_PITCH && id <= JointData::ID_HEAD_TILT)
							{
								if(cm730.ReadWord(id, MX28::P_TORQUE_ENABLE, &value, 0) == CM730::SUCCESS)
								{
									if(value == 0)
									{
										if(cm730.ReadWord(id, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
											MotionStatus::m_CurrentJoints.SetValue(id, value);
										else
											std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
									}
									else
									{
										if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &value, 0) == CM730::SUCCESS)
											MotionStatus::m_CurrentJoints.SetValue(id, value);
										else
											std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
									}
								}
								else
									std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
							}
						}														
						
					    motion_timer->Start();
						MotionManager::GetInstance()->SetEnable(true);
						
						int index = (int)atoi(p_str_tok[1].c_str());
						if(p_str_tok[0] == "play")
							Action::GetInstance()->Start(index);
						else
							Action::GetInstance()->Start(index, &page);
                    }
					else if(p_str_tok[0] == "info")
					{
						int ipage, istep;
						if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
						{
							new_sock << "{[" << ipage << ":" << istep << "]}\n";
							std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
						}
						else
						{
							new_sock << "{[OK]}\n";
							std::cout << "[END]" << std::endl;
							MotionManager::GetInstance()->SetEnable(false);
							motion_timer->Stop();
						}
					}
                    else if(p_str_tok[0] == "stop")
                    {
						Action::GetInstance()->Stop();
                    }
                    else if(p_str_tok[0] == "stopinfo")
                    {
                        Action::GetInstance()->Stop();

                        int ipage, istep;
                        if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
                        {
                            new_sock << "{[" << ipage << ":" << istep << "]}\n";
                            std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
                        }
                        else
                        {
                            new_sock << "{[OK]}\n";
                            std::cout << "[END]" << std::endl;
                            MotionManager::GetInstance()->SetEnable(false);
                            motion_timer->Stop();
                        }
                    }
					else if(p_str_tok[0] == "break")
                    {
						Action::GetInstance()->Brake();
                    }
                    else if(p_str_tok[0] == "breakinfo")
                    {
                        Action::GetInstance()->Brake();

                        int ipage, istep;
                        if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
                        {
                            new_sock << "{[" << ipage << ":" << istep << "]}\n";
                            std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
                        }
                        else
                        {
                            new_sock << "{[OK]}\n";
                            std::cout << "[END]" << std::endl;
                            MotionManager::GetInstance()->SetEnable(false);
                            motion_timer->Stop();
                        }
                    }
                    else if(p_str_tok[0] == "RDownload")
                    {
						int rcv_len = (int)sizeof(Action::PAGE);
						int i_data = 0;
						unsigned char *data = (unsigned char*)&page;

                        new_sock << "{[READY]}\n";
						while(rcv_len > 0)
						{
							i_data += new_sock.recv(data, rcv_len);
							data += i_data;
							rcv_len -= i_data;
						}
                        new_sock << "{[SUCCESS]}\n";
                        new_sock << "{[ME]}\n";
                    }                    
                    else if(p_str_tok[0] == "upload")
                    {
                        // send page data
						int index = (int)(atoi(p_str_tok[1].c_str()) / sizeof(Action::PAGE));
						int num = (int)(atoi(p_str_tok[2].c_str()) / sizeof(Action::PAGE));
						
						for(int i=0; i<num; i++)
						{
							Action::GetInstance()->LoadPage(index + i, &page);
							new_sock.send((unsigned char*)&page, (int)sizeof(Action::PAGE));
						}

                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "ld")
                    {
						int index = (int)(atoi(p_str_tok[1].c_str()) / sizeof(Action::PAGE));
						int num = (int)(atoi(p_str_tok[2].c_str()) / sizeof(Action::PAGE));
						int rcv_len;
						int i_data;
						unsigned char *data;

						std::cout << "[READY]" << std::endl;
                        new_sock << "{[READY]}\n";

                        // byte stream
						for(int i=0; i<num; i++)
						{
							i_data = 0;
							rcv_len = (int)sizeof(Action::PAGE);
							data = (unsigned char*)&page;
							while(rcv_len > 0)
							{
								i_data += new_sock.recv(data, rcv_len);
								data += i_data;
								rcv_len -= i_data;
							}
							Action::GetInstance()->SavePage(index + i, &page);
							std::cout << "[SAVE:" << index + i << "]" << std::endl;
						}
                    }
					else
					{
						std::cout << " [Invalid:" << p_str_tok[0] << "]" << std::endl;
					}
                }
Exemplo n.º 5
0
int GetCurrentPosition(CM730 &cm730)
{
	int m=Robot::READY,p,j,pos[31];
	int dMaxAngle1,dMaxAngle2,dMaxAngle3;
	double dAngle;
	int rl[6] = { JointData::ID_R_ANKLE_ROLL,JointData::ID_R_ANKLE_PITCH,JointData::ID_R_KNEE,JointData::ID_R_HIP_PITCH,JointData::ID_R_HIP_ROLL,JointData::ID_R_HIP_YAW };
	int ll[6] = { JointData::ID_L_ANKLE_ROLL,JointData::ID_L_ANKLE_PITCH,JointData::ID_L_KNEE,JointData::ID_L_HIP_PITCH,JointData::ID_L_HIP_ROLL,JointData::ID_L_HIP_YAW };

	for(p=0;p<31;p++) 
		{
		pos[p]	= -1;
		}
	for(p=0; p<6; p++)
		{
		if(cm730.ReadWord(rl[p], MX28::P_PRESENT_POSITION_L, &pos[rl[p]], 0) != CM730::SUCCESS)
			{
			printf("Failed to read position %d",rl[p]);
			}
		if(cm730.ReadWord(ll[p], MX28::P_PRESENT_POSITION_L, &pos[ll[p]], 0) != CM730::SUCCESS)
			{
			printf("Failed to read position %d",ll[p]);
			}
		}
	// compare to a couple poses
	// first sitting - page 48
	Action::GetInstance()->LoadPage(48, &Page);
	j = Page.header.stepnum-1;
	dMaxAngle1 = dMaxAngle2 = dMaxAngle3 = 0;
	for(p=0;p<6;p++)
		{
		dAngle = abs(MX28::Value2Angle(pos[rl[p]]) - MX28::Value2Angle(Page.step[j].position[rl[p]]));
		if(dAngle > dMaxAngle1)
			dMaxAngle1 = dAngle;
		dAngle = abs(MX28::Value2Angle(pos[ll[p]]) - MX28::Value2Angle(Page.step[j].position[ll[p]]));
		if(dAngle > dMaxAngle1)
			dMaxAngle1 = dAngle;
		}				
	// squating - page 15
	Action::GetInstance()->LoadPage(15, &Page);
	j = Page.header.stepnum-1;
	for(int p=0;p<6;p++)
		{
		dAngle = abs(MX28::Value2Angle(pos[rl[p]]) - MX28::Value2Angle(Page.step[j].position[rl[p]]));
		if(dAngle > dMaxAngle2)
			dMaxAngle2 = dAngle;
		dAngle = abs(MX28::Value2Angle(pos[ll[p]]) - MX28::Value2Angle(Page.step[j].position[ll[p]]));
		if(dAngle > dMaxAngle2)
			dMaxAngle2 = dAngle;
		}				
	// walkready - page 9
	Action::GetInstance()->LoadPage(9, &Page);
	j = Page.header.stepnum-1;
	for(int p=0;p<6;p++)
		{
		dAngle = abs(MX28::Value2Angle(pos[rl[p]]) - MX28::Value2Angle(Page.step[j].position[rl[p]]));
		if(dAngle > dMaxAngle3)
			dMaxAngle3 = dAngle;
		dAngle = abs(MX28::Value2Angle(pos[ll[p]]) - MX28::Value2Angle(Page.step[j].position[ll[p]]));
		if(dAngle > dMaxAngle3)
			dMaxAngle3 = dAngle;
		}				
	if(dMaxAngle1<20 && dMaxAngle1<dMaxAngle2 && dMaxAngle1<dMaxAngle3)
		m = Robot::SITTING;
	if(dMaxAngle2<20 && dMaxAngle2<dMaxAngle1 && dMaxAngle2<dMaxAngle3)
		m = Robot::READY;
	if(dMaxAngle3<20 && dMaxAngle3<dMaxAngle1 && dMaxAngle3<dMaxAngle2)
		m = Robot::SOCCER;
	printf("Sitting = %d, Squating = %d, Standing = %d\n",dMaxAngle1,dMaxAngle2,dMaxAngle3);
	printf("Robot is %s\n",m==Robot::READY?"Ready":m==Robot::SOCCER?"Soccer":m==Robot::SITTING?"Sitting":"None");
	return m;
}