Revision 599
General work on SLAM
data_requests.c | ||
---|---|---|
41 | 41 |
wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ENCODER_TYPE,NULL,0,robot_id,0); |
42 | 42 |
} |
43 | 43 |
|
44 |
void request_all(int robot_id){ |
|
45 |
wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ALL_TYPE,NULL,0,robot_id,0); |
|
46 |
|
|
47 |
} |
|
48 |
|
|
44 | 49 |
void receive_handle(char type, int source, unsigned char* packet, int length){ |
45 | 50 |
int i=0; |
46 | 51 |
BomNode* head; |
... | ... | |
78 | 83 |
case ENCODER_TYPE: |
79 | 84 |
/*I have no idea yet, unsupported.*/ |
80 | 85 |
break; |
86 |
|
|
87 |
case ALL_TYPE: |
|
88 |
short* data = malloc(5*sizeof(short)); |
|
89 |
for(i=0;i<5;i++) |
|
90 |
data[i]= ((((short)packet[2*i])<<8)&0xff00 ) | ( (short)(packet[2*i+1])&255); |
|
91 |
|
|
92 |
head = malloc(sizeof(BomNode)); |
|
93 |
BomNode* iter = head; |
|
94 |
|
|
95 |
while(i<length){ |
|
96 |
iter->robot_id = ((short)packet[i]<<8) + ((short)packet[i+1]); |
|
97 |
i+=2; |
|
98 |
iter->value = packet[i++]; |
|
99 |
if(i<length){ |
|
100 |
iter->next = malloc(sizeof(BomNode)); |
|
101 |
iter = iter->next; |
|
102 |
} |
|
103 |
} |
|
104 |
|
|
105 |
bom_handle(source,&head); |
|
106 |
IR_handle(source, &data); |
|
107 |
encoder_handle(source,NULL); |
|
108 |
break; |
|
81 | 109 |
} |
82 | 110 |
} |
83 | 111 |
|
112 |
|
Also available in: Unified diff