robot.cpp

机器人的行为控制模拟程序。用于机器人的环境识别。A robot action decision simulation used for robot enviroment recognition.

    
    unsigned int i = 0;
    
    // Check against all walls
    while (w != NULL)
    {
        // The wall is checked using a line point to opposing corners
        box.x1 = w->x;
        box.y1 = w->y;
        
        box.x2 = w->x + w->x_size;
        box.y2 = w->y + w->y_size;
            
            
        dist[i] = check_box_intersect(main_line, box, intersect);
            
       // if (dist[i] > MAX_IR_DIST || dist[i] == -1)
//        dist[i] = MAX_IR_DIST;
            
        // check Intersection
        if  (dist[i]  > 0 && dist[i]  < MAX_IR_DIST)
        {
            points[i] = intersect;

            p = intersect;
            found = true;            
        }

        w=w->next;
        
        i++;
    }
    
    
      // Calculate and return distance */
    double smallest_dist = world.x_size + world.y_size;
    int smallest_index = -1;
    
    for (i = 0; i < world.num_walls; i++)
    {
        if (dist[i] > 0 && dist[i] <= smallest_dist)
        {
            smallest_dist = dist[i];
            smallest_index = i;
        }
    }
  
  
  if (smallest_index == -1) return(-1);
  
    p = points[smallest_index];

         //   world_add_impact(points[smallest_index].x, points[smallest_index].y);    
    
    return(dist[smallest_index]);

}
/* End of check sensor */



/* Update all values for the aminal */
void robot_update(void)
{
    double x_diff = robot.x - world.target_x;
    double y_diff = robot.y - world.target_y;
    
    double target_dist = sqrt(x_diff * x_diff + y_diff * y_diff);
    
    if (target_dist < 0.3)
    {
        world_set_target(f_rand(1, world.x_size-1), f_rand(1, world.y_size-1));
    }
    
    
    
    compute_grad_point(robot.x, robot.y, robot.nav_dir, robot.nav_mag);


    point v;
    
    compute_waypoint_vector(robot.x, robot.y, v);

    // Compute distance between waypoints
    double waypoint_seperation =  robot.size;

    double dist = sqrt(v.x * v.x + v.y * v.y);


//    char b[400];    
//    sprintf(b, "Cur waypoint: %d  Dist: %f  sep_dist: %f \n", world.cur_waypoint, dist, waypoint_seperation);
//    write_console(b);
    static double last_waypoint_time = 0;

    // Point to next waypoint if close enough
    if (dist < waypoint_seperation && world.cur_waypoint > 0)
    {
        last_waypoint_time = get_time();
          world.cur_waypoint--;
    }


    if (get_time() - last_waypoint_time  > 1.0)
        world_compute_map_grid();

    
    double d = 0.5;


    // Read all sensors    
    
    // Claculate distance to tip of sensor
     dist = robot.sensor_dist + robot.sensor_size / 2.0;    

    // Calculate angle and delta for sensor check
    double angle = - M_PI / 3.0;
    double delta = 2 * M_PI / 6.0;

    // Check the sensors moving clockwise around the animal
    for (int i = 0; i < 6; i++, angle += delta)
    {
        robot.touch[i] = check_sensor(robot.x, robot.y, robot.dir + angle, dist);
        
        robot.ir[i] =  MAX_IR_DIST - check_vision(robot.x, robot.y, robot.dir + angle, robot.wall_found[i], robot.wall_points[i]);
    }


  
    //robot.nav_dir = atan2(sum_y, sum_x);

    
    double nav_off = robot.nav_dir - robot.dir;
    
//    if (nav_off > M_PI) nav_off -= 2*M_PI;
  
    if (nav_off > 2*M_PI) nav_off -= 2*M_PI;
    if (nav_off < 0) nav_off += 2*M_PI;
    
    if (nav_off > M_PI) nav_off = nav_off - 2*M_PI;
    
        
    robot.nav_off = nav_off;
  
    
    //char b[400];
    //sprintf(b, "nav mag  %f \n",  robot.nav_mag);
  //  write_console(b);




//    angle = - M_PI / 5.0;
//    delta = M_PI / 5.0;    

   
//    check_vision(robot.x, robot.y, robot.dir, robot.num_vision_points, robot.vision,
//        robot.end_x, robot.end_y);

    if (!manual_mode)
        robot_auto_mode();





    // Direction change rate is the difference of drive speeds
  /*  a->ddir = a->l_drive - a->r_drive;

    // Speed is the sum of drive speeds
    a->speed = (a->l_drive + a->r_drive) / 2.0;    
*/
    // Add delta to direction
    robot.dir += robot.ddir;

    if (robot.dir > M_PI ) robot.dir -= 2*M_PI; 
    if (robot.dir < -M_PI ) robot.dir += 2*M_PI; 


//    if (robot.dir < 0 ) robot.dir += 2*M_PI;



    // Calulate x and y deltas
    robot.dx = robot.speed * cos(robot.dir);
    robot.dy = robot.speed * sin(robot.dir);


    double test_x = robot.x + robot.dx;
    double test_y = robot.y + robot.dy;



    point p;
    bool found;

    bool hit = false;


    unsigned int check_points = 12;

    // Calculate angle and delta for sensor check
     angle = 0;
    delta =  (2 * M_PI) / (double)check_points;

   // Check the sensors moving clockwise around the animal
    for (int i = 0; i < check_points; i++, angle += delta)
    { 
        if ((d = check_vision(test_x, test_y, robot.dir + angle, found, p)) < robot.size/2.0)
        {
            robot.dx -= (robot.size/2.0 - d) * cos(robot.dir + angle);
            robot.dy -= (robot.size/2.0 - d) * sin(robot.dir + angle);    
        } 
    }
    
   // } hit = true;
    
/*    if (check_vision(test_x, test_y, robot.dir + M_PI/4.0, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir - M_PI/4.0, found, p) < 0.15) hit = true;

    if (check_vision(test_x, test_y, robot.dir + M_PI, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir + 3*M_PI/4.0, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir + 5*M_PI/4.0, found, p) < 0.15) hit = true;*/

    
    //if (!hit)
    {
        // Add deltas to positon
        robot.x += robot.dx;
        robot.y += robot.dy;
    }


        // Check collision with outer walls        
        if (robot.x < robot.x_size/2)
        {            
            robot.x = robot.x_size/2;
          //  robot.wall_hit++;    
        }
        
        if (robot.x > world.x_size - robot.x_size/2)
        {
            robot.x = world.x_size - robot.x_size/2;
          //  robot.wall_hit++;        
        }
            
        if (robot.y < robot.y_size/2)
        {
            robot.y = robot.y_size/2;
           // robot.wall_hit++;                    
        }
            
        if (robot.y > world.y_size - robot.y_size/2)
        {
            robot.y = world.y_size - robot.y_size/2;
           // robot.wall_hit++;        
        }
 
    
    static double last_x = 0;
    static double last_y = 0;
    
    if ((last_x-robot.x) * (last_x - robot.x) +
        (last_y-robot.y) * (last_y - robot.y) > (robot.size * robot.size)/64.0)
    {
    last_x = robot.x;
    last_y = robot.y;
        
    world_add_marker(robot.x , robot.y );
    }    
        
    
    
    
    
}


