bool Branch::isVirtualHeadHealthy()
{
if (!isHeadVirtual())
{
return false;
}
//double save_virtual_angle = global_paraMgr.skeleton.getDouble("Save Virtual Angle");
//double save_virtual_dist = global_paraMgr.skeleton.getDouble("Branches Merge Max Dist");
double head_length = GlobalFun::computeEulerDist(curve[0].P(), curve[1].P());
//double bad_virtual_angle = global_paraMgr.skeleton.getDouble("Bad Virtual Angle");
//double follow_dist = global_paraMgr.skeleton.getDouble("Follow Sample Radius");
double angle = getHeadAngle();
//if (angle > bad_virtual_angle || head_length > follow_dist)
//{
// return false;
//}
//if (angle < save_virtual_angle || (head_length < save_virtual_dist && angle < bad_virtual_angle))
//{
// return true;
//}
//else
//{
return false;
//}
}
void Branch::inactiveAndDropVirtualHead()
{
if (!isHeadVirtual())
{
return;
}
CVertex& head = curve[0];
head.is_skel_virtual = false;
if (back_up_head.X() > -5)
{
head.P() = back_up_head;
if (getHeadAngle() > 100)
{
curve.erase(curve.begin());
}
}
else
{
curve.erase(curve.begin());
}
}
task main()
{
// pose ===========================================================
float pose[3];
float* pose_ptr = &pose;
float x=0,y=0,phi=0;
pose[0]=x;pose[1]=y;pose[2]=phi;
// speeds =========================================================
float v=CRUISE, w=0;
float w_look = 0.0;
float w_head = 0.0;
// logObstacle behavior ===========================================
// obstacle log
float obstacles_xy[N_ANGLES][2];
for(int i=0; i<N_ANGLES; i++){
obstacles_xy[i][0]=10.0;//x
obstacles_xy[i][1]=10.0;//y
}
float* obstacle_ptr;
float temp_o_x,temp_o_y;
//ranging
float range;
// head tracking
float halfViewingAngle = headCalibrate();
float headAngle = 0;
float angleDelta = 2.0*halfViewingAngle/(float)N_ANGLES;
float headAngleWorld = 0.0;
float prevHeadAngleWorld = headAngleWorld-2.0*angleDelta;
resetAngleCounter(N_ANGLES-1,-1);
// collision =====================================================
bool collision_flag = false;
//more state variables for pose ===================================
float ticks[2];
float* ticks_ptr;
float prevTicks[2];
prevTicks[0]=0;prevTicks[1]=0;
resetWheelEncoders();
while(1){
// update state ================================================================================================
ticks_ptr = readWheelEncoders();
ticks[0]=ticks_ptr[0];ticks[1]=ticks_ptr[1];
pose_ptr = update_odometry(pose,ticks,prevTicks);
pose[0]=pose_ptr[0];pose[1]=pose_ptr[1];pose[2]=pose_ptr[2];
x=pose_ptr[0];y=pose_ptr[1];phi=pose_ptr[2];
//store for next pass
prevTicks[0] = ticks[0];
prevTicks[1] = ticks[1];
// end update state ============================================================================================
// logObstacle behavior ========================================================================================
headAngle = getHeadAngle();
range = getRange();
headAngleWorld = headAngle-halfViewingAngle+phi;
if(abs(headAngleWorld - prevHeadAngleWorld) >= angleDelta){
prevHeadAngleWorld = headAngleWorld;
obstacle_ptr = getObstaclePosition(headAngleWorld,range,pose);
temp_o_x = obstacle_ptr[0];temp_o_y = obstacle_ptr[1];
obstacles_xy[AngleCounter][0] = temp_o_x;
obstacles_xy[AngleCounter][1] = temp_o_y;
incrementAngleCounter();
}
// end logObstacle behavior ====================================================================================
// collide behavior ============================================================================================
collision_flag = checkForCollisions(obstacles_xy,pose);
// end collide behavior ========================================================================================
// send actuator commands ======================================================================================
v = CRUISE;
if(collision_flag && v>0.0){
v = 0.0;
}
set_motor_speeds(v,0.0);
//worked before the angle was limited
//if(collision_flag)v
// //halt();
// //set_motor_speeds(0.0,0.0);
// set_motor_speeds(0.0,5.0);
//}else{
// set_motor_speeds(v,0.0);
//}
w_look = lookBackAndForth(w_look);
w_head = w_look;
headRotate(w_head);
// end send actuator commands ==================================================================================
}
}
