main.p

机器人运动控制

//
// Simple script that will loop through an array of motions, play each one, and return
//

// let's set up some parameters to the build system

#include "pleo/limits.inc"
#pragma semicolon 1
#pragma tabsize 4
#pragma amxram MAIN_RAM_MAX

// LifeOS includes
#include <Property.inc>
#include <Motion.inc>
#include <Log.inc>
#include <Script.inc>
#include <Sound.inc>
#include <Util.inc>
#include <Sensor.inc>
#include <Joint.inc>

// local includes
#include "motions.inc"
#include "sounds.inc"
#include "user_properties.inc"

new newJointID[] = {
    JOINT_RIGHT_SHOULDER,
    JOINT_LEFT_SHOULDER,
    JOINT_RIGHT_ELBOW,  
    JOINT_LEFT_ELBOW,
	JOINT_HEAD,	
	
	JOINT_TAIL_VERTICAL,

	JOINT_NECK_VERTICAL,
	JOINT_NECK_HORIZONTAL,
    JOINT_LEFT_KNEE,
    JOINT_RIGHT_KNEE,
    JOINT_LEFT_HIP,
    JOINT_RIGHT_HIP,
    JOINT_TORSO,
	JOINT_TAIL_HORIZONTAL,  
    
	  

};


new jointName[50];
new motions[] = 
{
    #include "motions.p"
};


public main()
{
    

	//-----------------------------------------
	//pleo will runing the neutral positon positon 
	//------------------------------------------
	moveJointListNeutral(0,13,4000,128);
	 sound_set_volume(150);
//	   GoStraight();
//	   balanceLeft();
//	   balanceRight();
//	  LeftFront();

//	  RightFront();
  //  SitUp();
     moveJointListNeutral(0,13,4000,128);
 //   while(!sensor_read(SENSOR_ARSE))
//	    {}
 	SitDown2();
	 
	  OpenTar();
//	   while(!sensor_read(SENSOR_ARSE))
//	   {}
	
	 while(1)
	 {
 	 	PickLeg();
	 }
      

	
	
	while(!sensor_read(SENSOR_WAKEUP))
	{}
    // we let it return here. the main routine should be called again
}


OpenTar()
{
	moveJointListNeutral(4,6,2000,128);
	moveJointListMin(4,6,2000,128);   
}
StepByStep()
{

    new index = 0;
    new Motion: handle = 0;
    
    // play all the motions in our array
	
	
    while (index < sizeof motions)
    {
        new motion = motions[index];
        
        while (property_get(property_animation_paused) == true)
        {
        }
        
        // .. and (try to) play it
        handle = motion_play(motion);

        printf("motion -> %d,index= %d\n", motion,index);

        // wait for it.
        while (motion_is_playing(handle))
        {
        }
       sound_play(snd_beep_short);
		while(!sensor_read(SENSOR_WAKEUP))
		{
			if(sensor_read(SENSOR_BACK))
			{
			  index--;
			  break;
			}
		}
		
        index++;
    }
}
wait_for_time(value)
{
	new curTime=time();
	while(time()-curTime<value)
	{
		
	}
}

wait_for_wakeup()
{
	while(!sensor_read(SENSOR_WAKEUP))
	{}
}
LeftFront()
{
	MoveMotion(12,14);
}

RightFront()
{
	MoveMotion(15,17);
}
balanceRight()
{
	MoveMotion(10,11);
}

balanceLeft()
{
	MoveMotion(8,9);
}
SitUp()
{
	monitor_exec("joi neut");
	monitor_exec("joi pwm 12 30");
	monitor_exec("joi pwm 10 30");
	monitor_exec("joi pwm 4 30");
	monitor_exec("joi pwm 6 30");
	monitor_exec("joi pwm 5 30");
	monitor_exec("joi pwm 7 30");
	monitor_exec("joi pwm 0 -30");
	monitor_exec("joi pwm 2 -30");
	wait_for_time(5000);
	
	new i=0;
	while(i<4)
	{
	moveJointListNeutral(4,5,1000,128);
	moveJointListMax(4,5,1000,128);
	i++;
	}
}
GetUp()
{
	printf("Pleo should be get up from lay down position\n");
	MoveMotion(0,0);
}
GoStraight()
{
	printf("Pleo go straight forward.\n");
	//MoveMotion(1,3);
	MoveMotion(2,3);
	moveJointListNeutral(4,6,3000,128);
	new i=0;
	for(i=0;i<4;i++)
	{
		moveJointListNeutral(4,6,1000,128);
		moveJointListMax(4,6,1000,128);
		
	}
}
OpenEye()
{
	printf("Pleo close and open eye\n");
	MoveMotion(4,6);
}

