예제 #1
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);
}
예제 #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 <<"]";//defines log files name
	std::string logname = "/home/robot/Logs/log";
	logname+=p.str();
    startLog(logFile,logname);
    cvflann::Logger::setDestination("/home/robot/Logs/log_rami_and_lihen");
    cvflann::Logger::setLevel(4);
    char* test_text = "works";
    cvflann::Logger::log(1,test_text);
	if (sem_init(&ball_sem, 0, 1) != 0)
	{
		printf("Couldn't sem_init\n");
	}
	for (int i=0; i<BrainBall.x_detail ;i++)
	{
		BrainBall.x_place[i] = 0;
	}



    printf( "\n===== 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(&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);
	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 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();

   //pthread_create(&VisionT, NULL, VisionThread,(void *) NULL);
     pthread_create(&gyroT, NULL, gyroThread,(void *) NULL);

    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(4);
			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')