Revision 175
robot actually does stuff. Moves, but GUI control is off.
trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.cpp | ||
---|---|---|
77 | 77 |
return 0; |
78 | 78 |
} |
79 | 79 |
|
80 |
void colonet_wl_send(int client_source, short dest,
|
|
80 |
void colonet_wl_send(short client_source, short dest,
|
|
81 | 81 |
ColonetMessageType msg_type, unsigned char msg_code, |
82 | 82 |
unsigned char* args) { |
83 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", |
|
84 |
client_source, dest, msg_code); |
|
85 |
|
|
83 | 86 |
ColonetRobotServerPacket pkt; |
84 | 87 |
pkt.client_id = client_source; |
85 | 88 |
pkt.msg_code = msg_code; |
... | ... | |
88 | 91 |
pkt.data[i] = args[i]; |
89 | 92 |
} |
90 | 93 |
|
91 |
char pkt_buffer[40]; |
|
92 |
memcpy(pkt_buffer, (char*)(&pkt), sizeof(ColonetRobotServerPacket)); |
|
93 |
|
|
94 | 94 |
if (dest == GLOBAL_DEST) { |
95 | 95 |
printf("sending to global dest\n"); |
96 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, pkt_buffer, |
|
97 |
1 + PACKET_DATA_LEN, 0); |
|
96 |
|
|
97 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, |
|
98 |
(char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
|
98 | 99 |
} else { |
99 | 100 |
printf("sending to specific robot.\n"); |
100 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, |
|
101 |
pkt_buffer, 1 + PACKET_DATA_LEN, dest, |
|
101 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, |
|
102 |
(char)msg_type, (char*)(&pkt), |
|
103 |
sizeof(ColonetRobotServerPacket), |
|
104 |
dest, |
|
102 | 105 |
COLONET_RESPONSE_PACKET_FRAME_ID); |
103 | 106 |
} |
104 | 107 |
} |
trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.h | ||
---|---|---|
42 | 42 |
* |
43 | 43 |
* @return void |
44 | 44 |
*/ |
45 |
void colonet_wl_send(int client_source, short dest,
|
|
45 |
void colonet_wl_send(short client_source, short dest,
|
|
46 | 46 |
ColonetMessageType msg_type, unsigned char msg_code, |
47 | 47 |
unsigned char* args); |
48 | 48 |
|
trunk/code/projects/colonet/lib/colonet_dragonfly/colonet_dragonfly.c | ||
---|---|---|
62 | 62 |
unsigned char* args; //up to 7 char args |
63 | 63 |
unsigned int int_args[3]; //up to 3 int (2-byte) args |
64 | 64 |
|
65 |
orb_set_color(BLUE); |
|
66 | 65 |
usb_puts("Packet received.\n"); |
66 |
char buf[40]; |
|
67 |
sprintf(buf, "length=%d\n", length); |
|
68 |
|
|
69 |
int i; |
|
70 |
for (i = 0; i < length; i++) { |
|
71 |
sprintf(buf, "%d: %d ", i, packet[i]); |
|
72 |
usb_puts(buf); |
|
73 |
} |
|
74 |
usb_puts("\n"); |
|
75 |
|
|
67 | 76 |
//printf("received message from %d of length %d\n", source, length); |
68 | 77 |
|
69 | 78 |
///assert(length == sizeof(ColonetRobotServerPacket)); |
70 | 79 |
packet_string_to_struct(&pkt, packet); |
71 | 80 |
|
81 |
char buf2[40]; |
|
82 |
sprintf(buf2, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code); |
|
83 |
usb_puts(buf2); |
|
84 |
|
|
72 | 85 |
args = pkt.data; |
73 | 86 |
|
74 | 87 |
int_args[0] = two_bytes_to_int(args[0], args[1]); |
... | ... | |
76 | 89 |
int_args[2] = two_bytes_to_int(args[4], args[5]); |
77 | 90 |
|
78 | 91 |
if (type == COLONET_REQUEST) { |
92 |
usb_puts("type is colonet request\n"); |
|
93 |
|
|
79 | 94 |
/* TODO - send back data! */ |
80 | 95 |
|
81 | 96 |
switch (pkt.msg_code) { |
... | ... | |
115 | 130 |
break; |
116 | 131 |
} |
117 | 132 |
} else if (type == COLONET_COMMAND) { |
118 |
if(pkt.msg_code >= USER_DEFINED_MSG_ID_START && |
|
119 |
pkt.msg_code <= USER_DEFINED_MSG_ID_END){ |
|
120 |
if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { |
|
121 |
/* Call the user's handler function if it the function's address |
|
122 |
* is non-zero (has been set) */ |
|
123 |
user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); |
|
124 |
} |
|
125 |
} |
|
133 |
usb_puts("type is colonet command...\n"); |
|
126 | 134 |
|
127 |
switch(pkt.msg_code){ |
|
128 |
default: |
|
129 |
printf("%s: Error - message code %d not implemented\n", __FUNCTION__, |
|
130 |
pkt.msg_code); |
|
131 |
break; |
|
135 |
/* TODO uncomment this stuff */ |
|
136 |
/* if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */ |
|
137 |
/* pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */ |
|
138 |
/* if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */ |
|
139 |
/* /\* Call the user's handler function if it the function's address */ |
|
140 |
/* * is non-zero (has been set) *\/ */ |
|
141 |
/* user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */ |
|
142 |
/* } */ |
|
143 |
/* } */ |
|
132 | 144 |
|
145 |
switch (pkt.msg_code) { |
|
146 |
/* default: */ |
|
147 |
/* printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */ |
|
148 |
/* pkt.msg_code); */ |
|
149 |
/* break; */ |
|
150 |
|
|
133 | 151 |
//Buzzer |
134 | 152 |
case BUZZER_INIT: |
135 | 153 |
buzzer_init(); |
... | ... | |
172 | 190 |
break; |
173 | 191 |
//Motors |
174 | 192 |
case MOTORS_INIT: |
193 |
usb_puts("calling motors_init\n"); |
|
175 | 194 |
motors_init(); |
176 | 195 |
break; |
177 | 196 |
case MOTOR1_SET: |
197 |
usb_puts("calling motor1_set\n"); |
|
178 | 198 |
motor1_set(int_args[0], int_args[1]); |
179 | 199 |
break; |
180 | 200 |
case MOTOR2_SET: |
201 |
usb_puts("calling motor2_set\n"); |
|
181 | 202 |
motor2_set(int_args[0], int_args[1]); |
182 | 203 |
break; |
183 | 204 |
case MOTORS_OFF: |
205 |
usb_puts("calling motors_off\n"); |
|
184 | 206 |
motors_off(); |
185 | 207 |
break; |
186 |
|
|
187 |
/* |
|
188 | 208 |
case MOVE: |
189 |
|
|
209 |
buf[40]; |
|
210 |
sprintf(buf, "calling move with: %d, %d\n", int_args[0], int_args[1]); |
|
211 |
usb_puts(buf); |
|
212 |
move(args[0], args[1]); |
|
190 | 213 |
break; |
191 |
*/ |
|
192 |
|
|
193 | 214 |
case XBEE_INIT: |
194 | 215 |
xbee_init(); |
195 | 216 |
break; |
... | ... | |
238 | 259 |
break; |
239 | 260 |
} |
240 | 261 |
} else { |
241 |
printf("%s: Error: Invalid colonet message type", __FUNCTION__); |
|
262 |
char buf[80]; |
|
263 |
sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__); |
|
264 |
usb_puts(buf); |
|
242 | 265 |
} |
243 | 266 |
} |
244 | 267 |
|
trunk/code/projects/colonet/lib/colonet_defs.h | ||
---|---|---|
37 | 37 |
* wireless library to the robots and from the robots to the colonet wireless |
38 | 38 |
* library. */ |
39 | 39 |
typedef struct { |
40 |
int client_id; // ID number of the client sending the packet. |
|
40 |
short client_id; // ID number of the client sending the packet. |
|
41 |
// Size of short is common to robots and server. |
|
41 | 42 |
unsigned char msg_code; // Specific instruction for the robot -- see below. |
42 | 43 |
unsigned char data[PACKET_DATA_LEN]; |
43 | 44 |
} ColonetRobotServerPacket; |
trunk/code/projects/colonet/ColonetServer/Command.cpp | ||
---|---|---|
145 | 145 |
return 0; |
146 | 146 |
} |
147 | 147 |
|
148 |
int Command::parse_send_to_robot(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) { |
|
148 |
int Command::parse_send_to_robot(int number_tokens, |
|
149 |
char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], |
|
150 |
int pool_index) { |
|
149 | 151 |
int i; |
150 | 152 |
unsigned char int_tokens[MAX_TOKENS]; |
151 | 153 |
int number_int_tokens = number_tokens; |
... | ... | |
170 | 172 |
} |
171 | 173 |
|
172 | 174 |
// Send packet to robot |
173 |
fprintf(stderr, "Calling colonet_wl_send(%d, %d, %d, arguments)\n", int_tokens[0], int_tokens[1], int_tokens[2]); |
|
174 |
colonet_wl_send(pool_index, int_tokens[0], (ColonetMessageType)int_tokens[1], int_tokens[2], arguments); |
|
175 |
fprintf(stderr, "Calling colonet_wl_send(%d, %d, %d, arguments)\n", |
|
176 |
int_tokens[0], int_tokens[1], int_tokens[2]); |
|
177 |
colonet_wl_send((short)pool_index, int_tokens[0], |
|
178 |
(ColonetMessageType)int_tokens[1], int_tokens[2], arguments); |
|
175 | 179 |
|
176 | 180 |
return 0; |
177 | 181 |
} |
Also available in: Unified diff