// check battery and return volt value or if below value, sit robot
// this returns a bit value from 0-255, 0=0Volts 11=11.1Volt 120=12.0 Volt etc.
// NOTE: current testing revealed this might not be the case even though doc's state
// might need to verify with some testing
int BatteryVoltLevel() {
int voltage;
if(cm730.ReadByte(CM730::ID_CM, CM730::P_VOLTAGE, &voltage, 0) == CM730::SUCCESS)
{
return voltage;
}
// failed, no read from cm730
return -1;
}
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 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;
}
int main(void)
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_output = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->SetCameraSettings(CameraSettings()); // set default
LinuxCamera::GetInstance()->LoadINISettings(ini); // load from ini
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
ColorFinder* ball_finder = new ColorFinder();
ball_finder->LoadINISettings(ini);
httpd::ball_finder = ball_finder;
BallTracker tracker = BallTracker();
BallFollower follower = BallFollower();
ColorFinder* red_finder = new ColorFinder(0, 15, 45, 0, 0.3, 50.0);
red_finder->LoadINISettings(ini, "RED");
httpd::red_finder = red_finder;
ColorFinder* yellow_finder = new ColorFinder(60, 15, 45, 0, 0.3, 50.0);
yellow_finder->LoadINISettings(ini, "YELLOW");
httpd::yellow_finder = yellow_finder;
ColorFinder* blue_finder = new ColorFinder(225, 15, 45, 0, 0.3, 50.0);
blue_finder->LoadINISettings(ini, "BLUE");
httpd::blue_finder = blue_finder;
httpd::ini = ini;
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
linux_cm730.SetPortName(U2D_DEV_NAME1);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
}
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
/////////////////////////////////////////////////////////////////////
MotionManager::GetInstance()->LoadINISettings(ini);
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)
{
#ifdef MX28_1024
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
#else
fprintf(stderr, "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");
exit(0);
#endif
}
else if(27 <= firm_ver)
{
#ifdef MX28_1024
fprintf(stderr, "MX-28's firmware is not support 1024 resolution!! \n");
fprintf(stderr, "Remove '#define MX28_1024' from 'MX28.h' file and rebuild.\n\n");
exit(0);
#else
Action::GetInstance()->LoadFile((char*)MOTION_FILE_PATH);
#endif
}
else
exit(0);
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
MotionManager::GetInstance()->SetEnable(true);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3");
Action::GetInstance()->Start(15);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
while(1)
{
StatusCheck::Check(cm730);
Point2D ball_pos, red_pos, yellow_pos, blue_pos;
LinuxCamera::GetInstance()->CaptureFrame();
memcpy(rgb_output->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageSize);
if(StatusCheck::m_cur_mode == READY || StatusCheck::m_cur_mode == VISION)
{
ball_pos = ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
red_pos = red_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
yellow_pos = yellow_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
blue_pos = blue_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
unsigned char r, g, b;
for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
{
r = 0; g = 0; b = 0;
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 255;
g = 128;
b = 0;
}
if(red_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 255;
g = 0;
b = 0;
}
}
if(yellow_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 255;
g = 255;
b = 0;
}
}
if(blue_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 0;
g = 0;
b = 255;
}
}
if(r > 0 || g > 0 || b > 0)
{
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 0] = r;
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 1] = g;
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 2] = b;
}
}
}
else if(StatusCheck::m_cur_mode == SOCCER)
{
tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame));
for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 0] = 255;
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 1] = 128;
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 2] = 0;
}
}
}
streamer->send_image(rgb_output);
if(StatusCheck::m_is_started == 0)
continue;
switch(StatusCheck::m_cur_mode)
{
case READY:
break;
case SOCCER:
if(Action::GetInstance()->IsRunning() == 0)
{
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
follower.Process(tracker.ball_position);
if(follower.KickBall != 0)
{
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Action::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
if(follower.KickBall == -1)
{
Action::GetInstance()->Start(12); // RIGHT KICK
fprintf(stderr, "RightKick! \n");
}
else if(follower.KickBall == 1)
{
Action::GetInstance()->Start(13); // LEFT KICK
fprintf(stderr, "LeftKick! \n");
}
}
}
break;
case MOTION:
if(LinuxActionScript::m_is_running == 0)
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH);
break;
case VISION:
int detected_color = 0;
detected_color |= (red_pos.X == -1)? 0 : VisionMode::RED;
detected_color |= (yellow_pos.X == -1)? 0 : VisionMode::YELLOW;
detected_color |= (blue_pos.X == -1)? 0 : VisionMode::BLUE;
if(Action::GetInstance()->IsRunning() == 0)
VisionMode::Play(detected_color);
break;
}
}
return 0;
}
int main(int argc, char *argv[])
{
int trackerSel;
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
minIni* ini1 = new minIni(M_INI);
StatusCheck::m_ini = ini;
StatusCheck::m_ini1 = ini1;
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
linux_cm730.SetPortName(U2D_DEV_NAME1);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
}
Walking::GetInstance()->LoadINISettings(ini);
usleep(100);
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
//MotionManager::GetInstance()->StartThread();
//LinuxMotionTimer::Initialize(MotionManager::GetInstance());
LinuxMotionTimer linuxMotionTimer;
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
/////////////////////////////////////////////////////////////////////
// MotionManager::GetInstance()->LoadINISettings(ini);
int firm_ver = 0,retry=0;
//important but allow a few retries
while(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);
retry++;
if(retry >=3) exit(1);// if we can't do it after 3 attempts its not going to work.
}
if(0 < firm_ver && firm_ver < 40)
{
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
}
else
{
fprintf(stderr, "Wrong firmware version %d!! \n\n", JointData::ID_HEAD_PAN);
exit(0);
}
//conversion! ////////////////
/*
Action::GetInstance()->LoadFile("../../../Data/motion.bin");
int j,k,p,a;
double f;
for(k=0;k<Action::MAXNUM_PAGE;k++)
{
Action::GetInstance()->LoadPage(k, &Page);
for(j=0;j<Action::MAXNUM_STEP;j++)
{
for(p=0;p<31;p++)
{
a = Page.step[j].position[p];
if(a < 1024)
{
f = ((a-512)*10)/3+2048;
a = (int)f;
if(a<0) a =0;
if(a>4095) a = 4095;
Page.step[j].position[p] = a;
}
}
}
Action::GetInstance()->SavePage(k, &Page);
}
exit(0);
*/
//copy page ////////////////
if(argc>1 && strcmp(argv[1],"-copy")==0)
{
printf("Page copy -- uses files motion_src.bin and motion_dest.bin\n");
if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_src.bin") == false)
{
printf("Unable to open source file\n");
exit(1);
}
int k;
void *page1;
page1 = malloc(sizeof(Robot::Action::PAGE));
printf("Page to load:");
if(scanf("%d",&k) != EOF)
{
if(Action::GetInstance()->LoadPage(k, (Robot::Action::PAGE *)page1) == false)
{
printf("Unable to load page %d\n",k);
exit(1);
}
if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_dest.bin") == false)
{
printf("Unable to open destination file\n");
exit(1);
}
if(Action::GetInstance()->SavePage(k, (Robot::Action::PAGE *)page1) == false)
{
printf("Unable to save page %d\n",k);
exit(1);
}
printf("Completed successfully.\n");
exit(0);
}
}
/////////////////////////////
/*
Walking::GetInstance()->m_Joint.SetEnableBody(true,true);
MotionManager::GetInstance()->SetEnable(true);
Walking::GetInstance()->LoadINISettings(m_ini);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
PS3Controller_Start();
LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3");
Action::GetInstance()->Start(15);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
*/
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->LoadINISettings(ini);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Head::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
MotionManager::GetInstance()->SetEnable(true);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
if(PS3Controller_Start() == 0)
printf("PS3 controller not installed.\n");
cm730.WriteWord(CM730::P_LED_HEAD_L, cm730.MakeColor(1,1,1),0);
//determine current position
StatusCheck::m_cur_mode = GetCurrentPosition(cm730);
//LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3");
if((argc>1 && strcmp(argv[1],"-off")==0) || (StatusCheck::m_cur_mode == SITTING))
{
cm730.DXLPowerOn(false);
//for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
// cm730.WriteByte(id, MX28::P_TORQUE_ENABLE, 0, 0);
}
else
{
Action::GetInstance()->Start(15);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
}
while(1)
{
StatusCheck::Check(cm730);
if(StatusCheck::m_is_started == 0)
continue;
}
return 0;
}
