// Initializes the servos, gets the motion path
// gets access to Motion Manger etc.
bool Initialize() {
minIni* ini = new minIni(INI_FILE_PATH);
// check if on dev 0
// Motion manager initialize queries all servos
if (MotionManager::GetInstance()->Initialize(&cm730) == false) {
// if not try dev 1
linux_cm730.SetPortName(U2D_DEV_NAME1);
if (MotionManager::GetInstance()->Initialize(&cm730) == false) {
printf("\n The Framework has encountered a connection error to the subcontroller!\n");
printf("Failed to initialize Motion Manager! Meaning the program is unable to communicate with Subcontroller.\n");
printf("Please check if subcontroller serial port is recognized in /dev/ as TTYUSB0 or TTYUSB1, or if port is currently in use by another program.\n");
return false;
}
}
// load config ini for walking gait params
Walking::GetInstance()->LoadINISettings(ini);
usleep(100);
// load config ini offset values Maybe to comment out if not used
MotionManager::GetInstance()->LoadINISettings(ini);
// instantiates each motion module to motion manager
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
// MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
// instatiate sys timer and start
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
// load motion binary to action module
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
return true;
}
void Motion::StartEngines()
{
printf("===== Action script for DARwIn =====\n\n");
ChangeCurrentDir();
minIni* ini = new minIni(INI_FILE_PATH);
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
//////////////////// 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");
}
}
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());
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);
MotionManager::GetInstance()->ResetGyroCalibration();
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
}
int main(int argc, char *argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
ros::init(argc,argv, "WALK_TUNER");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("motiondirector", 1000, chatterCallback);
change_current_dir();
LinuxArbotixPro linux_arbotixpro("/dev/ttyUSB0");
ArbotixPro arbotixpro(&linux_arbotixpro);
minIni* ini;
if (argc == 2)
ini = new minIni(argv[1]);
else
ini = new minIni(INI_FILE_PATH);
mjpg_streamer* streamer = new mjpg_streamer(0, 0);
httpd::ini = ini;
//////////////////// Framework Initialize ////////////////////////////
if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
LinuxMotionTimer linuxMotionTimer;
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
/////////////////////////////////////////////////////////////////////
DrawIntro(&arbotixpro);
MotionManager::GetInstance()->SetEnable(true);
MotionManager::GetInstance()->ResetGyroCalibration();
MotionManager::GetInstance()->Process();
ros::spin();
exit(0);
/*
while(1)
{
if(MotionStatus::FALLEN != STANDUP && (Walking::GetInstance()->IsRunning() == true) )
{
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
if(MotionStatus::FALLEN == FORWARD)
Action::GetInstance()->Start(1); // FORWARD GETUP 10
else if(MotionStatus::FALLEN == BACKWARD)
Action::GetInstance()->Start(2); // BACKWARD GETUP 11
while(Action::GetInstance()->IsRunning() == 1) usleep(8000);
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
}
}
*/
/* while (1)
{
int ch = _getch();
if (ch == 0x1b)
{
ch = _getch();
if (ch == 0x5b)
{
ch = _getch();
if (ch == 0x41) // Up arrow key
MoveUpCursor();
else if (ch == 0x42) // Down arrow key
MoveDownCursor();
else if (ch == 0x44) // Left arrow key
MoveLeftCursor();
else if (ch == 0x43)
MoveRightCursor();
}
}
else if ( ch == '[' )
DecreaseValue(false);
else if ( ch == ']' )
IncreaseValue(false);
else if ( ch == '{' )
DecreaseValue(true);
else if ( ch == '}' )
IncreaseValue(true);
else if ( ch >= 'A' && ch <= 'z' )
{
char input[128] = {0,};
char *token;
int input_len;
char cmd[80];
char strParam[20][30];
int num_param;
int idx = 0;
BeginCommandMode();
printf("%c", ch);
input[idx++] = (char)ch;
while (1)
{
ch = _getch();
if ( ch == 0x0A )
break;
else if ( ch == 0x7F )
{
if (idx > 0)
{
ch = 0x08;
printf("%c", ch);
ch = ' ';
printf("%c", ch);
ch = 0x08;
printf("%c", ch);
input[--idx] = 0;
}
}
else if ( ch >= 'A' && ch <= 'z' )
{
if (idx < 127)
{
printf("%c", ch);
input[idx++] = (char)ch;
}
}
}
fflush(stdin);
input_len = strlen(input);
if (input_len > 0)
{
token = strtok( input, " " );
if (token != 0)
{
strcpy( cmd, token );
token = strtok( 0, " " );
num_param = 0;
while (token != 0)
{
strcpy(strParam[num_param++], token);
token = strtok( 0, " " );
}
if (strcmp(cmd, "exit") == 0)
{
if (AskSave() == false)
break;
}
if (strcmp(cmd, "re") == 0)
DrawScreen();
else if (strcmp(cmd, "save") == 0)
{
Walking::GetInstance()->SaveINISettings(ini);
SaveCmd();
}
else if (strcmp(cmd, "mon") == 0)
{
MonitorCmd();
}
else if (strcmp(cmd, "help") == 0)
HelpCmd();
else
PrintCmd("Bad command! please input 'help'");
}
}
EndCommandMode();
}
}
DrawEnding();*/
//
//ros::spin();
}
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(int argc, char *argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
change_current_dir();
LinuxCM730 linux_cm730("/dev/ttyUSB0");
CM730 cm730(&linux_cm730);
minIni* ini;
if(argc==2)
ini = new minIni(argv[1]);
else
ini = new minIni(INI_FILE_PATH);
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
LinuxMotionTimer linuxMotionTimer;
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Start();
/////////////////////////////////////////////////////////////////////
DrawIntro(&cm730);
MotionManager::GetInstance()->SetEnable(true);
MotionManager::GetInstance()->ResetGyroCalibration();
while(1)
{
int ch = _getch();
if(ch == 0x1b)
{
ch = _getch();
if(ch == 0x5b)
{
ch = _getch();
if(ch == 0x41) // Up arrow key
MoveUpCursor();
else if(ch == 0x42) // Down arrow key
MoveDownCursor();
else if(ch == 0x44) // Left arrow key
MoveLeftCursor();
else if(ch == 0x43)
MoveRightCursor();
}
}
else if( ch == '[' )
DecreaseValue(false);
else if( ch == ']' )
IncreaseValue(false);
else if( ch == '{' )
DecreaseValue(true);
else if( ch == '}' )
IncreaseValue(true);
else if( ch >= 'A' && ch <= 'z' )
{
char input[128] = {0,};
char *token;
int input_len;
char cmd[80];
char strParam[20][30];
int num_param;
int idx = 0;
BeginCommandMode();
printf("%c", ch);
input[idx++] = (char)ch;
while(1)
{
ch = _getch();
if( ch == 0x0A )
break;
else if( ch == 0x7F )
{
if(idx > 0)
{
ch = 0x08;
printf("%c", ch);
ch = ' ';
printf("%c", ch);
ch = 0x08;
printf("%c", ch);
input[--idx] = 0;
}
}
else if( ch >= 'A' && ch <= 'z' )
{
if(idx < 127)
{
printf("%c", ch);
input[idx++] = (char)ch;
}
}
}
fflush(stdin);
input_len = strlen(input);
if(input_len > 0)
{
token = strtok( input, " " );
if(token != 0)
{
strcpy( cmd, token );
token = strtok( 0, " " );
num_param = 0;
while(token != 0)
{
strcpy(strParam[num_param++], token);
token = strtok( 0, " " );
}
if(strcmp(cmd, "exit") == 0)
{
if(AskSave() == false)
break;
}
if(strcmp(cmd, "re") == 0)
DrawScreen();
else if(strcmp(cmd, "save") == 0)
{
Walking::GetInstance()->SaveINISettings(ini);
SaveCmd();
}
else if(strcmp(cmd, "mon") == 0)
{
MonitorCmd();
}
else if(strcmp(cmd, "help") == 0)
HelpCmd();
else
PrintCmd("Bad command! please input 'help'");
}
}
EndCommandMode();
}
}
DrawEnding();
exit(0);
}
int main(int argc, char *argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
int ch;
char filename[128];
change_current_dir();
if(argc < 2)
strcpy(filename, MOTION_FILE_PATH); // Set default motion file path
else
strcpy(filename, argv[1]);
minIni* ini = new minIni(INI_FILE_PATH);
/////////////// 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("Initializing Motion Manager failed!\n");
return 0;
}
MotionManager::GetInstance()->SetEnable(false);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Stop();
//MotionManager::GetInstance()->StopThread();
/////////////////////////////////////////////////////////////////////
MotionManager::GetInstance()->LoadINISettings(ini);
DrawIntro(&cm730);
while(1)
{
ch = _getch();
if(ch == 0x1b)
{
ch = _getch();
if(ch == 0x5b)
{
ch = _getch();
if(ch == 0x41) // Up arrow key
MoveUpCursor();
else if(ch == 0x42) // Down arrow key
MoveDownCursor();
else if(ch == 0x44) // Left arrow key
MoveLeftCursor();
else if(ch == 0x43) // Right arrow key
MoveRightCursor();
}
}
else if( ch == '[' )
UpDownValue(&cm730, -1);
else if( ch == ']' )
UpDownValue(&cm730, 1);
else if( ch == '{' )
UpDownValue(&cm730, -10);
else if( ch == '}' )
UpDownValue(&cm730, 10);
else if( ch == ' ' )
ToggleTorque(&cm730);
else if( ch >= 'A' && ch <= 'z' )
{
char input[128] = {0,};
char *token;
int input_len;
char cmd[80];
int num_param;
int iparam[30];
int idx = 0;
BeginCommandMode();
printf("%c", ch);
input[idx++] = (char)ch;
while(1)
{
ch = _getch();
if( ch == 0x0A )
break;
else if( ch == 0x7F )
{
if(idx > 0)
{
ch = 0x08;
printf("%c", ch);
ch = ' ';
printf("%c", ch);
ch = 0x08;
printf("%c", ch);
input[--idx] = 0;
}
}
else if( ( ch >= 'A' && ch <= 'z' ) || ch == ' ' || ( ch >= '0' && ch <= '9'))
{
if(idx < 127)
{
printf("%c", ch);
input[idx++] = (char)ch;
}
}
}
fflush(stdin);
input_len = strlen(input);
if(input_len > 0)
{
token = strtok( input, " " );
if(token != 0)
{
strcpy( cmd, token );
token = strtok( 0, " " );
num_param = 0;
while(token != 0)
{
iparam[num_param++] = atoi(token);
token = strtok( 0, " " );
}
if(strcmp(cmd, "exit") == 0)
{
if(AskSave() == false)
break;
}
else if(strcmp(cmd, "re") == 0)
{
ReadStep(&cm730);
DrawPage();
}
else if(strcmp(cmd, "help") == 0)
HelpCmd();
else if(strcmp(cmd, "set") == 0)
{
if(num_param > 0)
SetValue(&cm730, iparam[0]);
else
PrintCmd("Need parameter");
}
else if(strcmp(cmd, "on") == 0)
OnOffCmd(&cm730, true, num_param, iparam);
else if(strcmp(cmd, "off") == 0)
OnOffCmd(&cm730, false, num_param, iparam);
else if(strcmp(cmd, "save") == 0)
SaveCmd(ini);
else if(strcmp(cmd, "pgain") == 0)
{
if(num_param > 0)
GainCmd(&cm730, iparam[0], P_GAIN_COL);
else
PrintCmd("Need parameter");
}
else if(strcmp(cmd, "igain") == 0)
{
if(num_param > 0)
GainCmd(&cm730, iparam[0], I_GAIN_COL);
else
PrintCmd("Need parameter");
}
else if(strcmp(cmd, "dgain") == 0)
{
if(num_param > 0)
GainCmd(&cm730, iparam[0], D_GAIN_COL);
else
PrintCmd("Need parameter");
}
else
PrintCmd("Bad command! please input 'help'");
}
}
EndCommandMode();
}
}
DrawEnding();
return 0;
}
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(int argc, char *argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
char filename[128];
change_current_dir();
if(argc < 2)
strcpy(filename, MOTION_FILE_PATH); // Set default motion file path
else
strcpy(filename, argv[1]);
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Initializing Motion Manager failed!\n");
exit(0);
}
MotionManager::GetInstance()->SetEnable(false);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
linuxMotionTimer.Initialize(MotionManager::GetInstance());
linuxMotionTimer.Stop();
/////////////////////////////////////////////////////////////////////
head_ctrl.SetMode( EMOHead::COMMAND );
/* BLINKS */
for ( int i = 0; i < 10; ++i )
{
blink_with_delay( 200000 );
}
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_chappie.mp3");
/* EYE DIRECTIONS */
head_ctrl.SetLookDirection( EMOHead::LOOK_LEFT );
sleep( 1 );
head_ctrl.SetLookDirection( EMOHead::LOOK_RIGHT );
sleep( 1 );
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_chicken.mp3");
head_ctrl.SetLookDirection( EMOHead::LOOK_LEFT );
sleep( 1 );
head_ctrl.SetLookDirection( EMOHead::LOOK_RIGHT );
sleep( 1 );
head_ctrl.SetLookDirection( EMOHead::LOOK_FORWARD );
sleep( 1 );
/* ANTENNA POSITIONS */
head_ctrl.SetSpeeds( 64, 64, 64 );
head_ctrl.SetPositions( 255, 255, 255 );
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_whats_that.mp3");
sleep( 1 );
head_ctrl.SetSpeeds( 8, 8, 8 );
head_ctrl.SetPositions( 0, 0, 0 );
sleep( 1 );
head_ctrl.SetSpeeds( 32, 32, 32 );
head_ctrl.SetPositions( 128, 128, 128 );
sleep( 1 );
head_ctrl.SetSpeeds( 8, 8, 8 );
head_ctrl.SetPositions( 255, 0, 255 );
sleep( 1 );
head_ctrl.SetSpeeds( 64, 64, 64 );
head_ctrl.SetPositions( 0, 255, 0 );
sleep( 1 );
head_ctrl.SetSpeeds( 32, 32, 32 );
head_ctrl.SetPositions( 128, 128, 128 );
sleep( 1 );
head_ctrl.SetSpeeds( 4, 4, 4 );
head_ctrl.SetPositions( 0, 0, 0 );
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_no_he_can_talk_hes_smart.mp3");
/* EYE DOT PATTERN */
for ( int i = 0; i < 0xff; ++i )
{
head_ctrl.SetEyeDotsRight( i );
head_ctrl.SetEyeDotsLeft( i );
usleep( 10000 );
}
head_ctrl.SetEyeDots( EMOHead::DOTS_UP );
sleep( 1 );
head_ctrl.SetEyeDots( EMOHead::DOTS_DOWN );
sleep( 1 );
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_i_cant_do_a_heist.mp3");
head_ctrl.SetEyeDotsRight( EMOHead::DOTS_WORRIED_RIGHT );
head_ctrl.SetEyeDotsLeft( EMOHead::DOTS_WORRIED_LEFT );
sleep( 3 );
head_ctrl.SetEyeDotsRight( EMOHead::DOTS_SAD_RIGHT );
head_ctrl.SetEyeDotsLeft( EMOHead::DOTS_SAD_LEFT );
sleep( 1 );
head_ctrl.SetEyeDots( EMOHead::DOTS_STAR );
sleep( 1 );
head_ctrl.SetEyeDots( EMOHead::DOTS_PLUS );
sleep( 1 );
LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_clever_little_robot.mp3");
//ASCII art style spinner on high res OLED displays =P
for ( int i=0; i < 5; ++i )
{
head_ctrl.SetEyeDots( EMOHead::DOTS_BACKSLASH );
usleep( 100000 );
head_ctrl.SetEyeDots( EMOHead::DOTS_BAR );
usleep( 100000 );
head_ctrl.SetEyeDots( EMOHead::DOTS_FWDSLASH );
usleep( 100000 );
head_ctrl.SetEyeDots( EMOHead::DOTS_MINUS );
usleep( 100000 );
}
head_ctrl.SetEyeDots( EMOHead::DOTS_SHIP );
sleep( 1 );
head_ctrl.SetEyeDots( EMOHead::DOTS_FULL );
//head_ctrl.SetMode( EMOHead::DEMO );
exit(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;
}