/* Manually Set motions for the first anaimal */
void robot_motion(double l, double r)
{
    robot.l_drive = l;
    robot.r_drive = r;
}
/* End of animal_motion */


/* Setup an animal for the first time */
void robot_init( void )
{
    robot.size = 0.5;
   
    robot.x_size = robot.size;
    robot.y_size = robot.size;
    
    robot.num_vision_points = 5;
    robot.num_touch_points = 6;
      
    robot.sensor_dist = (robot.x_size + robot.y_size) / 4.0;
    robot.sensor_size = (robot.x_size + robot.y_size) / 8.0;    
                                

    robot.x = f_rand(robot.x_size, world.x_size - robot.x_size);
    robot.y = f_rand(robot.y_size, world.y_size - robot.y_size);

    robot.dir = f_rand(0, 2*M_PI);

    robot.r_drive = 0;
    robot.l_drive = 0;
                    
                                
    // Clear touch sensor inputs
    for (int i = 0; i < 6; i++)    
        robot.touch[i] = 0;

    // Clear vision inputs    
  /*  for (int i = 0; i < robot.num_vision_points; i++)    
    {
        // Clear vision vectors
        robot.end_x[2*i] = robot.x;
        robot.end_y[2*i] = robot.y;
        
        robot.end_x[2*i+1] = robot.x;
        robot.end_y[2*i+1] = robot.y;
    }  */
    

    for (int i = 0; i < 6; i++)
       robot.wall_found[i] = false;
       
       
    robot.draw_vision = false;
}
/* End of animat_init */