Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / utilities / robot_slave / colonet_dragonfly.c @ 13

History | View | Annotate | Download (8.07 KB)

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

    
11
#include <dragonfly_lib.h>
12
#include <string.h>
13
#include "colonet_dragonfly.h"
14

    
15
typedef struct {
16
  unsigned char msgId; //is this necessary?
17
  void (*handler)(void);
18
} UserHandler;
19

    
20
/* Globals (internal) */
21
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
22

    
23
/* Internal function prototypes */
24
static void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf);
25
static unsigned int two_bytes_to_int(char high, char low);
26
static void fill_packet(ColonetPacket* pkt, unsigned char msg_dest, 
27
                        unsigned char msg_code, unsigned char* data, 
28
                        unsigned char num_robots);
29
static unsigned char compute_checksum(ColonetPacket* pkt);
30
static void send_packet(ColonetPacket* pkt);
31

    
32
static int colonet_robot_id;
33

    
34
void colonet_set_id(int id)
35
{
36
  colonet_robot_id = id;
37
}
38

    
39
/* Public functions */
40
int colonet_handle_message(char* pkt_buf)
41
{
42
  ColonetPacket pkt;
43
  unsigned char* args; //up to 7 char args
44
  unsigned int int_args[3]; //up to 3 int (2-byte) args
45

    
46
  packet_string_to_struct(&pkt, pkt_buf);
47

    
48
  if(pkt.msg_dest != GLOBAL_DEST && pkt.msg_dest != colonet_robot_id){ 
49
    //message not intended for us
50
    return 1;
51
  }
52

    
53
  args = pkt.data;
54
  
55
  int_args[0] = two_bytes_to_int(args[0], args[1]);
56
  int_args[1] = two_bytes_to_int(args[2], args[3]);
57
  int_args[2] = two_bytes_to_int(args[4], args[5]);
58

    
59
  if(pkt.msg_type == COLONET_REQUEST){
60
    printf("got colonet request\n");
61

    
62
    ColonetPacket response_pkt;
63
    char response_data[PACKET_DATA_LEN];
64

    
65
    memset(response_data, 0, PACKET_DATA_LEN);
66
    memset((char*)&response_pkt, 0, PACKET_SIZE);
67
    
68

    
69
    switch(pkt.msg_code){
70
      //Sharp
71
    case READ_DISTANCE:
72
      break;
73
    case LINEARIZE_DISTANCE:
74
      break;
75
    case LOG_DISTANCE:
76
      break;
77

    
78
      //Analog
79
    case CALL_ANALOG8:
80
      break;
81
    case CALL_ANALOG10:
82
      break;
83
    case WHEEL:
84
      break;
85
    case BATTERY:
86
      response_data[0] = battery8();
87
      printf("battery life: %d\n", response_data[0]);
88

    
89
      fill_packet(&response_pkt, args[0], BATTERY, response_data, 
90
                  ((ColonetPacket*)pkt_buf)->num_robots);
91

    
92
      send_packet(&response_pkt);
93

    
94
      printf("got battery request and sent packet\n");
95
      break;
96

    
97
      //BOM
98
    case GETMAXBOM:
99
      break;
100

    
101
      //Dio
102
    case DIGITAL_INPUT:
103
      break;
104
    case BUTTON1_READ:
105
      break;
106
    case BUTTON2_READ:
107
      break;
108

    
109
      //Bumper
110
    case DETECT_BUMP:
111
      break;
112
    }
113
  }else if(pkt.msg_type == COLONET_COMMAND){
114
    if(pkt.msg_code >= USER_DEFINED_MSG_ID_START && 
115
       pkt.msg_code <= USER_DEFINED_MSG_ID_END){
116
      if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) {
117
        /* Call the user's handler function if it the function's address
118
         * is non-zero (has been set) */
119
        user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler();
120
      }
121

    
122
      return 0;
123
    }
124

    
125
    switch(pkt.msg_code){
126
    default:
127
      printf("%s: Error - message code %d not implemented\n", __FUNCTION__,
128
             pkt.msg_code);
129
      return -1;
130
      break;
131

    
132
      //Buzzer
133
    case BUZZER_INIT:
134
      buzzer_init();
135
      break;
136
    case BUZZER_SET_VAL:
137
      buzzer_set_val(args[0]);
138
      break;
139
    case BUZZER_SET_FREQ:
140
      buzzer_set_freq(args[0]);
141
      break;
142
    case BUZZER_CHIRP:
143
      buzzer_chirp(int_args[0], int_args[1]);
144
      break;
145
    case BUZZER_OFF:
146
      buzzer_off();
147
      break;
148
      
149
      //LCD
150
    case LCD_INIT:
151
      lcd_init();
152
      break;
153
    case LCD_CLEAR_SCREEN:
154
      lcd_clear_screen();
155
      break;
156
    case LCD_PUTBYTE:
157
      lcd_putbyte(args[0]);
158
      break;
159
    case LCD_PUTCHAR:
160
      lcd_putchar((char)args[0]);
161
      break;
162
    case LCD_PUTSTR:
163
      lcd_putstr((char*)args);
164
      break;
165
    case LCD_GOTOXY: 
166
      lcd_gotoxy(int_args[0], int_args[1]);
167
      break;
168
    case LCD_PUTINT: 
169
      lcd_putint(int_args[0]);
170
      break;
171
    case ORB_INIT:
172
      orb_init();
173
      break;
174
      //Orb
175
    case ORB_SET:
176
      orb_set(args[0], args[1], args[2]);
177
      break;
178
    case ORB_SET_COLOR:
179
      orb_set_color(int_args[0]);
180
      break;
181
    case ORB_DISABLE:
182
      orb_disable();
183
      break;
184
    case ORB_ENABLE:
185
      orb_enable();
186
      break;
187
    case LED_INIT:
188
      break;
189
    case LED_USER:
190
      break;
191
    case ORB_SEND:
192
      //orb_send();
193
      break;
194
      //Motors
195
    case MOTORS_INIT:
196
      motors_init();
197
      break;
198
    case MOTOR1_SET:
199
      motor1_set(args[0], args[1]);
200
      break;
201
    case MOTOR2_SET:
202
      motor2_set(args[0], args[1]);
203
      break;
204
    case MOTORS_OFF:
205
      motors_off();
206
      break;
207

    
208

    
209
    case MOVE_AVOID:
210
      move_avoid(args[0], args[1], args[2]);
211
      break;
212
      /*
213
    case MOVE:
214
      
215
      break;
216
      */
217

    
218
    case XBEE_INIT:
219
      xbee_init();
220
      break;
221

    
222
    case XBEE_PUTC:
223
      xbee_putc((char)args[0]);
224
      break;
225

    
226
    case USB_INIT:
227
      usb_init();
228
      break;
229

    
230
    case USB_PUTC:
231
      usb_putc((char)args[0]);
232
      break;
233

    
234
    case PRINTF:
235
      printf("%s", pkt.data);
236
      break;
237
    case KILL_ROBOT:
238
      motors_off();
239
      buzzer_off();
240
      exit(0); //kill the robot
241
      break;
242
      //Time
243
    case DELAY_MS:
244
      delay_ms(int_args[0]);
245
      break;
246

    
247
      //Analog
248
    case ANALOG_INIT:
249
      analog_init();
250
      break;
251

    
252
      //BOM
253
    case BOM_ON:
254
      bom_on();
255
      break;
256
    case BOM_OFF:
257
      bom_off();
258
      break;
259
    case OUTPUT_HIGH:
260
      output_high(int_args[0]);
261
      break;
262
    case OUTPUT_LOW:
263
      output_low(int_args[0]);
264
      break;
265

    
266
      //Dio
267
    case DIGITAL_OUTPUT:
268
      digital_output(int_args[0], int_args[1]);
269
      break;
270
    }
271
  }else{
272
    printf("%s: Error: Invalid colonet message type", __FUNCTION__);
273
    return -1;
274
  }
