示例#1
0
// 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;
}
示例#2
0
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);
}
示例#3
0
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();
   
}
示例#4
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')	
示例#5
0
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);
}
示例#6
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;
}
示例#7
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;
}
示例#8
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);
}
示例#9
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;
}