robot.cpp

用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控制端。控制电机为标准舵机。

       
  /*  char b[300];
    sprintf(b, "data: %x %x %x %x %x %x %x %x", 
        buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7]);
    add_line(b);
    */   
       
       
       
    LeaveCriticalSection(&telemetry_data_lock); 
}
/* End of get_telemetry (buffer) */



#define num_long_ave 50

double long_aves[num_long_ave];
double long_ave = 0;
double ave_offset = 0;


double vel = 0;
double pos = 0;

void reset_vals( void )
{
  ave_offset = long_ave;
 vel = 0;
pos = 0;   
     
}



/* Set telemeter data from buffer */
void robot_class::set_telemetry(unsigned char * buffer)   // telemetry into buffer
{
    EnterCriticalSection(&telemetry_data_lock);

    // Decode the compacted analog data
    telemetry_data.analog_data[0] = (buffer[1] + (buffer[9] & 0x03) *256 )  ;
    telemetry_data.analog_data[1] = (buffer[2] + ((buffer[9]>>2)& 0x03) *256) ;
    telemetry_data.analog_data[2] = (buffer[3] + ((buffer[9]>>4)& 0x03) *256)  ;

    telemetry_data.analog_data[3] = (buffer[4] + ((buffer[9]>>6)& 0x03 )*256) ;
    telemetry_data.analog_data[4] = (buffer[5] + ((buffer[10])& 0x03) *256)  ;  
    telemetry_data.analog_data[5] = (buffer[6] + ((buffer[10]>>2)& 0x03) *256)  ;  

    telemetry_data.analog_data[6] = (buffer[7] + ((buffer[10]>>4)& 0x03) *256)  ;  
    telemetry_data.analog_data[7] = (buffer[8] + ((buffer[10]>>6)& 0x03) *256)  ;  
    
//     telemetry_data.encoders[0] = buffer[8];     
     
     telemetry_data.encoder_counts[0] = buffer[11];
     telemetry_data.encoder_counts[0] += (unsigned int)(buffer[12]) << 8;

     telemetry_data.encoder_counts[1] = buffer[13];
     telemetry_data.encoder_counts[1] += (unsigned int)(buffer[14]) << 8;

     telemetry_data.encoder_counts[2] = buffer[15];
     telemetry_data.encoder_counts[2] += (unsigned int)(buffer[16]) << 8;
     
     telemetry_data.encoder_counts[3] = buffer[17];
     telemetry_data.encoder_counts[3] += (unsigned int)(buffer[18]) << 8;
     
      char b[100];    
     
     
     
     static double last_time = get_time();
     static int last_count = 0;
     
     if (telemetry_data.encoder_counts[0] != last_count)
     {
         last_count =  telemetry_data.encoder_counts[0];
         last_time = get_time();
     }
     
//     sprintf(b, "last_time: %f  cur_time: %f diff: %f", last_time, get_time(), get_time() - last_time);
//     add_line(b);
         
    bool  moving = false; 
         
    if (get_time() - last_time < 0.10 || control_data.left_motor_target > 0
                                      || control_data.right_motor_target > 0)
    {
        moving = true;                 
    }      
                                          
   
     
       
     for (int i = 1; i < num_long_ave; i++)
          long_aves[i-1] = long_aves[i];
     
     long_aves[num_long_ave-1] = (double)(telemetry_data.analog_data[6]);
     
     double sum = 0;     

     for (int i = 0; i < num_long_ave; i++)
         sum += long_aves[i];
         
     long_ave =  sum / (double)(num_long_ave);     
     
     
     
     
     
     
     

     
     #define num_ave 3
     
     static double last_nums[num_ave];
     
     
     for (int i = 1; i < num_ave; i++)
          last_nums[i-1] = last_nums[i];
     
     last_nums[num_ave-1] = (double)(telemetry_data.analog_data[6]) - ave_offset;
     
     sum = 0;     

     for (int i = 0; i < num_ave; i++)
         sum += last_nums[i];

     double accl = sum / (double)(num_ave);
     
//     double accl = (double)telemetry_data.analog_data[6] - 480;
     


    static double last_accl_time = get_time();

     

     
     if (moving)//fabs(accl) > 5)
     {
      double cur_time = get_time();
      
      double time_diff = cur_time - last_accl_time;
      
      last_accl_time = cur_time;
      
      vel += accl * time_diff;
                    
     } else
     vel = 0;
     
      static double last_pos_time = get_time();

  
     
     
   //  if (fabs(vel) > 1)
     {
      double cur_time = get_time();
      
      double time_diff = cur_time - last_pos_time;
      
      last_pos_time = cur_time;
      
      pos += vel * time_diff * 10;
     }   



     
  //   FILE *f = fopen("accl.csv", "at");
   //  fprintf(f, "%d\n", telemetry_data.analog_data[6]);
  //   fclose(f);
     
     
     sprintf(b, "long ave: %f  Accel: %6.2f  vel: %6.2f  pos: %6.2f", long_ave, accl, vel, pos);
   //  add_line(b);
     
         /*
     sprintf(b, "Encoder counts: %5d %5d %5d %5d",
             telemetry_data.encoder_counts[0],
            telemetry_data.encoder_counts[1],
            telemetry_data.encoder_counts[2],
            telemetry_data.encoder_counts[3]);
     add_line(b);*/
     

     
     
    
    LeaveCriticalSection(&telemetry_data_lock);     
}
/* End of set_telemetry (buffer) */


/* Apply new motion data to robot */
void robot_class::set_motion(double x_move, double y_move)
{
//    char b[400];
//    sprintf(b, "x_motion: %f  y_motion: %f", x_move, y_move);
//    add_line(b);

    double l_motor = 2*y_move + x_move;
    double r_motor = 2*y_move - x_move;
    
    if (l_motor > 1) l_motor = 1;
    if (l_motor < -1) l_motor = -1;

    if (r_motor > 1) r_motor = 1;
    if (r_motor < -1) r_motor = -1;

//    sprintf(b, "l_motor: %f  r_motor: %f", l_motor, r_motor);
//    add_line(b);

    control_type c;

    robot.get_control(&c);

    // Set motion values
    c.left_motor_target = fabs(l_motor) * 255; 
    c.right_motor_target = fabs(r_motor) * 255;

    c.left_motor_dir = l_motor < 0;
    c.right_motor_dir = r_motor < 0;  
    
    
    char b[300];
    
/*    sprintf(b, "l:  %f  r:  %f     ld: %d  rd:  %d", 
    
      c.left_motor_target,
    c.right_motor_target,

    c.left_motor_dir, 
    c.right_motor_dir);  
    
    
    
    debug(b);*/
    
    
    
    robot.set_control(c);
}
/* End of set_motion */


/* Return the robot update speed */
double robot_class::get_update_speed(void)
{
    EnterCriticalSection(&update_speed_lock);

    // Copy update speed
    double speed = update_speed;

    LeaveCriticalSection(&update_speed_lock);

    return(speed);
}
/* End of get_update_speed */


/* This is called when an update is done */
void robot_class::update_complete(bool fail)
{
    // Check for update fail
    if (fail)
    {
        updates = 0;    
        update_speed = 0;
        return;
    }

    // Init on first run
    static double last_time = get_time();
    
    // Currect
    double cur_time = get_time();
    
    // Update sucess
    updates++;
    
    // Update speed 10 times per second
    if (cur_time - last_time > 0.1)
    {
        // Calculate update speed
        update_speed = updates / (cur_time - last_time);
        updates = 0;

        // Set new time offset
        last_time = cur_time;
    }
}
/* End of update_complete */