Revision 525
Did some slam work, trying to get together a good statistical way of reading range values.
There will be experimenting.... boring horrible experimenting
| data_requests.c | ||
|---|---|---|
| 1 | 1 |
#include "data_requests.h" |
| 2 | 2 |
#include <stdlib.h> |
| 3 | 3 |
#include <stdio.h> |
| 4 |
#include <pthread.h> |
|
| 5 |
#include "../../../libwireless/lib/wireless.h" |
|
| 4 |
#include "../../libwireless/lib/wireless.h" |
|
| 6 | 5 |
#include <string.h> |
| 7 | 6 |
|
| 8 | 7 |
PacketGroupHandler pgh; |
| 9 |
pthread_t* dr_thread; |
|
| 10 | 8 |
|
| 11 | 9 |
void receive_handle(char type, int source, unsigned char* packet, int length); |
| 12 | 10 |
|
| 13 |
void (*bom_handle)(BomNode** head); |
|
| 14 |
void (*IR_handle)(short** data); |
|
| 15 |
void (*encoder_handle)(unsigned char** data); |
|
| 11 |
void (*bom_handle)(short id, BomNode** head);
|
|
| 12 |
void (*IR_handle)(short id, short** data);
|
|
| 13 |
void (*encoder_handle)(short id, unsigned char** data);
|
|
| 16 | 14 |
|
| 17 |
void data_requests_init(void(*bom_data_handler)(BomNode** head), |
|
| 18 |
void(*IR_data_handler)(short** data), |
|
| 19 |
void(*encoder_data_handler)(unsigned char** data)){
|
|
| 15 |
void data_requests_init(void(*bom_data_handler)(short id, BomNode** head),
|
|
| 16 |
void(*IR_data_handler)(short id, short** data),
|
|
| 17 |
void(*encoder_data_handler)(short id, unsigned char** data)){
|
|
| 20 | 18 |
bom_handle = bom_data_handler; |
| 21 | 19 |
IR_handle = IR_data_handler; |
| 22 | 20 |
encoder_handle = encoder_data_handler; |
| ... | ... | |
| 43 | 41 |
wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ENCODER_TYPE,NULL,0,robot_id,0); |
| 44 | 42 |
} |
| 45 | 43 |
|
| 46 |
/*void* pthread_handle(void* arg){
|
|
| 47 |
RawData* data = (RawData*)arg; |
|
| 48 |
unsigned char* packet = data->packet; |
|
| 49 |
int length = data->length; |
|
| 50 |
char type = data->type; |
|
| 51 |
int i=0; |
|
| 52 |
BomNode* head; |
|
| 53 |
switch(type){
|
|
| 54 |
|
|
| 55 |
case BOM_TYPE: |
|
| 56 |
//BomNode* head; |
|
| 57 |
head = malloc(sizeof(BomNode)); |
|
| 58 |
|
|
| 59 |
BomNode* iter = head; |
|
| 60 |
|
|
| 61 |
while(i<length){
|
|
| 62 |
iter->robot_id = ((short)packet[i]<<8) + ((short)packet[i+1]); |
|
| 63 |
i+=2; |
|
| 64 |
iter->value = packet[i++]; |
|
| 65 |
if(i<length){
|
|
| 66 |
iter-> next = malloc(sizeof(BomNode)); |
|
| 67 |
iter = iter->next; |
|
| 68 |
} |
|
| 69 |
} |
|
| 70 |
bom_handle(head); |
|
| 71 |
break; |
|
| 72 |
|
|
| 73 |
case IR_TYPE: |
|
| 74 |
An IR packet consists of just the 5 bytes representing the IR |
|
| 75 |
values, in order. |
|
| 76 |
if(length != 10) break; |
|
| 77 |
short data[10]; |
|
| 78 |
for(i=0;i<5;i++){
|
|
| 79 |
data[i] = ((short)packet[2*i]<<8)+((short)packet[2*i+1]); |
|
| 80 |
} |
|
| 81 |
IR_handle(data); |
|
| 82 |
break; |
|
| 83 |
|
|
| 84 |
case ENCODER_TYPE: |
|
| 85 |
I have no idea, unsupported. |
|
| 86 |
break; |
|
| 87 |
} |
|
| 88 |
free(data->packet); |
|
| 89 |
free(data); |
|
| 90 |
return NULL; |
|
| 91 |
|
|
| 92 |
}*/ |
|
| 93 |
|
|
| 94 |
|
|
| 95 | 44 |
void receive_handle(char type, int source, unsigned char* packet, int length){
|
| 96 | 45 |
int i=0; |
| 97 | 46 |
BomNode* head; |
| 98 |
printf("Packet contents: [ ");
|
|
| 99 |
for(i=0;i<length;i++){
|
|
| 100 |
printf("%i ",packet[i]);
|
|
| 101 |
} |
|
| 102 | 47 |
switch(type){
|
| 103 | 48 |
|
| 104 | 49 |
case BOM_TYPE: |
| 105 |
printf("I shouldn't be here during testing\n");
|
|
| 106 | 50 |
head = malloc(sizeof(BomNode)); |
| 107 | 51 |
BomNode* iter = head; |
| 108 | 52 |
|
| ... | ... | |
| 111 | 55 |
i+=2; |
| 112 | 56 |
iter->value = packet[i++]; |
| 113 | 57 |
if(i<length){
|
| 114 |
iter-> next = malloc(sizeof(BomNode));
|
|
| 58 |
iter->next = malloc(sizeof(BomNode)); |
|
| 115 | 59 |
iter = iter->next; |
| 116 | 60 |
} |
| 117 | 61 |
} |
| 118 |
bom_handle(head); |
|
| 62 |
bom_handle(source,&head);
|
|
| 119 | 63 |
break; |
| 120 | 64 |
|
| 121 | 65 |
case IR_TYPE: |
| 122 | 66 |
/*An IR packet consists of just the 10 bytes representing the IR |
| 123 | 67 |
values as shorts, in order.*/ |
| 124 | 68 |
if(length != 10) break; |
| 69 |
|
|
| 125 | 70 |
short* data = malloc(5*sizeof(short)); |
| 126 | 71 |
for(i=0;i<5;i++){
|
| 127 | 72 |
data[i]= ((((short)packet[2*i])<<8)&0xff00 ) | ( (short)(packet[2*i+1])&255); |
| 128 | 73 |
} |
| 129 | 74 |
|
| 130 |
IR_handle(&data); |
|
| 75 |
IR_handle(source, &data);
|
|
| 131 | 76 |
break; |
| 132 | 77 |
|
| 133 | 78 |
case ENCODER_TYPE: |
| 134 |
/*I have no idea, unsupported.*/ |
|
| 79 |
/*I have no idea yet, unsupported.*/
|
|
| 135 | 80 |
break; |
| 136 | 81 |
} |
| 137 | 82 |
} |
| 138 | 83 |
|
| 139 |
|
|
| 140 |
/*void receive_handle(char type, int source, unsigned char* packet, int length){
|
|
| 141 |
|
|
| 142 |
RawData* data = malloc(sizeof(nawData)); |
|
| 143 |
if(!data){
|
|
| 144 |
return; |
|
| 145 |
} |
|
| 146 |
|
|
| 147 |
unsigned char* newPacket = malloc(length); |
|
| 148 |
memcpy(newPacket, packet,length); |
|
| 149 |
|
|
| 150 |
data->packet=newPacket; |
|
| 151 |
data->source=source; |
|
| 152 |
data->length=length; |
|
| 153 |
data->type=type; |
|
| 154 |
|
|
| 155 |
dr_thread = malloc(sizeof(pthread_t)); |
|
| 156 |
pthread_create(dr_thread,NULL,pthread_handle,(void*)data); |
|
| 157 |
}*/ |
|
Also available in: Unified diff