Project

General

Profile

Statistics
| Revision:

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

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
/* Private functions */
48

    
49
/** @brief Handles colonet packets.  Should be called by parse_buffer
50
 * when it is determined that a colonet message has been received.
51
 *
52
 * @param robot_id The robot id
53
 * @param pkt_buf The packet buffer (e.g. wl_buf)
54
 *
55
 * @return -1 on error (invalid msgId), 0 on success
56
 */
57
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length) {
58
  ColonetRobotServerPacket pkt;
59
  unsigned char* args; //up to 7 char args
60
  unsigned int int_args[3]; //up to 3 int (2-byte) args
61

    
62
  usb_puts("Packet received.\n");
63
  char buf[40];
64
  sprintf(buf, "length=%d\n", length);
65

    
66
  int i;
67
  for (i = 0; i < length; i++) {
68
    sprintf(buf, "%d: %d ", i, packet[i]);
69
    usb_puts(buf);
70
  }
71
  usb_puts("\n");
72

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

    
75
  ///assert(length == sizeof(ColonetRobotServerPacket));
76
  packet_string_to_struct(&pkt, packet);
77

    
78
  char buf2[40];
79
  sprintf(buf2, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
80
  usb_puts(buf2);
81

    
82
  args = pkt.data;
83

    
84
  int_args[0] = two_bytes_to_int(args[0], args[1]);
85
  int_args[1] = two_bytes_to_int(args[2], args[3]);
86
  int_args[2] = two_bytes_to_int(args[4], args[5]);
87

    
88
  if (type == COLONET_REQUEST) {
89
    usb_puts("type is colonet request\n");
90

    
91
    /* TODO - send back data! */
92

    
93
    switch (pkt.msg_code) {
94
      //Sharp
95
    case READ_DISTANCE:
96
      break;
97
    case LINEARIZE_DISTANCE:
98
      break;
99
    case LOG_DISTANCE:
100
      break;
101

    
102

    
103
      //Analog
104
    case CALL_ANALOG8:
105
      break;
106
    case CALL_ANALOG10:
107
      break;
108
    case WHEEL:
109
      break;
110
    case BATTERY:
111
            wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, BATTERY,
112
                "brouhaha", 8, 0xA, 0);
113
      break;
114

    
115
      //BOM
116
    case GETMAXBOM:
117
      break;
118

    
119
      //Dio
120
    case DIGITAL_INPUT:
121
      break;
122
    case BUTTON1_READ:
123
      break;
124
    case BUTTON2_READ:
125
      break;
126

    
127
      //Bumper
128
    case DETECT_BUMP:
129
      break;
130
    }
131
  } else if (type == COLONET_COMMAND) {
132
    usb_puts("type is colonet command...\n");
133

    
134
/* TODO uncomment this stuff */
135
/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
136
/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
137
/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
138
/*         /\* Call the user's handler function if it the function's address */
139
/*          * is non-zero (has been set) *\/ */
140
/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
141
/*       } */
142
/*     } */
143

    
144
    switch (pkt.msg_code) {
145
/*     default: */
146
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
147
/*              pkt.msg_code); */
148
/*       break; */
149

    
150
      //Buzzer
151
    case BUZZER_INIT:
152
      buzzer_init();
153
      break;
154
    case BUZZER_SET_VAL:
155
      buzzer_set_val(args[0]);
156
      break;
157
    case BUZZER_SET_FREQ:
158
      buzzer_set_freq(args[0]);
159
      break;
160
    case BUZZER_CHIRP:
161
      buzzer_chirp(int_args[0], int_args[1]);
162
      break;
163
    case BUZZER_OFF:
164
      buzzer_off();
165
      break;
166

    
167
    // Battery
168
    case BATTERY:
169
      usb_puts("Got battery request.\n");
170

    
171
/*
172
      int xbeeDongleID = 0xA;  // too bad this is hard-coded.
173
      char data[20];
174
      data[0] = 0;  // client ID specified in bytes 0-3
175
      data[1] = 0;
176
      data[2] = 0;
177
      data[3] = 0;
178
      data[4] = 0;  // ???
179
      data[5] = RESPONSE_TO_CLIENT_REQUEST;
180
      data[6] = ' ';
181
      data[7] = BATTERY;
182
      data[8] = ' ';
183
      data[9] = (char) wl_get_xbee_id();  // robot number (self)
184
      data[10] = ' ';
185
      data[11] = (char) battery8();  // battery reading
186
      data[12] = '\0';
187

188
      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, COLONET_RESPONSE, data, 13, xbeeDongleID, 0);
189
      usb_puts("lol \n");
190
*/
191
      break;
192

    
193
    case ORB_INIT:
194
      orb_init();
195
      break;
196
      //Orb
197
    case ORB_SET:
198
      orb_set(args[0], args[1], args[2]);
199
      break;
200
    case ORB_SET_COLOR:
201
      orb_set_color(int_args[0]);
202
      break;
203
    case ORB_DISABLE:
204
      orb_disable();
205
      break;
206
    case ORB_ENABLE:
207
      orb_enable();
208
      break;
209
    case LED_INIT:
210
      break;
211
    case LED_USER:
212
      break;
213
    case ORB_SEND:
214
      //orb_send();
215
      break;
216
      //Motors
217
    case MOTORS_INIT:
218
      usb_puts("calling motors_init\n");
219
      motors_init();
220
      break;
221
    case MOTOR1_SET:
222
             sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
223
      usb_puts(buf);
224
      motor1_set(args[0], args[1]);
225
      break;
226
    case MOTOR2_SET:
227
      sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
228
      usb_puts(buf);
229
      motor2_set(args[0], args[1]);
230
      break;
231
    case MOTORS_OFF:
232
      usb_puts("calling motors_off\n");
233
      motors_off();
234
      break;
235
    case MOVE:
236
      sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);
237
      usb_puts(buf);
238
      move(args[0], args[1]);
239
      break;
240
    case XBEE_INIT:
241
      xbee_init();
242
      break;
243

    
244
    case XBEE_PUTC:
245
      xbee_putc((char)args[0]);
246
      break;
247

    
248
    case USB_INIT:
249
      usb_init();
250
      break;
251

    
252
    case USB_PUTC:
253
      usb_putc((char)args[0]);
254
      break;
255

    
256
    case PRINTF:
257
      printf("%s", pkt.data);
258
      break;
259
    case KILL_ROBOT:
260
      motors_off();
261
      buzzer_off();
262
      exit(0); //kill the robot
263
      break;
264
      //Time
265
    case DELAY_MS:
266
      delay_ms(int_args[0]);
267
      break;
268

    
269
      //Analog
270
    case ANALOG_INIT:
271
      //TODO: how do we really init the analog? this doesn't work:
272
      //analog_init();
273
      break;
274

    
275
      //BOM
276
    case BOM_ON:
277
      bom_on();
278
      break;
279
    case BOM_OFF:
280
      bom_off();
281
      break;
282

    
283
      //Dio
284
    case DIGITAL_OUTPUT:
285
      digital_output(int_args[0], int_args[1]);
286
      break;
287
    }
288
  } else {
289
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
290
    usb_puts(buf);
291
  }
292
}
293

    
294
/* colonet_add_message
295
 * Adds a user-defined message
296
 */
297
int colonet_add_message(unsigned char msgId, void (*handler)(void)) {
298
  if(msgId < USER_DEFINED_MSG_ID_START  /* || msgId > USER_DEFINED_MSG_ID_END */){
299
    return -1;
300
  }
301

    
302
  /* Register this function in the array */
303
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
304
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
305

    
306
  return 0;
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
}