Revision 175
robot actually does stuff. Moves, but GUI control is off.
| 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 |
|
Also available in: Unified diff