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;
}
}
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);
}
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;
}
}
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;
}