int main()
{
Init();
Config();
Start();
// Run this loop waiting for either motion detected, e.g., something triggering the robot to wake up,
// or the PC has sent operational state configuration information, i.e., the robot is ready to do something
// Note: Run this loop every 1 second so as to not overwhelm the serial port with junk.
while (!MotionDetected() && !OpStateReceived())
{
ProcessIncomingMessages();
CheckEnvironment();
SendStatusMessages();
pause(INIT_LOOP_WAIT_TIME);
};
// We're ready to do something, enable odometry and sensors
ENABLE_ODOM();
ENABLE_SENSORS();
// Run the main loop
while (1)
{
ProcessIncomingMessages();
CheckEnvironment();
UpdateMotorSpeed();
SendStatusMessages();
}
}
void DnsHealthMonitor::OnTimer(
__in_opt KAsyncContextBase* const,
__in KAsyncContextBase& context
)
{
if (context.Status() == STATUS_SUCCESS)
{
CheckEnvironment();
StartTimer();
}
else
{
// Timer was cancelled
KInvariant(context.Status() == STATUS_CANCELLED);
}
}
void main(int argc, char *argv[])
{
int n;
// Debug_Init();
setbuf(stdout,NULL);
CheckEnvironment();
ArgInit(argc, argv);
DeleteTempFiles();
CreateD32AEnvVar();
CheckExec(argv);
oldfilesize=GetFileSize(oldfilename);
UnbindExec();
OpenExec();
Exec_Type=FindExecType();
Print("\n");
if(!verbose) Print("+ Unbinding Exec : Ok.\n");
else Print("+ Unbinding Exec : Ok. (%s-style)\n",execstyle[Exec_Type-1]);
CloseExec();
if(advanced)
{
if(!quiet) printf("+ Preprocessing Exec : ");
n=relocate_exec(tempname1);
if(n!=0)
{
Print("Error! \n");
err_relocator(n);
}
if(!verbose) Print("Ok.\n");
else Print("Ok. (%d fixups)\n",relocated_fixups);
}
OpenExec();
CompressExec();
CloseExec();
if(bind)
{
BindExec();
if(!verbose) Print("+ Binding Exec : Ok.\n");
else Print("+ Binding Exec : Ok. (Stub file used: \"%s\")\n",stubnames[bindtype]);
}
else
{
unlink(newfilename);
copy_file(tempname2,newfilename);
unlink(tempname2);
}
newfilesize=GetFileSize(newfilename);
DeleteTempFiles();
if(!quiet)
{
printf("\n Original Exec Stats: \"%s\", %s-style, %d bytes\n", oldfilename, execstyle[Exec_Type-1], oldfilesize);
printf("Compressed Exec Stats: \"%s\", %s-style, %d bytes (%1.1f%%)\n", newfilename, execstyle[2], newfilesize, (float)(newfilesize+0.01)/(float)(oldfilesize+0.01) *100);
}
else
{
if(!silent) printf("SC/32A: File \"%s\" has been successfully compressed\n", oldfilename);
}
exit(0);
}
void CPHMovementControl::Calculate(const xr_vector<DetailPathManager::STravelPathPoint>& path,float speed, u32& travel_point, float& precision )
{
#ifdef DEBUG
if(ph_dbg_draw_mask1.test(ph_m1_DbgTrackObject)&&(!!pObject->cName())&&stricmp(PH_DBG_ObjectTrack(),*pObject->cName())==0)
{
Msg("CPHMovementControl::Calculate in %s (Object Position) %f,%f,%f",PH_DBG_ObjectTrack(),pObject->Position().x,pObject->Position().y,pObject->Position().z);
Msg("CPHMovementControl::Calculate in %s (CPHMovementControl::vPosition) %f,%f,%f",PH_DBG_ObjectTrack(),vPosition.x,vPosition.y,vPosition.z);
}
#endif
if(!m_character->b_exist) return;
if(m_capture)
{
if(m_capture->Failed()) xr_delete(m_capture);
}
Fvector new_position;
m_character->IPosition(new_position);
int index=0;//nearest point
//float distance;//distance
bool near_line;
m_path_size=path.size();
Fvector dir;
dir.set(0,0,0);
if(m_path_size==0)
{
speed=0;
vPosition.set(new_position);
}
else if(b_exect_position)
{
m_start_index=travel_point;
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
if((m_path_size-1)>(int)travel_point)
dir.sub(path[travel_point+1].position,path[travel_point].position);
else
dir.sub(path[travel_point].position,new_position);
m_start_index=travel_point;
dir.y=0.f;
dir.normalize_safe();
vPosition.set(new_position);
m_path_distance=0;
SetPathDir (dir);
vPathPoint.set(vPosition);
}
else {
Fvector dif;
dif.sub(new_position,vPathPoint);
float radius = dif.magnitude()*2.f;
if(m_path_size==1)
{
speed=0.f;
vPosition.set(new_position); //todo - insert it in PathNearestPoint
index=0;
vPathPoint.set(path[0].position);
Fvector _d;
_d.sub(path[0].position,new_position);
SetPathDir (_d);
m_path_distance=GetPathDir().magnitude();
if(m_path_distance>EPS)
{
Fvector _d = GetPathDir();
_d.mul(1.f/m_path_distance);
SetPathDir(_d);
}
near_line=false;
}
else
{
m_path_distance=dInfinity;
near_line=true;
if(m_start_index<m_path_size)
{
PathNearestPointFindUp(path,new_position,index,radius,near_line);
PathNearestPointFindDown(path,new_position,index,radius,near_line);
}
if(m_path_distance>radius)
{
m_start_index=0;
PathNearestPoint(path,new_position,index,near_line);
}
vPosition.set(new_position);//for PathDirLine && PathDirPoint
if(near_line) PathDIrLine(path,index,m_path_distance,precision,dir);
else PathDIrPoint(path,index,m_path_distance,precision,dir);
travel_point=(u32)index;
m_start_index=index;
if(fis_zero(speed)) dir.set(0,0,0);
}
}
dir.y=0.f;
//VERIFY(!(fis_zero(dir.magnitude())&&!fis_zero(speed)));
dir.normalize_safe();
/////////////////////////////////////////////////////////////////
if(bExernalImpulse)
{
//vAccel.add(vExternalImpulse);
Fvector V;
V.set(dir);
//V.mul(speed*fMass/fixed_step);
V.mul(speed*10.f);
V.add(vExternalImpulse);
m_character->ApplyForce(vExternalImpulse);
speed=V.magnitude();
if(!fis_zero(speed))
{
dir.set(V);
dir.mul(1.f/speed);
}
speed/=10.f;
vExternalImpulse.set(0.f,0.f,0.f);
bExernalImpulse=false;
}
/////////////////////////
//if(!PhyssicsOnlyMode()){
// Fvector v;//m_character->GetVelocity(v);
// v.mul(dir,speed);
// SetVelocity(v);//hk
//
//}
/////////////////////////
m_character->SetMaximumVelocity(speed);
m_character->SetAcceleration(dir);
//////////////////////////////////////////////////////
m_character->GetSmothedVelocity(vVelocity);
fActualVelocity=vVelocity.magnitude();
gcontact_Was=m_character->ContactWas();
const ICollisionDamageInfo* di=m_character->CollisionDamageInfo();
fContactSpeed=0.f;
{
fContactSpeed=di->ContactVelocity();
gcontact_Power = fContactSpeed/fMaxCrashSpeed;
gcontact_HealthLost = 0;
if (fContactSpeed>fMinCrashSpeed)
{
gcontact_HealthLost =
((fContactSpeed-fMinCrashSpeed))/(fMaxCrashSpeed-fMinCrashSpeed);
}
}
CheckEnvironment(vPosition);
bSleep=false;
b_exect_position=false;
//m_character->Reinit();
}
void CPHMovementControl::Calculate(Fvector& vAccel,const Fvector& camDir,float /**ang_speed/**/,float jump,float /**dt/**/,bool /**bLight/**/){
if(m_capture)
{
if(m_capture->Failed()) xr_delete(m_capture);
}
Fvector previous_position;previous_position.set(vPosition);
m_character->IPosition(vPosition);
if(bExernalImpulse)
{
vAccel.add(vExternalImpulse);
m_character->ApplyForce(vExternalImpulse);
vExternalImpulse.set(0.f,0.f,0.f);
bExernalImpulse=false;
}
//vAccel.y=jump;
float mAccel=vAccel.magnitude();
m_character->SetCamDir(camDir);
m_character->SetMaximumVelocity(mAccel/10.f);
//if(!fis_zero(mAccel))vAccel.mul(1.f/mAccel);
m_character->SetAcceleration(vAccel);
if(!fis_zero(jump)) m_character->Jump(vAccel);
m_character->GetSavedVelocity(vVelocity);
fActualVelocity=vVelocity.magnitude();
//Msg("saved avel %f", fActualVelocity);
gcontact_Was=m_character->ContactWas();
fContactSpeed=0.f;
const ICollisionDamageInfo* di=m_character->CollisionDamageInfo();
{
fContactSpeed=di->ContactVelocity();
gcontact_Power = fContactSpeed/fMaxCrashSpeed;
gcontact_HealthLost = 0;
if (fContactSpeed>fMinCrashSpeed)
{
gcontact_HealthLost =
((fContactSpeed-fMinCrashSpeed))/(fMaxCrashSpeed-fMinCrashSpeed);
}
}
if(m_character->LastMaterialIDX()!=u16(-1))
{
const SGameMtl *last_material=GMLib.GetMaterialByIdx(m_character->LastMaterialIDX());
if(last_material->Flags.test(SGameMtl::flInjurious))
{
gcontact_HealthLost+=Device.fTimeDelta*last_material->fInjuriousSpeed;
}
}
//CPhysicsShellHolder * O=di->DamageObject();
//SCollisionHitCallback* cc= O ? O->get_collision_hit_callback() : NULL;
const ICollisionDamageInfo *cdi=CollisionDamageInfo();
if(cdi->HitCallback())cdi->HitCallback()->call(static_cast<CGameObject*>(m_character->PhysicsRefObject()),fMinCrashSpeed,fMaxCrashSpeed,fContactSpeed,gcontact_HealthLost,CollisionDamageInfo());
TraceBorder(previous_position);
CheckEnvironment(vPosition);
bSleep=false;
m_character->Reinit();
}
