void chose_command(std::string control, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud){
if (control.compare("collision") == 0)
obstacle_detection(cloud);
else if (control.compare("identification") == 0)
environement_identification(cloud);
else
std::cout << "no treatment for this type of command"<<std::endl;
}
void make_humanoid()
{
glPushMatrix();
glScalef(0.5,0.5,0.5);
prev_block = get_humanoid_block(humanoid,world);
humanoid.loc_x = r_x + jump_translate_x;
humanoid.loc_y = world.height + r_y + jump_translate_y;
if(robot_slide)
humanoid.loc_y -= world.height;
humanoid.loc_z = r_z + jump_translate_z;
//detect_boundary_collision(humanoid,world);
if( !(power_taken == 1 && ( power_choosen == SPEED || power_choosen == INVISIBLE) ) )
detect_missing_blocks_collision(humanoid,world);
//if(detect_is_on_moving_block(humanoid,world))
//{
// vector2d block_num = get_humanoid_block(humanoid,world);
// humanoid.loc_y += world.block_motion_value[block_num.x][block_num.z]*2.0;
//}
//collect_coins(humanoid,world);
/*if(prev_r.x!=humanoid.loc_x || prev_r.y!=humanoid.loc_y || prev_r.z!=humanoid.loc_z)
printf("H == %f %f %f\n",humanoid.loc_x,humanoid.loc_y,humanoid.loc_z);
prev_r.x = humanoid.loc_x;
prev_r.y = humanoid.loc_y;
prev_r.z = humanoid.loc_z;*/
coin_detection();
power_detection();
obstacle_detection();
if(!robot_jump)
px = r_x,pz = r_z,py=r_y;
humanoid.body(humanoid.loc_x,humanoid.loc_y + 22.5 +(robot_slide?world.height:0),humanoid.loc_z,ba,lsa,lea,rsa,rea,lta,lka,rta,rka,lta_x,rta_x);
glPopMatrix();
}
int main(void) //main
{
//motor_test();
return 1;
initialize();
init_LCD();
_delay_ms(1000);
clear_display();
sei(); //enabling global interrupts
while(1)
{
next_node();
clear_display();
print_string("NEXT NODE 1");
get_to_node();
clear_display();
print_string("REACHED NODE 1");
next_node();
clear_display();
print_string("NEXT NODE 2");
get_to_node();
clear_display();
print_string("REACHED NODE 2");
change_dir(1);
if (obstacle_detection()){
}
next_node();
clear_display();
print_string("NEXT NODE 3");
get_to_node();
clear_display();
print_string("REACHED NODE 3");
if (obstacle_detection()){
}
}
return 0;
}
