int main(void)
{
printf( "\n===== Head tracking Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(ini);
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
BallTracker tracker = BallTracker();
tracker.LoadINISettings(ini);
httpd::ball_finder = &tracker.finder;
//////////////////// Framework Initialize ////////////////////////////
LinuxArbotixPro linux_arbotixpro(U2D_DEV_NAME);
ArbotixPro arbotixpro(&linux_arbotixpro);
if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
LinuxMotionTimer::Initialize(MotionManager::GetInstance());
MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false);
MotionManager::GetInstance()->SetEnable(true);
/////////////////////////////////////////////////////////////////////
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8);
while (1)
{
Point2D pos;
LinuxCamera::GetInstance()->CaptureFrame();
tracker.Process(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
for (int i = 0; i < rgb_ball->m_NumberOfPixels; i++)
{
if (tracker.finder.m_result->m_ImageData[i] == 1)
{
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 0] = 255;
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 1] = 0;
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 2] = 0;
}
}
streamer->send_image(rgb_ball);
}
return 0;
}
int main(void)
{
printf( "\n===== Camera Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(ini);
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
while (1)
{
LinuxCamera::GetInstance()->CaptureFrame();
streamer->send_image(LinuxCamera::GetInstance()->fbuffer->m_YUVFrame);
}
return 0;
}
int main(void)
{
printf( "\n===== Action script Tutorial for DARwIn =====\n\n");
minIni* ini = new minIni(INI_FILE_PATH);
change_current_dir();
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
//////////////////// Framework Initialize ////////////////////////////
LinuxCM730 linux_cm730("/dev/ttyUSB0");
CM730 cm730(&linux_cm730);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
/////////////////////////////////////////////////////////////////////
MotionManager::GetInstance()->SetEnable(true);
Action::GetInstance()->Start(1); /* Init(stand up) pose */
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
printf("Press the ENTER key to begin!\n");
getchar();
LinuxActionScript::ScriptStart("script.asc");
while(LinuxActionScript::m_is_running == 1) sleep(10);
return 0;
}
int main(void)
{
printf( "\n===== Head tracking Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(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();
//////////////////// Framework Initialize ////////////////////////////
LinuxCM730 linux_cm730(U2D_DEV_NAME);
CM730 cm730(&linux_cm730);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false);
MotionManager::GetInstance()->SetEnable(true);
/////////////////////////////////////////////////////////////////////
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8);
struct timeval tval;
gettimeofday(&tval, NULL);
double prevtime = tval.tv_sec + tval.tv_usec / 1000000.0;
while(1)
{
Point2D pos;
LinuxCamera::GetInstance()->CaptureFrame();
tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame));
rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
for(int i = 0; i < rgb_ball->m_NumberOfPixels; i++)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 0] = 255;
rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 1] = 0;
rgb_ball->m_ImageData[i*rgb_ball->m_PixelSize + 2] = 0;
}
}
streamer->send_image(rgb_ball);
gettimeofday(&tval, NULL);
double curtime = tval.tv_sec + tval.tv_usec / 1000000.0;
printf("FPS: % 2d\n", (int) (1 / (curtime - prevtime)));
prevtime = curtime;
}
return 0;
}
int main(void)
{
printf( "\n===== Action script Tutorial for DARwIn =====\n\n");
change_current_dir();
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
//////////////////// Framework Initialize ////////////////////////////
LinuxCM730 linux_cm730("/dev/ttyUSB0");
CM730 cm730(&linux_cm730);
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->Start();
/////////////////////////////////////////////////////////////////////
MotionManager::GetInstance()->SetEnable(true);
//Action* Action::m_UniqueInstance = new Action();
Action::GetInstance()->m_Joint.SetEnableBody(true, true);// this opposes to the torque disable
// Action::GetInstance()->Start(1); usleep(1000);
Action::GetInstance()->Start(7);
/* Init(stand up) pose */
//Action::GetInstance()->Start(1);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
printf("Press the ENTER key to begin!\n");
getchar();
LinuxActionScript::m_is_running = 0;
//Action::GetInstance()->Start(8);
//Action::GetInstance()->m_Joint.SetAngle(6, MotionStatus::m_CurrentJoints.GetValue(6));
//leftarm = JointData::GetAngle(int ID_R_SHOULDER_PITCH);
//Action::GetInstance()->m_Joint.SetAngle(5, MotionStatus::m_CurrentJoints.GetValue(5));
//leftarm = MotionStatus::m_CurrentJoints.GetAngle(6);
//rightarm = MotionStatus::m_CurrentJoints.GetAngle(5);
//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);
//Action::GetInstance()->m_Joint.SetValue(1, 2000);
//Action::GetInstance()->m_Joint.SetValue(2, 2000);
//while(Action::GetInstance()->IsRunning()) usleep(8000);
//Action::GetInstance()->m_Joint.SetAngle(6, -20);
//Action::GetInstance()->m_Joint.SetAngle(5, 20);
// LinuxActionScript::ScriptStart("script.asc");
while(LinuxActionScript::m_is_running == 1) sleep(10);
return 0;
}
int main(void)
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
stringstream p;
p<<"[" << timeinfo->tm_hour<< ":"<< timeinfo->tm_min << ":" << timeinfo->tm_sec <<"]";
std::string logname = "/home/robot/Desktop/LOG";
logname+=p.str();
startLog(logFile,logname);
for (int i=0; i<BrainBall.x_detail ;i++)
{
BrainBall.x_place[i] = 0;
}
printf( "\n===== Action script for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
linux_cm730.SetPortName("/dev/ttyUSB1");
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
}
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
LinuxMotionTimer linuxMotionTimer;
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
/////////////////////////////////////////////////////////////////////
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Head::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
MotionManager::GetInstance()->SetEnable(true);
Action::GetInstance()->Start(1); //Init(stand up) pose
Action::GetInstance()->Brake();
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
MotionManager::GetInstance()->ResetGyroCalibration();
printf("Press the ENTER key to begin!\n");
getchar();
//////
writeToLog(logFile,"started");
printf("0-Play\n");
printf("1-Kick\n");
printf("2-stand\n");
printf("3-Straight walking\n");
printf("4-Turn right\n");
printf("5-Turn left\n");
printf("6-stop walking\n");
printf("7-reset\n");
printf("8-GetTilt\n");
printf("9-SetTilt\n");
printf("q-Exit\n");
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
while(1)
{
char c =getchar();
if (c == '0')
{
play();
}
else if (c == '1')
{
writeToLog(logFile,"kick");
Action::GetInstance()->Start(2);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
}
else if(c == '2')
{
writeToLog(logFile,"stand");
Action::GetInstance()->Start(1);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
}
else if(c == '3')
{
if(Walking::GetInstance()->IsRunning() == false )
{
fprintf(stderr, "STARTING\n");
Action::GetInstance()->Start(9);
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
//Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true,true);
Walking::GetInstance()->X_MOVE_AMPLITUDE = 0;
Walking::GetInstance()->Y_MOVE_AMPLITUDE = 0;
Walking::GetInstance()->A_MOVE_AMPLITUDE = 0;
Walking::GetInstance()->Start();
}
else
Walking::GetInstance()->A_MOVE_AMPLITUDE = 0;
}
else if(c == '4')
{
if(Walking::GetInstance()->IsRunning() == true )
{
fprintf(stderr, "TURNING RIGHT\n");
Walking::GetInstance()->A_MOVE_AMPLITUDE = -5;
}
}
else if(c == '5')
{
if(Walking::GetInstance()->IsRunning() == true )
{
fprintf(stderr, "TURNING LEFT\n");
Walking::GetInstance()->A_MOVE_AMPLITUDE = 5;
}
}
else if(c == '6')
{
if(Walking::GetInstance()->IsRunning() == true )
{
fprintf(stderr, "STOPPING\n");
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Head::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
MotionManager::GetInstance()->SetEnable(true);
}
}
else if(c == '7')
{
for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
Reset(&cm730, i);
Reset(&cm730, CM730::ID_CM);
}
else if(c == '8')
{
float tilt;
tilt = GetTilt();
printf("%f\n", tilt);
}
else if(c == '9')
{
float tilt,pan;
cout<<"enter tilt\n";
cin>>tilt;
cout<<"enter pan\n";
cin>>pan;
SetTilt(pan,tilt);
}
else if(c == 'q')
int main(void)
{
//Register signal and signal handler
signal(SIGINT, signal_callback_handler);
printf( "\n===== Head tracking Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(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();
//////////////////// Framework Initialize ////////////////////////////
if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false)
{
linux_arbotixpro.SetPortName(U2D_DEV_NAME1);
if (MotionManager::GetInstance()->Initialize(&arbotixpro) == 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();
MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false);
MotionManager::GetInstance()->SetEnable(true);
/////////////////////////////////////////////////////////////////////
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
//Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8);
//Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8);
while (isRunning)
{
//usleep(10000);
Point2D pos;
LinuxCamera::GetInstance()->CaptureFrame();
tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame));
rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
for (int i = 0; i < rgb_ball->m_NumberOfPixels; i++)
{
if (ball_finder->m_result->m_ImageData[i] == 1)
{
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 0] = 255;
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 1] = 0;
rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 2] = 0;
}
}
streamer->send_image(rgb_ball);
}
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, const char** argv)
{
printf( "\n===== Face tracking Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(ini);
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
BallTracker tracker = BallTracker();
//////////////////// Framework Initialize ////////////////////////////
LinuxCM730 linux_cm730(U2D_DEV_NAME);
CM730 cm730(&linux_cm730);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
// return 0;
}
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false);
MotionManager::GetInstance()->SetEnable(true);
/////////////////////////////////////////////////////////////////////
//Head::GetInstance()->LoadINISettings(ini);
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8);
printf("Press the ENTER key to begin!\n");
getchar();
//parse command line arguments
char ftFile[256],conFile[256],triFile[256];
bool fcheck = false; double scale = 1; int fpd = -1; bool show = true;
if(parse_cmd(argc,argv,ftFile,conFile,triFile,fcheck,scale,fpd)<0)return 0;
//set other tracking parameters
std::vector<int> wSize1(1); wSize1[0] = 7;
std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7;
int nIter = 5; double clamp=3,fTol=0.01;
FACETRACKER::Tracker model(ftFile);
cv::Mat tri=FACETRACKER::IO::LoadTri(triFile);
cv::Mat con=FACETRACKER::IO::LoadCon(conFile);
//initialize camera and display window
cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text;
int64 t1,t0 = cvGetTickCount(); int fnum=0;
bool failed = true;
while(1)
{
Point2D pos;
LinuxCamera::GetInstance()->CaptureFrame();
//Create the image header which will contain the converted BYTE image
int channel = 3; //1 for black and white pictures, 3 for colored pictures
int step = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_WidthStep;//depends from the channel
//grab image, resize and flip
IplImage* I = cvCreateImageHeader(cv::Size(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Width,LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Height), IPL_DEPTH_8U, channel);
//Set the BYTE pointer as attribut of the IplImage --> "converts" BYTE *pData in IplImage *frame
cvSetData(I, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, step);
if(!I)continue; frame = I;
if(scale == 1)im = frame;
else cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows));
cv::flip(im,im,1);
cv::cvtColor(im,gray,CV_RGB2GRAY);
//track this image
std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1;
if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0){
int idx = model._clm.GetViewIdx(); failed = false;
Draw(im,model._shape,con,tri,model._clm._visi[idx]);
pos.X = model._rect.x+ model._rect.width/2;
pos.Y = model._rect.y+ model._rect.height/2;
}else{
if(show){cv::Mat R(im,cvRect(0,0,150,50)); R = cv::Scalar(0,0,255);}
model.FrameReset(); failed = true;
pos.X = -1;
pos.Y = -1;
}
//draw framerate on display image
if(fnum >= 9){
t1 = cvGetTickCount();
fps = 10.0/((double(t1-t0)/cvGetTickFrequency())/1e+6);
t0 = t1; fnum = 0;
}else fnum += 1;
if(show){
sprintf(sss,"%d frames/sec",(int)round(fps)); text = sss;
cv::putText(im,text,cv::Point(10,20),
CV_FONT_HERSHEY_SIMPLEX,0.5,CV_RGB(255,255,255));
}
//show image and check for user input
imshow("Face Tracker",im);
int c = cv::waitKey(10);
if(c == 27)break; else if(char(c) == 'd')model.Save("/home/koreki/Documents/perso/darwin/Linux/project/tutorial/face_tracker/koreki.tracker");//model.FrameReset();
//model.FrameReset();
// pos = facefinder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame);
tracker.Process(pos);
fprintf(stderr, "posx: %f, posy: %f \r", pos.X, pos.Y);
rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
rgb_ball->m_ImageData = im.data;
streamer->send_image(rgb_ball);
}
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;
}
