Project

General

Profile

Revision 411

Added by Justin Scheiner about 16 years ago

Laid out what I think will be the structure of SLAM for colony. This is an interesting project, and I think my strategy is a good one. It's laid out in the file server_main.c so read it if you're interested. I threw together the bare bones, but I realized that I might need to change my data_request code so that the order of packet arrival doesn't matter.

View differences:

data_response.c
16 16 
  Prepares the robot to receive and respond to data requests.
17 17 
*/
18 18 
void data_response_init(void){
19 
  usb_init();
20 
  usb_puts("USB initialized!\n\r");
19 
  //usb_init();
20 
  //usb_puts("USB initialized!\n\r");
21 21 
  range_init();
22 22 
  pgh.groupCode=DATA_REQUEST_GROUP;
23 23 
  pgh.timeout_handler = NULL;
......
25 25 
  pgh.handle_receive = handle_receive;
26 26 
  pgh.unregister = NULL;
27 27 
  wl_register_packet_group(&pgh);
28 
  usb_puts("Packet Group Registered!\n\r");
28 
  //usb_puts("Packet Group Registered!\n\r");
29 29 
}
30 30 

  		




  31
  31
  
    

  		




  ......




  35
  35
  
    
void handle_receive(char type, int source, unsigned char* packet, int length){


  		




  36
  36
  
    
  


  		




  37
  37
  
    
  char buf[MAX_PACKET_LENGTH]; 


  		




  38
  
  
    
  int size, i;


  		




  39
  
  
    
  usb_puts("Got a packet!\n\r");


  		




  
  38
  
    
  int size;


  		




  
  39
  
    
  int i=0;


  		




  
  40
  
    
  short int robot_id, ir_value; 


  		




  
  41
  
    
  char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 


  		




  
  42
  
    
  


  		




  40
  43
  
    
  switch(type){


  		




  41
  44
  
    
  case BOM_TYPE:


  		




  42
  45
  
    
    


  		




  43
  
  
    
    short int i=0, robot_id; 


  		




  44
  46
  
    
    wl_token_iterator_begin();


  		




  45
  47
  
    
    while(wl_token_iterator_has_next() && i<=96){


  		




  46
  48
  
    
      robot_id = wl_token_iterator_next();


  		




  47
  
  
    
      (*buf)[i++] = (char)((robot_id>>8) & 255);


  		




  48
  
  
    
      (*buf)[i++] = (char)(robot_id & 255);


  		




  49
  
  
    
      //TODO Ensure that wl_token_get.... returns the expected char. 


  		




  50
  
  
    
      (*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));


  		




  
  49
  
    
      buf[i++] = (char)((robot_id>>8) & 255);


  		




  
  50
  
    
      buf[i++] = (char)(robot_id & 255);


  		




  
  51
  
    
      buf[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));


  		




  51
  52
  
    
    }


  		




  52
  53
  
    
    


  		




  53
  54
  
    
    //i is one greater than the index of the last byte... the size.


  		




  54
  55
  
    
    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,i,source,0);


  		




  
  56
  
    
    break;


  		




  55
  57
  
    
  


  		




  56
  58
  
    
  case IR_TYPE:


  		




  57
  59
  
    

  		




  58
  
  
    
    usb_puts("Generating an IR packet.\n");


  		




  59
  
  
    
    char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 


  		




  60
  
  
    
    int i;


  		




  61
  
  
    
    short ir_value; 


  		




  
  60
  
    
    //usb_puts("Generating an IR packet.\n");


  		




  62
  61
  
    
    for(i=0;i<5;i++){


  		




  63
  62
  
    
      ir_value = (short)range_read_distance(ranges[i]);


  		




  64
  63
  
    
      buf[2*i] = (char)((ir_value>>8)&255);   


  		




  65
  64
  
    
      buf[2*i+1] = (ir_value&255);   


  		




  66
  
  
    
      usb_puts("Distance :\t");


  		




  67
  
  
    
      usb_puti(ir_value);


  		




  68
  
  
    
      usb_puts("\n");


  		




  
  65
  
    
      //usb_puts("Distance :\t");


  		




  
  66
  
    
      //usb_puti(ir_value);


  		




  
  67
  
    
      //usb_puts("\n");


  		




  69
  68
  
    
    }


  		




  70
  69
  
    
    size = 10;


  		




  71
  70
  
    

  		




  72
  
  
    
    usb_puts("Packet Contents: ");


  		




  73
  
  
    
    for(i = 0; i<size;i++){


  		




  74
  
  
    
        usb_puti(buf[i]);


  		




  
  71
  
    
    //usb_puts("Packet Contents: ");


  		




  
  72
  
    
    /*for(i = 0; i<size;i++){


  		




  
  73
  
    
        //usb_puti(buf[i]);


  		




  75
  74
  
    
        usb_putc(' ');


  		




  76
  
  
    
    }


  		




  
  75
  
    
    }*/


  		




  77
  76
  
    
    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,IR_TYPE,buf,size,source,0);


  		




  78
  77
  
    
  


  		




  
  78
  
    
    break; 


  		




  79
  79
  
    
  


  		




  80
  
  
    
  


  		




  81
  80
  
    
  case ENCODER_TYPE:


  		




  82
  81
  
    
    break; 


  		




  83
  82
  
    
    //Not yet supported.


  		




  ......




  105
  104
  
    
* 


  		




  106
  105
  
    
*/


  		




  107
  106
  
    
int generate_IR_packet(char** buf){


  		




  108
  
  
    
  usb_puts("Generating an IR packet.\n");


  		




  
  107
  
    
  //usb_puts("Generating an IR packet.\n");


  		




  109
  108
  
    
  char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 


  		




  110
  109
  
    
  int i;


  		




  111
  110
  
    
  short ir_value; 


  		




  ......




  113
  112
  
    
    ir_value = (short)range_read_distance(ranges[i]);


  		




  114
  113
  
    
    (*buf)[2*i] = (char)((ir_value>>8)&255);   


  		




  115
  114
  
    
    (*buf)[2*i+1] = (ir_value&255);   


  		




  116
  
  
    
    usb_puts("Distance :\t");


  		




  117
  
  
    
    usb_puti(ir_value);


  		




  118
  
  
    
    usb_puts("\n");


  		




  
  115
  
    
    //usb_puts("Distance :\t");


  		




  
  116
  
    
    //usb_puti(ir_value);


  		




  
  117
  
    
    //usb_puts("\n");


  		




  119
  118
  
    
  }


  		




  120
  119
  
    
  return 10;


  		




  121
  120
  
    
}


  		












Also available in:  Unified diff