Exemplo n.º 1
0
void StatusCheck::Check(CM730 &cm730)
{
	int value=0;

//////////////////////////////////////////////////////////////////////////////////////
// System Standby Toggle
//////////////////////////////////////////////////////////////////////////////////////		
	if(PS3.key.PS != 0)
		{
		if(Walking::GetInstance()->IsRunning() == true)
			{
			Walking::GetInstance()->Stop();
			while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
			}
		if(ToggleRobotStandby()==1)
		// wait for key release
		while(PS3.key.PS != 0) usleep(8000);
		}
	if(robotInStandby == 1) return;



//////////////////////////////////////////////////////////////////////////////////////
// IMU AUTO GETUP ROUTINE
//////////////////////////////////////////////////////////////////////////////////////		
/*
	 if(MotionStatus::FALLEN != STANDUP && (m_cur_mode == SOCCER) && m_is_started == 1)
   	{
     Walking::GetInstance()->Stop();
	 	resetLEDs(cm730);
	 	while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);

     Action::GetInstance()->m_Joint.SetEnableBody(true, true);
     while(Action::GetInstance()->IsRunning() == 1) usleep(8000);


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

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


//////////////////////////////////////////////////////////////////////////////////////
// Shut it down, sit down.
//////////////////////////////////////////////////////////////////////////////////////		
		if(PS3.key.Cross != 0)
			{
			resetLEDs(cm730);
      		Walking::GetInstance()->Stop();
			while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
			m_is_started    = 0;
      m_cur_mode      = SITTING;
      LinuxActionScript::m_stop = 1;


      Walking::GetInstance()->m_Joint.SetEnableBody(false);
      Action::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true);
      while(Action::GetInstance()->Start(16) == false) usleep(8000);
      while(Action::GetInstance()->IsRunning() == true) usleep(8000);
      Head::GetInstance()->m_Joint.SetEnableHeadOnly(true);
		while(PS3.key.Cross != 0) usleep(8000);			
			}
		

//////////////////////////////////////////////////////////////////////////////////////
// Start Walk Ready
//////////////////////////////////////////////////////////////////////////////////////
		if(PS3.key.Triangle != 0)
			{
			if(m_is_started == 0)
				{
				cm730.DXLPowerOn(true);
				}
			resetLEDs(cm730);
		  Walking::GetInstance()->Stop();
			while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
			int lastMode = m_cur_mode;
			m_cur_mode = SOCCER;      
	MotionManager::GetInstance()->Reinitialize();
    MotionManager::GetInstance()->SetEnable(true);
    m_is_started = 1;
    bLJState = bRJState = false;
    Head::GetInstance()->m_Joint.SetEnableBody(false);
    Walking::GetInstance()->m_Joint.SetEnableBody(false);
    Action::GetInstance()->m_Joint.SetEnableBody(true);

      if(lastMode == SITTING)
				Action::GetInstance()->Start(8); //50
			else
				Action::GetInstance()->Start(9); //9
      while(Action::GetInstance()->IsRunning() == true) usleep(8000);

      Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true);
      Action::GetInstance()->m_Joint.SetEnableBody(false);
			usleep(5000);
      Head::GetInstance()->m_Joint.SetEnableHeadOnly(true);
			while(PS3.key.Triangle != 0) usleep(8000);			
			}


//////////////////////////////////////////////////////////////////////////////////////
// Action Script Button Assignment
//////////////////////////////////////////////////////////////////////////////////////	

//////////////////////////////////////////////////////////////////////////////////////
// Select
//////////////////////////////////////////////////////////////////////////////////////			
		if(PS3.key.Select != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_SELECT);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.Select != 0) usleep(8000);
		}




//////////////////////////////////////////////////////////////////////////////////////
// Square
//////////////////////////////////////////////////////////////////////////////////////		
		if(PS3.key.Square != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_SQUARE);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.Square != 0) usleep(8000);
		}


//////////////////////////////////////////////////////////////////////////////////////
// Circle
//////////////////////////////////////////////////////////////////////////////////////		
		if(PS3.key.Circle != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_CIRCLE);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.Circle != 0) usleep(8000);
		}


//////////////////////////////////////////////////////////////////////////////////////
// R1
//////////////////////////////////////////////////////////////////////////////////////		
		if(PS3.key.R1 != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_R1);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.R1 != 0) usleep(8000);
		}


//////////////////////////////////////////////////////////////////////////////////////
// R2
//////////////////////////////////////////////////////////////////////////////////////
		if(PS3.key.R2 != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_R2);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.R2 != 0) usleep(8000);
		}



//////////////////////////////////////////////////////////////////////////////////////
// L1
//////////////////////////////////////////////////////////////////////////////////////
		if(PS3.key.L1 != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_L1);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.L1 != 0) usleep(8000);
		}


//////////////////////////////////////////////////////////////////////////////////////
// L2
//////////////////////////////////////////////////////////////////////////////////////		
		if(PS3.key.L2 != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_L2);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.L2 != 0) usleep(8000);
		}


//////////////////////////////////////////////////////////////////////////////////////
// Start
//////////////////////////////////////////////////////////////////////////////////////			
		if(PS3.key.Start != 0) 
		{
		if(LinuxActionScript::m_is_running == 0)
			{
			m_cur_mode = SOCCER;
		      LinuxActionScript::m_stop = 0;
		      Head::GetInstance()->m_Joint.SetEnableBody(false);
		      Walking::GetInstance()->m_Joint.SetEnableBody(false);
		      Action::GetInstance()->m_Joint.SetEnableBody(true);		
                	LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_START);
		      while(Action::GetInstance()->IsRunning() == true) usleep(8000);		
			}
		while(PS3.key.Select != 0) usleep(8000);
		}		




//////////////////////////////////////////////////////////////////////////////////////
// PS3 R/C Control code
//////////////////////////////////////////////////////////////////////////////////////


		if(Walking::GetInstance()->IsRunning() == true)
			{
			int rx,ry,dead_band=6;			
			double FBStep=0,RLTurn=0,RLStep=0,xd,yd;
			static double speedAdjSum=0;			

#ifdef Southpaw
			rx = -(PS3.key.RJoyX-128);
			ry = -(PS3.key.RJoyY-128);
#else
			rx = -(PS3.key.LJoyX-128);
			ry = -(PS3.key.LJoyY-128);			
#endif
			
//			fprintf(stderr, " (X:%d Y:%d)\n", rx, ry);
			
			if(abs(rx) > dead_band || abs(ry) > dead_band)
			{
				xd = (double)(rx-dead_band)/256;
				yd = (double)(ry-dead_band)/256;
				RLTurn = 50*xd;	
				FBStep = 45*yd;
//				fprintf(stderr, " (yd:%.1f)\n", yd);
				Walking::GetInstance()->HIP_PITCH_OFFSET = Walking::GetInstance()->HIP_PITCH_OFFSET_START + yd/2;
				if(FBStep < 0)
				{
					FBStep = 20*yd;
				}
 				speedAdjSum += yd;

				if(speedAdjSum > Walking::GetInstance()->UPPER_VELADJ_LIMIT)
				{ 
					speedAdjSum = Walking::GetInstance()->UPPER_VELADJ_LIMIT;
				}

				else if(speedAdjSum < Walking::GetInstance()->LOWER_VELADJ_LIMIT)
				{ 
					speedAdjSum = Walking::GetInstance()->LOWER_VELADJ_LIMIT;
				}
				else
				{
					speedAdjSum = 0;
				}
			}
			Walking::GetInstance()->speedAdj = speedAdjSum;
			Walking::GetInstance()->X_OFFSET = Walking::GetInstance()->X_OFFSET_START - speedAdjSum;
			
			double hip_offset = Walking::GetInstance()->HIP_PITCH_OFFSET;
//			fprintf(stderr, " (hip offset:%.1f)\n", hip_offset);
			Walking::GetInstance()->X_MOVE_AMPLITUDE = FBStep;
			Walking::GetInstance()->Y_MOVE_AMPLITUDE = RLStep;
			Walking::GetInstance()->A_MOVE_AMPLITUDE = RLTurn;	
//			fprintf(stderr, " (FB:%.1f RL:%.1f)\n", FBStep, RLTurn);		
			}
	else //things only done in auto mode
		{
		}
//////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////
// STOP WALKING GAIT ENGINE
//////////////////////////////////////////////////////////////////////////////////////

	if(Walking::GetInstance()->IsRunning() == true && PS3.key.Down != 0)
		{
		fprintf(stderr, "STOPPING WALKING GAIT\n");		
		resetLEDs(cm730);
		Walking::GetInstance()->Stop();
		while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
	
		}
//////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////
// START WALKING GAIT ENGINE
//////////////////////////////////////////////////////////////////////////////////////
if(Walking::GetInstance()->IsRunning() == false && PS3.key.Up != 0)
	{
	if(m_cur_mode == SOCCER)
		{
		fprintf(stderr, "STARTING WALKING GAIT\n");		
		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();			
		}
	}
//////////////////////////////////////////////////////////////////////////////////////



/*
//////////////////////////////////////////////////////////////////////////////////////
	// toggle head from auto to not	

	if(PS3.key.LeftHat != 0)
		{
		if(bLJState == true)
			{
			bLJState = false;
			if(m_cur_mode == SOCCER)
				PS3BallFollower::GetInstance()->bHeadAuto = false;
			else if(m_cur_mode == SITTING)
				LineFollower::GetInstance()->bHeadAuto = false;
			else if(m_cur_mode == LINE_FOLLOWING)
				LineFollower::GetInstance()->bHeadAuto = false;
			else if(m_cur_mode == ROBOT_FOLLOWING)
				RobotFollower::GetInstance()->bHeadAuto = false;

			//double pan,tilt;
			//pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
			//tilt = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);	
			//Head::GetInstance()->MoveByAngle(pan,tilt);
			}
		else
			{
			bLJState = true;
			if(m_cur_mode == SOCCER)
				{
				if(PS3BallFollower::GetInstance()->bFullAuto == true)
					PS3BallFollower::GetInstance()->bHeadAuto = true;
				else
					PS3BallFollower::GetInstance()->bHeadAuto = false;
				}
			else if(m_cur_mode == SITTING)
				{
				LineFollower::GetInstance()->bHeadAuto = true;
				}
			else if(m_cur_mode == LINE_FOLLOWING)
				{
				if(LineFollower::GetInstance()->bFullAuto == true)
					LineFollower::GetInstance()->bHeadAuto = true;
				else
					LineFollower::GetInstance()->bHeadAuto = false;
				}
			else if(m_cur_mode == ROBOT_FOLLOWING)
				{
				if(RobotFollower::GetInstance()->bFullAuto == true)
					RobotFollower::GetInstance()->bHeadAuto = true;
				else
					RobotFollower::GetInstance()->bHeadAuto = false;
				}
			}
		PS3Vibrate();		
		// wait for key release
		while(PS3.key.LeftHat != 0) usleep(8000);		
		}

*/

//////////////////////////////////////////////////////////////////////////////////////
// PS3 Head Control
//////////////////////////////////////////////////////////////////////////////////////	

	if((PS3BallFollower::GetInstance()->bHeadAuto == false && (m_cur_mode == SOCCER || m_cur_mode == SITTING)) )
		{
		int lx,ly,dead_band=6;
		double pan,tilt;
		pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
		tilt = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);	
		Point2D pos = Point2D(pan,tilt);

#ifdef Southpaw
		lx = -(PS3.key.LJoyX-128);
		ly = -(PS3.key.LJoyY-128);
#else		
		lx = -(PS3.key.RJoyX-128);
		ly = -(PS3.key.RJoyY-128);
#endif

		if(abs(lx) > dead_band || abs(ly) > dead_band)
			{
			pos.X = pan + 0.2*Camera::VIEW_V_ANGLE*(lx-dead_band)/256;
			pos.Y = tilt + 0.2*Camera::VIEW_H_ANGLE*(ly-dead_band)/256;		
			}
		Head::GetInstance()->MoveByAngle(pos.X,pos.Y);			
		//Head::GetInstance()->MoveTracking(pos);			
		}



//////////////////////////////////////////////////////////////////////////////////////


}
Exemplo n.º 2
0
// turn all servos off (make sure robot is sitting)
void ServoShutdown() {
    WalkToggle(false);  // turn off walking if is walking
    PlayAction(15);  // sit robot
    // shutdown
    cm730.DXLPowerOn(false);
}
Exemplo n.º 3
0
void ServoStartup() {
    cm730.DXLPowerOn(true);
}
Exemplo n.º 4
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;
}