PickLeg()
{
	
	//moveJointListMin(5,6,2000,128);
//	  monitor_exec("joi pwm 12 -30");
//	  moveJointListMin(5,6,2000,128);
//	  monitor_exec("joi pwm 13 -30");
//	  wait_for_time(4000);
	

	monitor_exec("joi angle 12 40");
	sound_stop();
	sound_play(snd_happy_new_year);
	
	new i=0;
	for(i=0;i<10;i++)
	{
		//MoveMotion(7,7);
	//	  wait_for_time(2000);
	moveJointListMax(2,5,1000,128);
	moveJointListMin(2,5,1000,128);
	
	
	}
	
}
SitDown2()
{
//	  sound_play(snd_growlx);
//	  wait_for_sound(snd_growlx);
//	  monitor_exec("joi neut");
	sound_play(snd_howl);
//	  wait_for_sound(snd_howl);
	monitor_exec("joi pwm 12 30");
	monitor_exec("joi pwm 10 30");
	monitor_exec("joi pwm 4 30");
	monitor_exec("joi pwm 6 30");
	monitor_exec("joi pwm 5 30");
	monitor_exec("joi pwm 7 30");
	monitor_exec("joi pwm 0 30");
	monitor_exec("joi pwm 2 30");
   //--
   	monitor_exec("joi pwm 1 30");
	monitor_exec("joi pwm 3 30");
	//--
	
	wait_for_time(5000);
	//

	//--
	monitor_exec("joi pwm 4 0");
	monitor_exec("joi pwm 6 0");
	monitor_exec("joi pwm 5 0");
	monitor_exec("joi pwm 7 0");
	//--
	monitor_exec("joi pwm 12 0");
	monitor_exec("joi pwm 10 0");
//	  monitor_exec("joi pwm 4 0");
//	  monitor_exec("joi pwm 6 0");
//	  monitor_exec("joi pwm 5 0");
//	  monitor_exec("joi pwm 7 0");
	monitor_exec("joi pwm 1 0");
	monitor_exec("joi pwm 3 0");
	
	monitor_exec("joi pwm 0 -30");
	monitor_exec("joi pwm 2 -30");
	monitor_exec("joi pwm 10 30");
	wait_for_time(4000);
	monitor_exec("joi pwm 0 0");
	monitor_exec("joi pwm 2 0");
	monitor_exec("joi pwm 10 0");
	
	//
	monitor_exec("joi neut");
	// modify the sit down position. modify at 2008.01.29 6:00pm
	monitor_exec("joi pwm 4 20");
	monitor_exec("joi pwm 6 20");
	wait_for_time(2000);
	monitor_exec("joi pwm 4 0");
	monitor_exec("joi pwm 6 0");
}
SitDown1()
{

	//---------pleo sit down ------------
	printf("Pleo will sit down.\n");
	
	monitor_exec("joi neut");
	monitor_exec("joi pwm 12 30");
	monitor_exec("joi pwm 10 30");
	monitor_exec("joi pwm 4 30");
	monitor_exec("joi pwm 6 30");
	monitor_exec("joi pwm 5 30");
	monitor_exec("joi pwm 7 30");
	monitor_exec("joi pwm 0 -30");
	monitor_exec("joi pwm 2 -30");
//	  wait_for_time(5000);
	

//	  monitor_exec("joi pwm 1 30");
//	  monitor_exec("joi pwm 3 30");

	wait_for_time(3000);

   monitor_exec("joi neut 0");

	 

	monitor_exec("joi pwm 4 30");
	monitor_exec("joi pwm 6 30");	
	monitor_exec("joi angle 0 90");
	monitor_exec("joi angle 2 90");
	monitor_exec("joi angle 10 60");
	monitor_exec("joi angle 12 30");
	wait_for_time(2000);
	monitor_exec("joi pwm 4 0");
	monitor_exec("joi pwm 6 0");
	monitor_exec("joi pwm 5 0");
	monitor_exec("joi pwm 7 0");		  
	monitor_exec("joi pwm 0 0");
	monitor_exec("joi pwm 2 0");
//	  monitor_exec("joi pwm 4 -30");
//	  monitor_exec("joi pwm 6 -30");

	
//	  monitor_exec("joi angle 10 50");
	// ----pleo end sit down---------	   	   	   
}
SitDown()
{
	//---------pleo sit down ------------
	printf("Pleo will sit down.\n");
	
	monitor_exec("joi neut");
	monitor_exec("joi pwm 12 30");
	monitor_exec("joi pwm 10 30");
	monitor_exec("joi pwm 4 30");
	monitor_exec("joi pwm 6 30");
	monitor_exec("joi pwm 5 30");
	monitor_exec("joi pwm 7 30");
	monitor_exec("joi pwm 0 -30");
	monitor_exec("joi pwm 2 -30");
	wait_for_time(5000);
//	  monitor_exec("joi pwm 1 30");
//	  monitor_exec("joi pwm 3 30");

//	  wait_for_time(3000);
	//wait_for_wakeup();
	monitor_exec("joi neut");
//    monitor_exec("joi neut 0");
//	  monitor_exec("joi neut 1");
//	  monitor_exec("joi neut 2");
//	  monitor_exec("joi neut 3");
	
	wait_for_time(4000);
//	  monitor_exec("joi angle 10 50");
	// ----pleo end sit down---------	   
}


