void StatusCheck::Check(CM730 &cm730)
{
    struct timeval _tv;
    gettimeofday(&_tv, NULL);
    double _curr_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);

    // let darwin to getup if it has fall down
    //if(MotionStatus::FALLEN != STANDUP && m_cur_mode == SOCCER && m_is_started != 0) 
    if(MotionStatus::FALLEN != STANDUP && (m_cur_mode == SOCCER || m_cur_mode == FACE) && m_is_started != 0) // modified by Judy
    {
        Walking::GetInstance()->Stop();
        while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);

        Action::GetInstance()->m_Joint.SetEnableBody(true, true);

        if(MotionStatus::FALLEN == FORWARD)
            Action::GetInstance()->Start(10);   // FORWARD GETUP
        else if(MotionStatus::FALLEN == BACKWARD)
            Action::GetInstance()->Start(11);   // BACKWARD GETUP

        while(Action::GetInstance()->IsRunning() == 1) usleep(8000);

        Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
        Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
    }

    if(m_cur_mode == SOCCER && m_is_started == 2)
    {
        if(m_soccer_sub_mode == SOCCER_PLAY && (_curr_time - m_soccer_start_time) > PLAY_TIME)
        {
            Walking::GetInstance()->Stop();
            while(Walking::GetInstance()->IsRunning() == true) usleep(8000);

            Action::GetInstance()->m_Joint.SetEnableBody(true, true);

            while(Action::GetInstance()->Start(15) == false) usleep(8000);
            while(Action::GetInstance()->IsRunning() == true) usleep(8000);

            // TODO: Torque OFF??
            cm730.WriteByte(254, MX28::P_TORQUE_ENABLE, 0, NULL);

            m_soccer_sub_mode = SOCCER_REST;
            gettimeofday(&_tv, NULL);
            m_soccer_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
        }
        else if(m_soccer_sub_mode == SOCCER_REST && (_curr_time - m_soccer_start_time) > REST_TIME)
        {
            MotionManager::GetInstance()->Reinitialize();
            MotionManager::GetInstance()->SetEnable(true);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Start soccer demonstration.mp3");

            Action::GetInstance()->m_Joint.SetEnableBody(true, true);

            while(Action::GetInstance()->Start(15) == false) usleep(8000);
            while(Action::GetInstance()->IsRunning() == true) usleep(8000);

            Action::GetInstance()->Start(9);
            while(Action::GetInstance()->IsRunning() == true) usleep(8000);

            Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
            Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

            MotionManager::GetInstance()->ResetGyroCalibration();
            while(1)
            {
                if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
                {
                    LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
                    break;
                }
                else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
                {
                    LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
                    MotionManager::GetInstance()->ResetGyroCalibration();
                }
                usleep(8000);
            }

            m_soccer_sub_mode = SOCCER_PLAY;
            gettimeofday(&_tv, NULL);
            m_soccer_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
        }
    }

    /**** added by Judy ****/
    if(m_cur_mode == FACE && m_is_started == 2)
    {
 
        if (MusicSocket::faceDetect ==-1){
            LinuxActionScript::PlayMP3("../../../Data/mp3/Face detection disabled.mp3");
        }else{

           if(m_face_sub_mode == FACE_DETECT && (_curr_time - m_face_start_time) > PLAY_TIME)
           {
               Walking::GetInstance()->Stop();
               while(Walking::GetInstance()->IsRunning() == true) usleep(8000);
   
               Action::GetInstance()->m_Joint.SetEnableBody(true, true);
   
               while(Action::GetInstance()->Start(15) == false) usleep(8000);
               while(Action::GetInstance()->IsRunning() == true) usleep(8000);
   
               // TODO: Torque OFF??
               cm730.WriteByte(254, MX28::P_TORQUE_ENABLE, 0, NULL);
   
               m_face_sub_mode = FACE_REST;
               gettimeofday(&_tv, NULL);
               m_face_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
           }
           else if(m_face_sub_mode == FACE_REST && (_curr_time - m_soccer_start_time) > REST_TIME)
           {
               MotionManager::GetInstance()->Reinitialize();
               MotionManager::GetInstance()->SetEnable(true);
               LinuxActionScript::PlayMP3("../../../Data/mp3/Start face detection demonstration.mp3");
   
               Action::GetInstance()->m_Joint.SetEnableBody(true, true);
   
               while(Action::GetInstance()->Start(15) == false) usleep(8000);
               while(Action::GetInstance()->IsRunning() == true) usleep(8000);
   
               Action::GetInstance()->Start(9);
               while(Action::GetInstance()->IsRunning() == true) usleep(8000);
   
               Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
               Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
   
               MotionManager::GetInstance()->ResetGyroCalibration();
               while(1)
               {
                   if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
                   {
                       LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
                       break;
                   }
                   else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
                   {
                       LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
                       MotionManager::GetInstance()->ResetGyroCalibration();
                   }
                   usleep(8000);
               }
   
               m_face_sub_mode = FACE_DETECT;
               gettimeofday(&_tv, NULL);
               m_face_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
           }
        }
    }
    /**** end ****/

    if(m_old_btn == MotionStatus::BUTTON)
    {
        if(m_cur_mode == READY && m_is_started == 0 && m_chicago_mode_cnt > MAX_CHICAGO_CNT)
        {
            fprintf(stderr, "CHICAGO MODE ! \n\n");
			m_cur_mode = SOCCER;
			m_old_btn == BTN_START;
        }
        else if(m_cur_mode == READY && m_is_started == 0 && m_chicago_mode_cnt <= MAX_CHICAGO_CNT)
        {
            m_chicago_mode_cnt++;
			return;
        }
		else
        {
            m_chicago_mode_cnt = 0;
            return;
        }
    }
    else
    {
        m_chicago_mode_cnt = 0;
        m_old_btn = MotionStatus::BUTTON;
    }

    if(m_old_btn & BTN_MODE)
    {
        fprintf(stderr, "Mode button pressed.. \n");

        if(m_is_started != 0)
        {
            m_is_started    = 0;
            m_cur_mode      = READY;
            LinuxActionScript::m_stop = 1;

            Walking::GetInstance()->Stop();
            Action::GetInstance()->m_Joint.SetEnableBody(true, true);

            while(Action::GetInstance()->Start(15) == false) usleep(8000);
            while(Action::GetInstance()->IsRunning() == true) usleep(8000);
        }
        else
        {
            m_cur_mode++;
            if(m_cur_mode >= MAX_MODE) m_cur_mode = READY;
        }

        MotionManager::GetInstance()->SetEnable(false);
        usleep(10000);

        if(m_cur_mode == READY)
        {
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3");
        }
        else if(m_cur_mode == SOCCER)
        {
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x01, NULL);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Autonomous soccer mode.mp3");
        }
        else if(m_cur_mode == MOTION)
        {
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Interactive motion mode.mp3");
        }
        else if(m_cur_mode == VISION)
        {
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x04, NULL);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Vision processing mode.mp3");
        
        }else if(m_cur_mode == FACE){ // added by Judy
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x01, NULL);
            LinuxActionScript::PlayMP3("../../../Data/mp3/Face processing mode.mp3");
        }
    }

    if(m_old_btn & BTN_START)
    {
        if(m_is_started == 0)
        {
            fprintf(stderr, "Start button pressed.. & started is false.. \n");

            if(m_cur_mode == SOCCER)
            {
                MotionManager::GetInstance()->Reinitialize();
                MotionManager::GetInstance()->SetEnable(true);
                if(m_chicago_mode_cnt > MAX_CHICAGO_CNT)
                {
				    m_chicago_mode_cnt = 0;
                    m_is_started = 2;
                }
				else
                    m_is_started = 1;
                LinuxActionScript::PlayMP3("../../../Data/mp3/Start soccer demonstration.mp3");

                Action::GetInstance()->m_Joint.SetEnableBody(true, true);

                Action::GetInstance()->Start(9);
                while(Action::GetInstance()->IsRunning() == true) usleep(8000);

                Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

                MotionManager::GetInstance()->ResetGyroCalibration();
                while(1)
                {
                    if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
                    {
                        LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
                        break;
                    }
                    else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
                    {
                        LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
                        MotionManager::GetInstance()->ResetGyroCalibration();
                    }
                    usleep(8000);
                }

                m_soccer_sub_mode = SOCCER_PLAY;
                gettimeofday(&_tv, NULL);
                m_soccer_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
            }
            else if(m_cur_mode == FACE) // added by Judy
            {

                MotionManager::GetInstance()->Reinitialize();
                MotionManager::GetInstance()->SetEnable(true);
                if(m_chicago_mode_cnt > MAX_CHICAGO_CNT)
                {
                    m_chicago_mode_cnt = 0;
                    m_is_started = 2;
                }
                else
                    m_is_started = 1;
                if(MusicSocket::faceDetect == -1){
                    LinuxActionScript::PlayMP3("../../../Data/mp3/Face detection disabled.mp3");
                }else{
                    LinuxActionScript::PlayMP3("../../../Data/mp3/Start face detection demonstration.mp3");
    
                    Action::GetInstance()->m_Joint.SetEnableBody(true, true);
    
                    Action::GetInstance()->Start(9);
                    while(Action::GetInstance()->IsRunning() == true) usleep(8000);
    
                    Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                    Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
    
                    MotionManager::GetInstance()->ResetGyroCalibration();
                    while(1)
                    {
                        if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
                        {
                            LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
                            break;
                        }
                        else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
                        {
                            LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
                            MotionManager::GetInstance()->ResetGyroCalibration();
                        }
                        usleep(8000);
		    }

                    m_face_sub_mode = FACE_DETECT;
                    gettimeofday(&_tv, NULL);
                    m_face_start_time = _tv.tv_sec + (_tv.tv_usec/1000000.0);
                }   
    
            }
            /**** end ****/
            else if(m_cur_mode == MOTION)
            {
                MotionManager::GetInstance()->Reinitialize();
                MotionManager::GetInstance()->SetEnable(true);
                m_is_started = 1;
                LinuxActionScript::PlayMP3("../../../Data/mp3/Start motion demonstration.mp3");

                // Joint Enable..
                Action::GetInstance()->m_Joint.SetEnableBody(true, true);

                Action::GetInstance()->Start(1);
                while(Action::GetInstance()->IsRunning() == true) usleep(8000);
            }
            else if(m_cur_mode == VISION)
            {
                MotionManager::GetInstance()->Reinitialize();
                MotionManager::GetInstance()->SetEnable(true);
                m_is_started = 1;
                LinuxActionScript::PlayMP3("../../../Data/mp3/Start vision processing demonstration.mp3");

                // Joint Enable...
                Action::GetInstance()->m_Joint.SetEnableBody(true, true);

                Action::GetInstance()->Start(1);
                while(Action::GetInstance()->IsRunning() == true) usleep(8000);
            }
        }
        else
        {
            fprintf(stderr, "Start button pressed.. & started is true.. \n");
        }
    }
}
Exemple #2
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(int argc, char *argv[])
{
	signal(SIGABRT, &sighandler);
    signal(SIGTERM, &sighandler);
	signal(SIGQUIT, &sighandler);
	signal(SIGINT, &sighandler);

	int ch;
	char filename[128];

    fprintf(stderr, "\n***********************************************************************\n");
    fprintf(stderr,   "*                      RoboPlus Server Program                        *\n");
    fprintf(stderr,   "***********************************************************************\n\n");

	change_current_dir();
	if(argc < 2)
		strcpy(filename, MOTION_FILE_PATH); // Set default motion file path
	else
		strcpy(filename, argv[1]);

	/////////////// Load/Create Action File //////////////////
	if(Action::GetInstance()->LoadFile(filename) == false)
	{
		printf("Can not open %s\n", filename);
		printf("Do you want to make a new action file? (y/n) ");
		ch = _getch();
		if(ch != 'y')
		{
			printf("\n");
			return 0;
		}

		if(Action::GetInstance()->CreateFile(filename) == false)
		{
			printf("Fail to create %s\n", filename);
			return 0;
		}
	}
	////////////////////////////////////////////////////////////


	//////////////////// Framework Initialize ////////////////////////////	
	if(MotionManager::GetInstance()->Initialize(&cm730) == false)
	{
		printf("Fail to initialize Motion Manager!\n");
			return 0;
	}
	MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());	
    LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
    motion_timer->Stop();
	/////////////////////////////////////////////////////////////////////

    int firm_ver = 0;
    if(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0)  != CM730::SUCCESS)
    {
        fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN);
        exit(0);
    }

    if(27 > firm_ver)
    {
        fprintf(stderr, "The RoboPlus(ver 1.0.23.0 or higher) Motion is not supported 1024 resolution..\n\n");
        fprintf(stderr, "Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
        exit(0);
    }

	std::cout << "[Running....]\n";
    try
    {
        // Create the socket
		LinuxServer new_sock;
        LinuxServer server ( TCPIP_PORT );

        while ( true )
        {
            std::cout << "[Waiting..]" << std::endl;            
            server.accept ( new_sock );
            std::cout << "[Accepted..]" << std::endl;

            try
            {
                while ( true )
                {
                    std::string data;
					Action::PAGE page;

                    new_sock >> data;
					std::cout << data << std::endl;

                    std::string* p_str_tok = std::string_split(data, " ");

                    if(p_str_tok[0] == "v")
                    {
                        new_sock << "{[DARwIn:" << VERSION << "]}\n";
                    }
                    else if(p_str_tok[0] == "E")
                    {
                        new_sock << "{[DARwIn:1.000]}";
                        new_sock << "{[PC:TCP/IP][DXL:1000000(BPS)]}";
						new_sock << "{";
						for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
							new_sock << "[" << id << ":029(MX-28)]";
						new_sock << "}";
                        new_sock << "{[DXL:" << JointData::NUMBER_OF_JOINTS << "(PCS)]}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "exit")
                    {
						Action::GetInstance()->Stop();
                    }
                    else if(p_str_tok[0] == "List")
                    {
                        new_sock << "{";
                        for(int i = 0; i < 256; i++)
						{
							Action::GetInstance()->LoadPage(i, &page);
                            new_sock << "[" << i << ":";
							new_sock.send(page.header.name, 14);
							new_sock << ":" << (int)page.header.next << ":" << (int)page.header.exit << ":" << (int)page.header.stepnum << "]";
						}
                        new_sock << "}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "Get" || p_str_tok[0] == "on" || p_str_tok[0] == "off")
                    {
						if(p_str_tok[0] == "on" || p_str_tok[0] == "off")
						{
							int torque;
							if(p_str_tok[0] == "on")
							{
								torque = 1;
								
							}
							else if(p_str_tok[0] == "off")
							{
								torque = 0;
							}

							int i;
							for(i=1; i<ARGUMENT_NAXNUM; i++)
							{
								if(p_str_tok[i].length() > 0)
								{
									cm730.WriteByte(atoi(p_str_tok[i].c_str()), MX28::P_TORQUE_ENABLE, torque, 0);
								}
								else
									break;
							}

							if(i == 1)
							{
								for(int id=1; id<JointData::NUMBER_OF_JOINTS; id++)
									cm730.WriteByte(id, MX28::P_TORQUE_ENABLE, torque, 0);
							}
						}

						int torque, position;
						new_sock << "{";
						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							new_sock << "[";
							std::cout << "[";
							if(id >= 1 && id < JointData::NUMBER_OF_JOINTS)
							{
								if(cm730.ReadByte(id, MX28::P_TORQUE_ENABLE, &torque, 0) == CM730::SUCCESS)
								{
									if(torque == 1)
									{
										if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &position, 0) == CM730::SUCCESS)
										{
											new_sock << position;
											std::cout << position;
										}
										else
										{
											new_sock << "----";
											std::cout << "Fail to read present position ID(" << id << ")";
										}
									}
									else
									{
										new_sock << "????";
										std::cout << "????";
									}
								}
								else
								{
									new_sock << "----";
									std::cout << "Fail to read torque ID(" << id << ")";
								}
							}
							else
							{
								new_sock << "----";
								std::cout << "----";
							}
							new_sock << "]";
							std::cout << "]";
						}
						std::cout << std::endl;
						new_sock << "}";
                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "go")
                    {
						int GoalPosition, StartPosition, Distance;
						
						new_sock << "{";
						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							if(id >= 1 && id < JointData::NUMBER_OF_JOINTS)
							{
								if(cm730.ReadWord(id, MX28::P_PRESENT_POSITION_L, &StartPosition, 0) == CM730::SUCCESS)
								{
									GoalPosition = (int)atoi(p_str_tok[id + 1].c_str());

									if( StartPosition > GoalPosition )
										Distance = StartPosition - GoalPosition;
									else
										Distance = GoalPosition - StartPosition;

									Distance >>= 2;
									if( Distance < 8 )
										Distance = 8;

									cm730.WriteWord(id, MX28::P_MOVING_SPEED_L, Distance, 0);
									if(cm730.WriteWord(id, MX28::P_GOAL_POSITION_L, GoalPosition, 0) == CM730::SUCCESS)
									{
										new_sock << "[" << GoalPosition << "]";
										std::cout << "[" << GoalPosition << "]";
									}
									else
									{
										new_sock << "[----]";
										std::cout << "[----]";
									}
								}
								else
								{
									new_sock << "[----]";
									std::cout << "[----]";
								}
							}
							else
							{
								new_sock << "[----]";
								std::cout << "[----]";
							}
						}
						std::cout << std::endl;
						new_sock << "}";
						new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "set")
                    {
						int id = (int)atoi(p_str_tok[1].c_str());
						int position = (int)atoi(p_str_tok[2].c_str());

						if(cm730.WriteWord(id, MX28::P_GOAL_POSITION_L, position, 0) == CM730::SUCCESS)
						{
							if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &position, 0) == CM730::SUCCESS)
							{
								std::cout << "{[" << position << "]}";
								new_sock << "{[" << position << "]}";
							}
							else
							{
								std::cout << "[Fail to read goal position ID(" << id << ")]";
								new_sock << "{[----]}";
							}
						}
						else
						{
							std::cout << "[Fail to write goal position ID(" << id << ")]";
							new_sock << "{[----]}";
						}
						std::cout << std::endl;
						new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "play" || p_str_tok[0] == "rplay")
                    {						
						int value;

						for(int id=0; id<ROBOPLUS_JOINT_MAXNUM; id++)
						{
							if(id >= JointData::ID_R_SHOULDER_PITCH && id <= JointData::ID_HEAD_TILT)
							{
								if(cm730.ReadWord(id, MX28::P_TORQUE_ENABLE, &value, 0) == CM730::SUCCESS)
								{
									if(value == 0)
									{
										if(cm730.ReadWord(id, MX28::P_PRESENT_POSITION_L, &value, 0) == CM730::SUCCESS)
											MotionStatus::m_CurrentJoints.SetValue(id, value);
										else
											std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
									}
									else
									{
										if(cm730.ReadWord(id, MX28::P_GOAL_POSITION_L, &value, 0) == CM730::SUCCESS)
											MotionStatus::m_CurrentJoints.SetValue(id, value);
										else
											std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
									}
								}
								else
									std::cout << "[Fail to communication ID(" << id << ")]" << std::endl;
							}
						}														
						
					    motion_timer->Start();
						MotionManager::GetInstance()->SetEnable(true);
						
						int index = (int)atoi(p_str_tok[1].c_str());
						if(p_str_tok[0] == "play")
							Action::GetInstance()->Start(index);
						else
							Action::GetInstance()->Start(index, &page);
                    }
					else if(p_str_tok[0] == "info")
					{
						int ipage, istep;
						if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
						{
							new_sock << "{[" << ipage << ":" << istep << "]}\n";
							std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
						}
						else
						{
							new_sock << "{[OK]}\n";
							std::cout << "[END]" << std::endl;
							MotionManager::GetInstance()->SetEnable(false);
							motion_timer->Stop();
						}
					}
                    else if(p_str_tok[0] == "stop")
                    {
						Action::GetInstance()->Stop();
                    }
                    else if(p_str_tok[0] == "stopinfo")
                    {
                        Action::GetInstance()->Stop();

                        int ipage, istep;
                        if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
                        {
                            new_sock << "{[" << ipage << ":" << istep << "]}\n";
                            std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
                        }
                        else
                        {
                            new_sock << "{[OK]}\n";
                            std::cout << "[END]" << std::endl;
                            MotionManager::GetInstance()->SetEnable(false);
                            motion_timer->Stop();
                        }
                    }
					else if(p_str_tok[0] == "break")
                    {
						Action::GetInstance()->Brake();
                    }
                    else if(p_str_tok[0] == "breakinfo")
                    {
                        Action::GetInstance()->Brake();

                        int ipage, istep;
                        if(Action::GetInstance()->IsRunning(&ipage, &istep) == 1)
                        {
                            new_sock << "{[" << ipage << ":" << istep << "]}\n";
                            std::cout << "[" << ipage << ":" << istep << "]" << std::endl;
                        }
                        else
                        {
                            new_sock << "{[OK]}\n";
                            std::cout << "[END]" << std::endl;
                            MotionManager::GetInstance()->SetEnable(false);
                            motion_timer->Stop();
                        }
                    }
                    else if(p_str_tok[0] == "RDownload")
                    {
						int rcv_len = (int)sizeof(Action::PAGE);
						int i_data = 0;
						unsigned char *data = (unsigned char*)&page;

                        new_sock << "{[READY]}\n";
						while(rcv_len > 0)
						{
							i_data += new_sock.recv(data, rcv_len);
							data += i_data;
							rcv_len -= i_data;
						}
                        new_sock << "{[SUCCESS]}\n";
                        new_sock << "{[ME]}\n";
                    }                    
                    else if(p_str_tok[0] == "upload")
                    {
                        // send page data
						int index = (int)(atoi(p_str_tok[1].c_str()) / sizeof(Action::PAGE));
						int num = (int)(atoi(p_str_tok[2].c_str()) / sizeof(Action::PAGE));
						
						for(int i=0; i<num; i++)
						{
							Action::GetInstance()->LoadPage(index + i, &page);
							new_sock.send((unsigned char*)&page, (int)sizeof(Action::PAGE));
						}

                        new_sock << "{[ME]}\n";
                    }
                    else if(p_str_tok[0] == "ld")
                    {
						int index = (int)(atoi(p_str_tok[1].c_str()) / sizeof(Action::PAGE));
						int num = (int)(atoi(p_str_tok[2].c_str()) / sizeof(Action::PAGE));
						int rcv_len;
						int i_data;
						unsigned char *data;

						std::cout << "[READY]" << std::endl;
                        new_sock << "{[READY]}\n";

                        // byte stream
						for(int i=0; i<num; i++)
						{
							i_data = 0;
							rcv_len = (int)sizeof(Action::PAGE);
							data = (unsigned char*)&page;
							while(rcv_len > 0)
							{
								i_data += new_sock.recv(data, rcv_len);
								data += i_data;
								rcv_len -= i_data;
							}
							Action::GetInstance()->SavePage(index + i, &page);
							std::cout << "[SAVE:" << index + i << "]" << std::endl;
						}
                    }
					else
					{
						std::cout << " [Invalid:" << p_str_tok[0] << "]" << std::endl;
					}
                }