275

    
276
  return 0;
277
}
278

    
279
/* colonet_add_message
280
 * Adds a user-defined message
281
 */
282
int colonet_add_message(unsigned char msgId, void (*handler)(void))
283
{
284
  if(msgId < USER_DEFINED_MSG_ID_START || msgId > USER_DEFINED_MSG_ID_END){
285
    return -1;
286
  }
287

    
288
  /* Register this function in the array */
289
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
290
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
291

    
292
  return 0;
293
}
294

    
295
/* Private functions */
296
static void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf)
297
{
298
  int i;
299
  
300
  printf("\npacket:");
301
  for(i = 0; i < 16; i++){
302
    printf("%d ", pkt_buf[i]);
303
  }
304

    
305
  printf("\n");
306

    
307
  dest_pkt->prefix[0] = pkt_buf[0];
308
  dest_pkt->prefix[1] = pkt_buf[1];
309
  dest_pkt->token_src = pkt_buf[2];
310
  dest_pkt->token_dest = pkt_buf[3];
311
  dest_pkt->msg_dest = pkt_buf[4];
312
  dest_pkt->msg_type = pkt_buf[5];
313
  dest_pkt->msg_code = pkt_buf[6];
314
  
315
  for(i = 0; i < PACKET_DATA_LEN; i++){
316
    dest_pkt->data[i] = pkt_buf[7+i];
317
  }
318

    
319
  dest_pkt->num_robots = pkt_buf[PACKET_SIZE-2];
320
  dest_pkt->checksum = pkt_buf[PACKET_SIZE-1];
321
}
322

    
323
static void fill_packet(ColonetPacket* pkt, unsigned char msg_dest, 
324
                        unsigned char msg_code, unsigned char* data, 
325
                        unsigned char num_robots)
326
{
327
  int i;
328

    
329
  pkt->prefix[0] = 'R';
330
  pkt->prefix[1] = 'C';
331
  pkt->token_src = 0;
332
  pkt->token_dest = 0;
333
  pkt->msg_dest = msg_dest;
334
  pkt->msg_code = msg_code;
335
  pkt->msg_type = COLONET_RESPONSE;
336
  
337
  for (i = 0; i < PACKET_DATA_LEN; i++)
338
  {
339
    pkt->data[i] = data[i];
340
  }
341

    
342
  pkt->num_robots = num_robots;
343
  
344
  pkt->checksum = compute_checksum(pkt);
345
}
346

    
347
static unsigned char compute_checksum(ColonetPacket* pkt)
348
{
349
  char checksum = 0;
350

    
351
  checksum -= pkt->token_src;
352
  checksum -= pkt->token_dest;
353
  checksum -= pkt->msg_dest;
354
  checksum -= pkt->msg_type;
355
  checksum -= pkt->msg_code;
356

    
357
  for (int i = 0; i < PACKET_DATA_LEN; i++)
358
    checksum -= pkt->data[i];
359

    
360
  checksum -= pkt->num_robots;
361

    
362
  return checksum;
363
}
364

    
365
static void send_packet(ColonetPacket* pkt)
366
{
367
  int i;
368

    
369
  for (i = 0; i < PACKET_SIZE; i++)
370
    xbee_putc(*((unsigned char*)pkt+i));
371
}
372

    
373
/* two_bytes_to_int(char a, char b)
374
 * Returns int of form [high][low]
375
 */
376
static unsigned int two_bytes_to_int(char high, char low)
377
{
378
  return (((unsigned int)high)<<8) + (unsigned int)low;
379
}