MoveMotion(sIndex,eIndex)
{
new index=sIndex;
 while (index < eIndex)
    {
        new motion = motions[index];
        
        // .. and (try to) play it
        new handle = motion_play(motion);

        printf("motion -> %d,index= %d\n", motion,index);

        // wait for it.
        while (motion_is_playing(handle))
        {
        }	 	 	 
        index++;
    }	 
	yield();  
}
// wait for the joint to finish its movement
waitForJoint(jointid, timeout = 3000)
{
	new start_time = time();
	new joint_stat = joint_get_status(jointid);
    while ( joint_stat != JOINT_STATUS_STOPPED && joint_stat != JOINT_STATUS_STALLED && (time() - start_time < timeout))
	{	
		yield();
		joint_stat = joint_get_status(jointid);
	}	
	printf("\ntime : %d \n",time() - start_time);
	printf("\njoint status : %d \n",joint_stat);
	return joint_get_status(jointid);


}


waitForJoints(sIndex, eIndex, timeout)
{

	new start_time = time();
	new moving = 1;
	
	while (moving) {

		moving = 0;
		for (new mID = sIndex; mID <= eIndex; mID++) {
			new jointid = newJointID[mID];
			if (  joint_get_status(jointid) != JOINT_STATUS_STOPPED && joint_get_status(jointid) != JOINT_STATUS_STALLED && (time() - start_time < timeout) ) {
				moving = 1;
				break;
			}
		}	
		yield();
	}	
	
}


stopJointMovement(jointid, timeout = 1000)
{

	new speed = 128;
	new vrValue = joint_get_position(jointid, angle_vr);
	joint_move_to( jointid, vrValue, speed, angle_vr );
}

// move a list of joints to neutral position 
public moveJointListNeutral(sIndex, eIndex,timeout, speed) {

	new mID;
		
	// move back to neutral point
	 
	for (mID = sIndex; mID <= eIndex; mID++) {
		new neu_degree = joint_get_neutral( newJointID[mID], angle_degrees);
		joint_move_to( newJointID[mID], neu_degree, speed, angle_degrees );
		joint_get_name(newJointID[mID], jointName );
		printf("%s \t - neutral degree:%d \n", jointName, neu_degree);
		//	  waitWakeupButton();
	}
	
	// Edward Chan:  This is a workaround solution as joint status is still not reliable
	//waitForJoints(sIndex, eIndex, target_vr, timeout/2);
	
	 waitForJoints(sIndex, eIndex, timeout);
}

// move a list of joints to maximium position 
public moveJointListMax(sIndex, eIndex,timeout ,speed) {

	new mID;
	
	 	   
	// move back to neutral point
	for (mID = sIndex; mID <= eIndex; mID++) {
		new max_degree = joint_get_max( newJointID[mID], angle_degrees);
		joint_move_to( newJointID[mID], max_degree, speed, angle_degrees );
		joint_get_name(newJointID[mID], jointName );
		printf("%s \t - max degree:%d \n", jointName, max_degree);
	}
	
	// Edward Chan:  This is a workaround solution as joint status is still not reliable
	// waitForJoints(sIndex, eIndex, target_vr, timeout/2);
	
	waitForJoints(sIndex, eIndex, timeout);
}


// move a list of joints to minimium position 
public moveJointListMin(sIndex, eIndex, timeout,speed) {

	new mID;

		
	// move back to neutral point
	for (mID = sIndex; mID <= eIndex; mID++) {
		new min_degree = joint_get_min( newJointID[mID], angle_degrees);
		joint_move_to( newJointID[mID], min_degree, speed, angle_degrees );
		joint_get_name(newJointID[mID], jointName );
		printf("%s \t - min degree:%d \n", jointName, min_degree);
	}
	
	// Edward Chan:  This is a workaround solution as joint status is still not reliable
	// waitForJoints(sIndex, eIndex, target_vr, timeout/2);
	
	waitForJoints(sIndex, eIndex, timeout);
}


// move a list of joints to minimium position 
public printJointListStatus(sIndex, eIndex) {

	new mID;
		
	// move back to neutral point
	for (mID = sIndex; mID <= eIndex; mID++) {
		new status = joint_get_status(newJointID[mID]);
		new vr = joint_get_position(newJointID[mID], angle_vr);
		new degree = joint_get_position(newJointID[mID], angle_degrees);
		joint_get_name(newJointID[mID], jointName );
		printf("%s status is  %d - VR:%d - degree:%d \n", jointName, status, vr, degree);

	}

}
bool: wait_for_sound(sound_name: sound = 0)
//bool: wait_for_sound(Sound: sound = 0)
{
    while (sound_is_playing(sound) == true)
    {
    //	  yield();
    }
}