int main(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;
}
Exemple #5
0
int main()
{
	signal(SIGABRT, &sighandler);
	signal(SIGTERM, &sighandler);
	signal(SIGQUIT, &sighandler);
	signal(SIGINT, &sighandler);

	char input[MAXNUM_INPUTCHAR];
	char *token;
	int input_len;
	char cmd[80];
	char param[20][30];
	int num_param;
	int iparam[20];	

	printf( "\n[Dynamixel Monitor for DARwIn %s]\n", PROGRAM_VERSION);

	if(cm730.Connect() == true)
	{
		Scan(&cm730);

		while(1)
		{
			Prompt(gID);
			gets(input);
			fflush(stdin);
			input_len = strlen(input);
			if(input_len == 0)
				continue;

			token = strtok( input, " " );
			if(token == 0)
				continue;

			strcpy( cmd, token );
			token = strtok( 0, " " );
			num_param = 0;
			while(token != 0)
			{
				strcpy( param[num_param++], token );
				token = strtok( 0, " " );
			}

			if(strcmp(cmd, "exit") == 0)
				break;
			else if(strcmp(cmd, "scan") == 0)
				Scan(&cm730);
			else if(strcmp(cmd, "help") == 0)
				Help();
			else if(strcmp(cmd, "id") == 0)
			{
				if(num_param != 1)
				{
					printf(" Invalid parameter!\n");
					continue;
				}
				
				iparam[0] = atoi(param[0]);
				if(cm730.Ping(iparam[0], 0) == CM730::SUCCESS)
				{
					gID = iparam[0];
				}
				else
				{
					printf(" Invalid ID(%d)!\n", iparam[0]);
					continue;
				}
			}
			else if(strcmp(cmd, "on") == 0)
			{
				if(num_param == 0)
				{
					cm730.WriteByte(gID, MX28::P_TORQUE_ENABLE, 1, 0);
					if(gID == CM730::ID_CM)
						printf(" Dynamixel power on\n");
				}
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							cm730.WriteByte(i, MX28::P_TORQUE_ENABLE, 1, 0);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "off") == 0)
			{
				if(num_param == 0)
				{
					cm730.WriteByte(gID, MX28::P_TORQUE_ENABLE, 0, 0);
					if(gID == CM730::ID_CM)
						printf(" Dynamixel power off\n");
				}
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							cm730.WriteByte(i, MX28::P_TORQUE_ENABLE, 0, 0);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "d") == 0)
				Dump(&cm730, gID);
			else if(strcmp(cmd, "reset") == 0)
			{
				int firm_ver = 0;
				if(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0)  != CM730::SUCCESS)
				{
					fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN);
					exit(0);
				}

				if(0 < firm_ver && firm_ver < 27)
				{
					fprintf(stderr, "\n MX-28's firmware is not support 4096 resolution!! \n");
					fprintf(stderr, " Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
					continue;
				}

				if(num_param == 0)
					Reset(&cm730, gID);
				else if(num_param == 1)
				{
					if(strcmp(param[0], "all") == 0)
					{
						for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++)
							Reset(&cm730, i);

						if(cm730.Ping(FSR::ID_R_FSR, 0) == CM730::SUCCESS)
							Reset(&cm730, FSR::ID_R_FSR);
						if(cm730.Ping(FSR::ID_L_FSR, 0) == CM730::SUCCESS)
							Reset(&cm730, FSR::ID_L_FSR);
						Reset(&cm730, CM730::ID_CM);
					}
					else
					{
						printf(" Invalid parameter!\n");
						continue;
					}
				}
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else if(strcmp(cmd, "wr") == 0)
			{
				if(num_param == 2)
					Write(&cm730, gID, atoi(param[0]), atoi(param[1]));
				else
				{
					printf(" Invalid parameter!\n");
					continue;
				}
			}
			else
				printf(" Bad command! please input 'help'.\n");
		}
	}
	else
		printf("Failed to connect CM-730!");

	printf("\nTerminated DXL Manager.\n");
	return 0;
}
Exemple #6
0
void StatusCheck::Check(CM730 &cm730)
{
	if(MotionStatus::FALLEN != STANDUP && m_cur_mode == SOCCER && m_is_started == 1)
	{
		Walking::GetInstance()->Stop();
		while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);

		Action::GetInstance()->m_Joint.SetEnableBody(true, true);

		if(MotionStatus::FALLEN == FORWARD)
			Action::GetInstance()->Start(10);   // FORWARD GETUP
		else if(MotionStatus::FALLEN == BACKWARD)
			Action::GetInstance()->Start(11);   // BACKWARD GETUP

		while(Action::GetInstance()->IsRunning() == 1) usleep(8000);

		Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
		Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
	}

	if(m_old_btn == MotionStatus::BUTTON)
		return;

	m_old_btn = MotionStatus::BUTTON;

	if(m_old_btn & BTN_MODE)
	{
		fprintf(stderr, "Mode button pressed.. \n");

		if(m_is_started == 1)
		{
			m_is_started    = 0;
			m_cur_mode      = READY;
			LinuxActionScript::m_stop = 1;

			Walking::GetInstance()->Stop();
			//if (MotionManager::GetInstance()->IsLogging() == true) {
			//	MotionManager::GetInstance()->StopLogging();
			//}
			if (MotionManager::GetInstance()->IsStreaming() == true) {
				MotionManager::GetInstance()->StopStreaming();
			}
			Action::GetInstance()->m_Joint.SetEnableBody(true, true);

			while(Action::GetInstance()->Start(15) == false) usleep(8000);
			while(Action::GetInstance()->IsRunning() == true) usleep(8000);
		}
		else
		{
			m_cur_mode++;
			if(m_cur_mode >= MAX_MODE) m_cur_mode = READY;
		}

		MotionManager::GetInstance()->SetEnable(false);
		usleep(10000);

		if(m_cur_mode == READY)
		{
			cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
			LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3");
		}
		else if(m_cur_mode == SOCCER)
		{
			cm730.WriteByte(CM730::P_LED_PANNEL, 0x01, NULL);
			LinuxActionScript::PlayMP3("../../../Data/mp3/Autonomous soccer mode.mp3");
		}
		else if(m_cur_mode == MOTION)
		{
			cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL);
			LinuxActionScript::PlayMP3("../../../Data/mp3/Interactive motion mode.mp3");
		}
		else if(m_cur_mode == VISION)
		{
			cm730.WriteByte(CM730::P_LED_PANNEL, 0x04, NULL);
			LinuxActionScript::PlayMP3("../../../Data/mp3/Vision processing mode.mp3");
		}
	}

	if(m_old_btn & BTN_START)
	{
		if(m_is_started == 0)
		{
			fprintf(stderr, "Start button pressed.. & started is false.. \n");

			if(m_cur_mode == SOCCER)
			{
				MotionManager::GetInstance()->Reinitialize();
				MotionManager::GetInstance()->SetEnable(true);
				m_is_started = 1;
				LinuxActionScript::PlayMP3("../../../Data/mp3/Start soccer demonstration.mp3");

				Action::GetInstance()->m_Joint.SetEnableBody(true, true);

				Action::GetInstance()->Start(9);
				while(Action::GetInstance()->IsRunning() == true) usleep(8000);

				Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
				Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

				MotionManager::GetInstance()->ResetGyroCalibration();
				while(1)
				{
					if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
					{
						LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
						//MotionManager::GetInstance()->StartLogging();
						//MotionManager::GetInstance()->StartStreaming();
						sleep(2);
						break;
					}
					else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
					{
						LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
						MotionManager::GetInstance()->ResetGyroCalibration();
					}
					usleep(8000);
				}
			}
			else if(m_cur_mode == MOTION)
			{
				MotionManager::GetInstance()->Reinitialize();
				MotionManager::GetInstance()->SetEnable(true);
				m_is_started = 1;
				LinuxActionScript::PlayMP3("../../../Data/mp3/Start motion demonstration.mp3");

				// Joint Enable..
				Action::GetInstance()->m_Joint.SetEnableBody(true, true);

				Action::GetInstance()->Start(1);
				while(Action::GetInstance()->IsRunning() == true) usleep(8000);
			}
			else if(m_cur_mode == VISION)
			{
				MotionManager::GetInstance()->Reinitialize();
				MotionManager::GetInstance()->SetEnable(true);
				m_is_started = 1;
				LinuxActionScript::PlayMP3("../../../Data/mp3/Start vision processing demonstration.mp3");

				// Joint Enable...
				Action::GetInstance()->m_Joint.SetEnableBody(true, true);

				Action::GetInstance()->Start(1);
				while(Action::GetInstance()->IsRunning() == true) usleep(8000);
			}
		}
		else
		{
			fprintf(stderr, "Start button pressed.. & started is true.. \n");
		}
	}
}
Exemple #7
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;
}