?? 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();
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -