Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / lib / colonet_dragonfly / colonet_dragonfly.c @ 349

History | View | Annotate | Download (7.69 KB)

1
/** @file colonet.c
2
 * @brief Colonet library for DRAGONFLY colony robots
3
 *
4
 * @author Eugene Marinelli
5
 * @date 10/10/07
6
 *
7
 * @bug Handler registration not tested
8
 * @bug Request reponding not implemented - only accepts commands.
9
 */
10

    
11
#include <assert.h>
12
#include <dragonfly_lib.h>
13
#include <string.h>
14
#include <wireless.h>
15

    
16
#include "colonet_dragonfly.h"
17

    
18
typedef struct {
19
  unsigned char msgId; //is this necessary?
20
  void (*handler)(void);
21
} UserHandler;
22

    
23
/* Globals (internal) */
24
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
25

    
26
/* Internal function prototypes */
27
static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf);
28
static unsigned int two_bytes_to_int(char high, char low);
29
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length);
30

    
31
static PacketGroupHandler colonet_pgh;
32

    
33
/* Public functions */
34
int colonet_init() {
35
  colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
36
  colonet_pgh.timeout_handler = NULL;
37
  colonet_pgh.handle_response = NULL;
38
  colonet_pgh.handle_receive = colonet_handle_receive;
39
  colonet_pgh.unregister = NULL;
40

    
41
  // TODO this should return an error if wl_init has not been called yet.
42
  wl_register_packet_group(&colonet_pgh);
43

    
44
  return 0;
45
}
46

    
47
/* colonet_add_message
48
 * Adds a user-defined message
49
 */
50
int colonet_add_message(unsigned char msgId, void (*handler)(void)) {
51
  if(msgId < USER_DEFINED_MSG_ID_START  /* || msgId > USER_DEFINED_MSG_ID_END */){
52
    return -1;
53
  }
54

    
55
  /* Register this function in the array */
56
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
57
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
58

    
59
  return 0;
60
}
61

    
62
/* Private functions */
63

    
64
/** @brief Handles colonet packets.  Should be called by parse_buffer
65
 * when it is determined that a colonet message has been received.
66
 *
67
 * @param robot_id The robot id
68
 * @param pkt_buf The packet buffer (e.g. wl_buf)
69
 *
70
 * @return -1 on error (invalid msgId), 0 on success
71
 */
72
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length) {
73
  ColonetRobotServerPacket pkt;
74
  unsigned char* args; //up to 7 char args
75
  unsigned int int_args[3]; //up to 3 int (2-byte) args
76

    
77
  usb_puts("Packet received.\n");
78
  char buf[40];
79
  sprintf(buf, "length=%d\n", length);
80

    
81
  int i;
82
  for (i = 0; i < length; i++) {
83
    sprintf(buf, "%d: %d ", i, packet[i]);
84
    usb_puts(buf);
85
  }
86
  usb_puts("\n");
87

    
88
  //printf("received message from %d of length %d\n", source, length);
89

    
90
  ///assert(length == sizeof(ColonetRobotServerPacket));
91
  packet_string_to_struct(&pkt, packet);
92

    
93
  char buf2[40];
94
  sprintf(buf2, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
95
  usb_puts(buf2);
96

    
97
  args = pkt.data;
98

    
99
  int_args[0] = two_bytes_to_int(args[0], args[1]);
100
  int_args[1] = two_bytes_to_int(args[2], args[3]);
101
  int_args[2] = two_bytes_to_int(args[4], args[5]);
102

    
103
  if (type == COLONET_REQUEST) {
104
    usb_puts("type is colonet request\n");
105

    
106
    /* TODO - send back data! */
107

    
108
    switch (pkt.msg_code) {
109
      //Sharp
110
    case READ_DISTANCE:
111
      break;
112
    case LINEARIZE_DISTANCE:
113
      break;
114
    case LOG_DISTANCE:
115
      break;
116

    
117

    
118
      //Analog
119
    case CALL_ANALOG8:
120
      break;
121
    case CALL_ANALOG10:
122
      break;
123
    case WHEEL:
124
      break;
125
    case BATTERY:
126
            wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, BATTERY,
127
                "brouhaha", 8, 0xA, 0);
128
      break;
129

    
130
      //BOM
131
    case GETMAXBOM:
132
      break;
133

    
134
      //Dio
135
    case DIGITAL_INPUT:
136
      break;
137
    case BUTTON1_READ:
138
      break;
139
    case BUTTON2_READ:
140
      break;
141

    
142
      //Bumper
143
    case DETECT_BUMP:
144
      break;
145
    }
146
  } else if (type == COLONET_COMMAND) {
147
    usb_puts("type is colonet command...\n");
148

    
149
/* TODO uncomment this stuff */
150
/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
151
/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
152
/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
153
/*         /\* Call the user's handler function if it the function's address */
154
/*          * is non-zero (has been set) *\/ */
155
/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
156
/*       } */
157
/*     } */
158

    
159
    switch (pkt.msg_code) {
160
/*     default: */
161
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
162
/*              pkt.msg_code); */
163
/*       break; */
164

    
165
      //Buzzer
166
    case BUZZER_INIT:
167
      buzzer_init();
168
      break;
169
    case BUZZER_SET_VAL:
170
      buzzer_set_val(args[0]);
171
      break;
172
    case BUZZER_SET_FREQ:
173
      buzzer_set_freq(args[0]);
174
      break;
175
    case BUZZER_CHIRP:
176
      buzzer_chirp(int_args[0], int_args[1]);
177
      break;
178
    case BUZZER_OFF:
179
      buzzer_off();
180
      break;
181

    
182
    // Battery
183
    case BATTERY:
184
      usb_puts("Got battery request.\n");
185

    
186
/*
187
      int xbeeDongleID = 0xA;  // too bad this is hard-coded.
188
      char data[20];
189
      data[0] = 0;  // client ID specified in bytes 0-3
190
      data[1] = 0;
191
      data[2] = 0;
192
      data[3] = 0;
193
      data[4] = 0;  // ???
194
      data[5] = RESPONSE_TO_CLIENT_REQUEST;
195
      data[6] = ' ';
196
      data[7] = BATTERY;
197
      data[8] = ' ';
198
      data[9] = (char) wl_get_xbee_id();  // robot number (self)
199
      data[10] = ' ';
200
      data[11] = (char) battery8();  // battery reading
201
      data[12] = '\0';
202

203
      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, COLONET_RESPONSE, data, 13, xbeeDongleID, 0);
204
      usb_puts("lol \n");
205
*/
206
      break;
207

    
208
    case ORB_INIT:
209
      orb_init();
210
      break;
211
      //Orb
212
    case ORB_SET:
213
      orb_set(args[0], args[1], args[2]);
214
      break;
215
    case ORB_SET_COLOR:
216
      orb_set_color(int_args[0]);
217
      break;
218
    case ORB_DISABLE:
219
      orb_disable();
220
      break;
221
    case ORB_ENABLE:
222
      orb_enable();
223
      break;
224
    case LED_INIT:
225
      break;
226
    case LED_USER:
227
      break;
228
    case ORB_SEND:
229
      //orb_send();
230
      break;
231
      //Motors
232
    case MOTORS_INIT:
233
      usb_puts("calling motors_init\n");
234
      motors_init();
235
      break;
236
    case MOTOR1_SET:
237
             sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
238
      usb_puts(buf);
239
      motor1_set(args[0], args[1]);
240
      break;
241
    case MOTOR2_SET:
242
      sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
243
      usb_puts(buf);
244
      motor2_set(args[0], args[1]);
245
      break;
246
    case MOTORS_OFF:
247
      usb_puts("calling motors_off\n");
248
      motors_off();
249
      break;
250
    case MOVE:
251
      sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);
252
      usb_puts(buf);
253
      move(args[0], args[1]);
254
      break;
255
    case XBEE_INIT:
256
      xbee_init();
257
      break;
258

    
259
    case XBEE_PUTC:
260
      xbee_putc((char)args[0]);
261
      break;
262

    
263
    case USB_INIT:
264
      usb_init();
265
      break;
266

    
267
    case USB_PUTC:
268
      usb_putc((char)args[0]);
269
      break;
270

    
271
    case PRINTF:
272
      printf("%s", pkt.data);
273
      break;
274
    case KILL_ROBOT:
275
      motors_off();
276
      buzzer_off();
277
      exit(0); //kill the robot
278
      break;
279
      //Time
280
    case DELAY_MS:
281
      delay_ms(int_args[0]);
282
      break;
283

    
284
      //Analog
285
    case ANALOG_INIT:
286
      //TODO: how do we really init the analog? this doesn't work:
287
      //analog_init();
288
      break;
289

    
290
      //BOM
291
    case BOM_ON:
292
      bom_on();
293
      break;
294
    case BOM_OFF:
295
      bom_off();
296
      break;
297

    
298
      //Dio
299
    case DIGITAL_OUTPUT:
300
      digital_output(int_args[0], int_args[1]);
301
      break;
302
    }
303
  } else {
304
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
305
    usb_puts(buf);
306
  }
307
}
308

    
309
static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf) {
310
  memcpy(dest_pkt, pkt_buf, sizeof(ColonetRobotServerPacket));
311
}
312

    
313
/* two_bytes_to_int(char a, char b)
314
 * Returns int of form [high][low]
315
 */
316
static unsigned int two_bytes_to_int(char high, char low) {
317
  return (((unsigned int)high)<<8) + (unsigned int)low;
318
}