/* ARGSUSED1 */
void CurveHandleKeys(unsigned char key, int x, int y)
{
if (key > '0' && key < '9')
CurveHandleEditMenu((key-'0')-1);
else if (key == 'd')
CurveHandleMenu(CMENU_DONEEDIT);
else {
switch(key) {
case 'f':
case ' ': Step++;
StopWalking(); break;
case 'F': Step += 5;
StopWalking(); break;
case 'b': Step--;
StopWalking(); break;
case 'B': Step -= 5;
StopWalking(); break;
}
Step %= CYCLE_SIZE;
if (Step < 0)
Step = CYCLE_SIZE + Step;
fStep = Step;
RedisplayBoth();
}
}
void Motion::StartWalking(double distance)
{
double timeToWalk = distance / BrainData::GetInstance()->WALK_SPEED;
StartWalking();
usleep(timeToWalk*1000000); //convert sec to microsec
StopWalking();
}
/* ARGSUSED2 */
void CurveHandleButton(int button, int state, int x, int y)
{
if (button == GLUT_RIGHT_BUTTON )
return;
if (state == GLUT_DOWN && CurveDownBtn == -1) {
if (button == GLUT_MIDDLE_BUTTON)
CurveHandleMenu(CMENU_WALK);
else
fStep = Step = (int)((float)x/glutGet(GLUT_WINDOW_WIDTH) * CYCLE_SIZE);
CurveDownBtn = button;
WasWalking = Walking;
StopWalking();
RedisplayBoth();
} else if (button == CurveDownBtn) {
CurveDownBtn = -1;
if (WasWalking) {
Walking = 1;
agvSetAllowIdle(0);
glutIdleFunc(IncrementStep);
}
}
}
void C_HL2MP_Player::HandleSpeedChanges( void )
{
int buttonsChanged = m_afButtonPressed | m_afButtonReleased;
if( buttonsChanged & IN_SPEED )
{
// The state of the sprint/run button has changed.
if ( IsSuitEquipped() )
{
if ( !(m_afButtonPressed & IN_SPEED) && IsSprinting() )
{
StopSprinting();
}
else if ( (m_afButtonPressed & IN_SPEED) && !IsSprinting() )
{
if ( CanSprint() )
{
StartSprinting();
}
else
{
// Reset key, so it will be activated post whatever is suppressing it.
m_nButtons &= ~IN_SPEED;
}
}
}
}
else if( buttonsChanged & IN_WALK )
{
if ( IsSuitEquipped() )
{
// The state of the WALK button has changed.
if( IsWalking() && !(m_afButtonPressed & IN_WALK) )
{
StopWalking();
}
else if( !IsWalking() && !IsSprinting() && (m_afButtonPressed & IN_WALK) && !(m_nButtons & IN_DUCK) )
{
StartWalking();
}
}
}
if ( IsSuitEquipped() && m_fIsWalking && !(m_nButtons & IN_WALK) )
StopWalking();
}
void GuyHandleKeys(unsigned char key, int x, int y)
{
switch(key) {
case 'f':
case ' ': Step++;
StopWalking(); break;
case 'F': Step += 5;
StopWalking(); break;
case 'b': Step--;
StopWalking(); break;
case 'B': Step -= 5;
StopWalking(); break;
}
Step %= CYCLE_SIZE;
if (Step < 0)
Step = CYCLE_SIZE + Step;
agvHandleKeys(key, x, y);
RedisplayBoth();
}
/// This should be subclassed to actually add some physics, based on your given game. By default it will send a PT_SET_ACCELERATION message to the entity using walkingAcceleration.
void PathableProperty::GoToWaypoint(Waypoint * wp)
{
if (wp == 0)
{
StopWalking();
return;
}
MoveToMessage mtm(wp->position);
owner->ProcessMessage(&mtm);
}
/// Sets new path to walk, first going to the first waypoint in the path.
void PathableProperty::StartWalking(Path alongPath)
{
StopWalking();
isWalking = false;
nextWaypoint = 0;
path = alongPath;
if (!path.Size())
return;
isWalking = true;
nextWaypoint = path[0];
GoToWaypoint(nextWaypoint);
}
void C_HL2MP_Player::HandleSpeedChanges( void )
{
int buttonsChanged = m_afButtonPressed | m_afButtonReleased;
if( buttonsChanged & IN_WALK )
{
if ( IsSuitEquipped() )
{
// The state of the WALK button has changed.
if( IsWalking() && !(m_afButtonPressed & IN_WALK) )
{
StopWalking();
}
else if( !IsWalking() && (m_afButtonPressed & IN_WALK) && !(m_nButtons & IN_DUCK) )
{
StartWalking();
}
}
}
if ( IsSuitEquipped() && m_fIsWalking && !(m_nButtons & IN_WALK) )
StopWalking();
}
void Motion::TurnByAngle(double angle)
{
if(angle == 0)
return;
else if(angle > 0)
StartWalking(0,0,30);
else if(angle < 0)
{
angle = -angle;
StartWalking(0,0,-30);
}
usleep(2000*1000);
//usleep(factor*angle*27);
StopWalking();
SetHeadTilt(HeadTilt(GetHeadTilt().Tilt,0));
}
/// Sets next waypoint as target. If next waypoint becomes 0, it will stop walking. If path is cyclic, will never stop walking until StopWalking is called.
bool PathableProperty::SetNextWaypointAsTarget()
{
int index = path.GetIndexOf(nextWaypoint);
++index;
if (index >= path.Size() - 1) // If final point, just go to it and stop.
{
// std::cout<<"\nIs final waypoint.";
GoToWaypoint(nextWaypoint);
Message msg("OnFinalDestinationSet");
owner->ProcessMessage(&msg);
StopWalking();
return false;
}
nextWaypoint = path[index];
// TODO check circularity and max size of path?
return true;
}
void C_HL2MP_Player::HandleSpeedChanges( void )
{
bool bCanSprint = CanSprint();
bool bIsSprinting = IsSprinting();
bool bWantSprint = (bCanSprint && IsSuitEquipped() && (m_nButtons & IN_SPEED));
if (bIsSprinting != bWantSprint)
{
// If someone wants to sprint, make sure they've pressed the button to do so. We want to prevent the
// case where a player can hold down the sprint key and burn tiny bursts of sprint as the suit recharges
// We want a full debounce of the key to resume sprinting after the suit is completely drained
if (bWantSprint)
{
StartSprinting();
}
else
{
StopSprinting();
}
}
bool bIsWalking = IsWalking();
// have suit, pressing button, not sprinting or ducking
bool bWantWalking;
if (IsSuitEquipped())
{
bWantWalking = (m_nButtons & IN_WALK) && !IsSprinting() && !(m_nButtons & IN_DUCK);
}
else
{
bWantWalking = true;
}
if (bIsWalking != bWantWalking)
{
if (bWantWalking)
{
StartWalking();
}
else
{
StopWalking();
}
}
}
/*****************************************************************************************
* Function Name: SetWalkingMode
* Description: This function Loads the settings for the walking module to a walking mode
* the function sets the max walking speed to 13 and -13.
* the function stops the motion, and needs to be enabled.
*
*****************************************************************************************/
void WalkingAndActions::SetWalkingMode(WalkingModes_t walkingMode)
{
if(m_walkingMode == walkingMode)
{
return;
}
StopWalking();
#ifdef DEBUG_SET_WALKING_MODE
cout << "[WalkingAndActions::SetWalkingMode]: Toggling walking mode to [" << ConvertWalkingModeToString(walkingMode) << "]\n";
#endif
usleep(500 * 1000);
switch(walkingMode)
{
//if changed to walking mode, change the max speed for the axis
//and load ini file, and change the walking mode variable
case RUNNING:
m_walking->LoadINISettings(m_running_ini);
m_maxXSpeed = RUNNING_MAX_FORWARD_SPEED;
m_minXSpeed = RUNNING_MAX_BACKWARD_SPEED;
m_maxYSpeed = MAX_LEFT_SPEED;
m_minYSpeed = MAX_RIGHT_SPEED;
m_maxLeftTurningAngle = MAX_TURN_LEFT_SPEED;
m_maxRightTurningAngle = MAX_TURN_RIGHT_SPEED;
m_walkingMode = RUNNING;
break;
case WALKING:
StartAction(STAND_UP_WALKING);
m_walking->LoadINISettings(m_walking_ini);
m_maxXSpeed = WALKING_MAX_FORWARD_SPEED;
m_minXSpeed = WALKING_MAX_BACKWARD_SPEED;
m_maxYSpeed = MAX_LEFT_SPEED;
m_minYSpeed = MAX_RIGHT_SPEED;
m_maxLeftTurningAngle = MAX_TURN_LEFT_SPEED;
m_maxRightTurningAngle = MAX_TURN_RIGHT_SPEED;
m_walkingMode = WALKING;
break;
default:
printf("No such walkingMode type as [%d]\n",walkingMode);
break;
}
}
void CSDKPlayer::HandleSpeedChanges( void )
{
bool bIsWalking = IsWalking();
bool bWantWalking;
bWantWalking = true;
if( bIsWalking != bWantWalking )
{
if ( bWantWalking )
{
StartWalking();
}
else
{
StopWalking();
}
}
}
/*****************************************************************************************
* Method Name: StartAction
* Description: This method gets an action and execute it
* Arguments: action - action to perform
*****************************************************************************************/
void WalkingAndActions::StartAction(Actions_t action)
{
#ifdef DEBUG_START_ACTION
cout << "[StartAction]: Action type: " << action << "\n";
#endif
#ifdef DEBUG_START_ACTION
printf("[StartAction]: Stopping StartWalking thread...\n");
#endif
StopThreadRunning();
#ifdef DEBUG_START_ACTION
printf("[StartAction]: StartWalking thread stopped.\n");
#endif
sigset_t mask;
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGUSR2);
bool isSigmaskSuccess = pthread_sigmask(SIG_BLOCK, &mask, NULL);
#ifdef DEBUG_START_ACTION
if(isSigmaskSuccess)
{
printf("[StartAction]: Sigmask failed\n");
}
else
{
printf("[StartAction]: Sigmask success\n");
}
#endif
pthread_mutex_lock(& m_walkingAndActionsMutex);
#ifdef DEBUG_START_ACTION
printf("[StartAction]: Mutex has been locked. \n");
#endif
//check if in walking mode
if( IsWalking() == true )
{
#ifdef DEBUG_START_ACTION
printf("[StartAction]: The robot is walking - StopWalking...\n");
#endif
StopWalking();
#ifdef DEBUG_START_ACTION
printf("[StartAction]: The robot has stopped walking.\n");
#endif
}
while( IsActionOnGoing() == true )
{
#ifdef DEBUG_START_ACTION
printf("[StartAction]: An action is ongoing...\n");
#endif
usleep(8000);
}
#ifdef DEBUG_START_ACTION
printf("[StartAction]: Walking and actions has been stopped. \n");
#endif
//action module take control of the body without head
m_action->m_Joint.SetEnableBodyWithoutHead(true,true);
//action module start the action
#ifdef DEBUG_START_ACTION
printf("[StartAction]: Starting the action...\n");
#endif
m_action->Start( (int)action );
#ifdef DEBUG_START_ACTION
cout << "[StartAction]: Check if action is still running...\n";
#endif
while(m_action->IsRunning())
{
usleep(8000);
}
#ifdef DEBUG_START_ACTION
cout << "[StartAction]: Action: " << (int)action << " has been done\n";
#endif
ResetWalkingParams();
#ifdef DEBUG_START_ACTION
cout << "[StartAction]: Walking Params has been reseted\n";
#endif
pthread_mutex_unlock(& m_walkingAndActionsMutex);
#ifdef DEBUG_START_ACTION
printf("[StartAction]: Mutex has been unlocked. \n");
#endif
pthread_sigmask(SIG_UNBLOCK, &mask, NULL);
}
void Motion::TurnToGoal(double angle)
{
StartWalking(0,40,5);
usleep(88888.8888*angle);
StopWalking();
}
