Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / lib / colonet_firefly+ / colonet.c @ 11

History | View | Annotate | Download (7 KB)

1
/** @file colonet.c
2
 * @brief Colonet library for colony robots
3

4
 * @author Eugene Marinelli
5
 * @date 10/26/06
6
 * 
7
 * @bug Handler registration not tested
8
 * @bug Request reponding not implemented - only accepts commands.
9
 */
10

    
11
#include <firefly+_lib.h>
12

    
13
#include "colonet.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
void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf);
25
unsigned int two_bytes_to_int(char high, char low);
26

    
27
/* Public functions */
28
int colonet_handle_message(unsigned char robot_id, char* pkt_buf)
29
{
30
  ColonetPacket pkt;
31
  unsigned char* args; //up to 7 char args
32
  unsigned int int_args[3]; //up to 3 int (2-byte) args
33

    
34
  packet_string_to_struct(&pkt, pkt_buf);
35

    
36
  if(pkt.msg_dest != GLOBAL_DEST && pkt.msg_dest != robot_id){ 
37
    //message not intended for us
38
    return 1;
39
  }
40

    
41
  args = pkt.data;
42
  
43
  /* Hack - could be done more 1337ly with casting, but this way we don't have
44
   * to worry about little/big endian stuff */
45
  int_args[0] = two_bytes_to_int(args[0], args[1]);
46
  int_args[1] = two_bytes_to_int(args[2], args[3]);
47
  int_args[2] = two_bytes_to_int(args[4], args[5]);
48

    
49
  if(pkt.msg_type == COLONET_REQUEST){
50
    switch(pkt.msg_code){
51
      //Sharp
52
    case READ_DISTANCE:
53
      break;
54
    case LINEARIZE_DISTANCE:
55
      break;
56
    case LOG_DISTANCE:
57
      break;
58

    
59
      //Serial
60
    case SERIAL_GETCHAR:
61
      break;
62
    case SERIAL_GETCHAR_NB:
63
      break;
64
    case SERIAL1_GETCHAR:
65
      break;
66
    case SERIAL1_GETCHAR_NB:
67
      break;
68

    
69
      //Analog
70
    case ANALOG8:
71
      break;
72
    case ANALOG10:
73
      break;
74
    case WHEEL:
75
      break;
76
    case BATTERY:
77
      break;
78

    
79
      //BOM
80
    case GETMAXBOM:
81
      break;
82

    
83
      //Dio
84
    case DIGITAL_INPUT:
85
      break;
86
    case BUTTON1_READ:
87
      break;
88
    case BUTTON2_READ:
89
      break;
90

    
91
      //Bumper
92
    case DETECT_BUMP:
93
      break;
94
    }
95

    
96
    /* TODO - send back data! */
97

    
98
  }else if(pkt.msg_type == COLONET_COMMAND){
99
    if(pkt.msg_code >= USER_DEFINED_MSG_ID_START && 
100
       pkt.msg_code <= USER_DEFINED_MSG_ID_END){
101
      if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) {
102
        /* Call the user's handler function if it the function's address
103
         * is non-zero (has been set) */
104
        user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler();
105
      }
106

    
107
      return 0;
108
    }
109

    
110
    switch(pkt.msg_code){
111
    default:
112
      printf("%s: Error - message code %d not implemented\n", __FUNCTION__,
113
             pkt.msg_code);
114
      return -1;
115
      break;
116

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

    
198
      /*
199
    case MOVE:
200
      
201
      break;
202
      */
203

    
204
      //SERIAL
205
    case SERIAL_INIT:
206
      serial_init(int_args[0]);
207
      break;
208
    case SERIAL_PUTCHAR:
209
      serial_putchar((char)args[0]);
210
      break;
211
    case SERIAL1_INIT:
212
      serial1_init(int_args[0]);
213
      break;
214
    case SERIAL1_PUTCHAR:
215
      serial1_putchar((char)args[0]);
216
      break;
217
    case PRINTF:
218
      printf("%s", pkt.data);
219
      break;
220
    case KILL_ROBOT:
221
      motors_off();
222
      buzzer_off();
223
      exit(0); //kill the robot
224
      break;
225
      //Time
226
    case DELAY_MS:
227
      delay_ms(int_args[0]);
228
      break;
229

    
230
      //Analog
231
    case ANALOG_INIT:
232
      analog_init();
233
      break;
234

    
235
      //BOM
236
    case BOM_ON:
237
      bom_on();
238
      break;
239
    case BOM_OFF:
240
      bom_off();
241
      break;
242
    case OUTPUT_HIGH:
243
      output_high(int_args[0]);
244
      break;
245
    case OUTPUT_LOW:
246
      output_low(int_args[0]);
247
      break;
248

    
249
      //Dio
250
    case DIGITAL_OUTPUT:
251
      digital_output(int_args[0], int_args[1]);
252
      break;
253
    case BUTTON1_WAIT:
254
      button1_wait();
255
      break;
256
    case BUTTON1_WAIT_LED:
257
      button1_wait_led();
258
      break;
259
    case BUTTON2_WAIT:
260
      button2_wait();
261
      break;
262
    case BUTTON2_WAIT_LED:
263
      button2_wait_led();
264
      break;
265

    
266
      //WL
267
    case WL_INIT:
268
      wl_init();
269
      break;
270
    case PARSE_BUFFER:
271
      parse_buffer();
272
      break;
273
    case WL_SEND:
274
      wl_send();
275
      break;
276
    }
277
  }else{
278
    printf("%s: Error: Invalid colonet message type", __FUNCTION__);
279
    return -1;
280
  }
281

    
282
  return 0;
283
}
284

    
285
/* colonet_add_message
286
 * Adds a user-defined message
287
 */
288
int colonet_add_message(unsigned char msgId, void (*handler)(void))
289
{
290
  if(msgId < USER_DEFINED_MSG_ID_START || msgId > USER_DEFINED_MSG_ID_END){
291
    return -1;
292
  }
293

    
294
  /* Register this function in the array */
295
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
296
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
297

    
298
  return 0;
299
}
300

    
301
/* Private functions */
302
static void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf)
303
{
304
  int i;
305
  
306
  printf("\npacket:");
307
  for(i = 0; i < 16; i++){
308
    printf("%d ", pkt_buf[i]);
309
  }
310

    
311
  printf("\n");
312

    
313
  dest_pkt->prefix[0] = pkt_buf[0];
314
  dest_pkt->prefix[1] = pkt_buf[1];
315
  dest_pkt->token_src = pkt_buf[2];
316
  dest_pkt->token_dest = pkt_buf[3];
317
  dest_pkt->msg_dest = pkt_buf[4];
318
  dest_pkt->msg_type = pkt_buf[5];
319
  dest_pkt->msg_code = pkt_buf[6];
320
  
321
  for(i = 0; i < PACKET_DATA_LEN; i++){
322
    dest_pkt->data[i] = pkt_buf[7+i];
323
  }
324

    
325
  dest_pkt->num_robots = pkt_buf[PACKET_SIZE-2];
326
  dest_pkt->checksum = pkt_buf[PACKET_SIZE-1];
327
}
328

    
329
/* two_bytes_to_int(char a, char b)
330
 * Returns int of form [high][low]
331
 */
332
static unsigned int two_bytes_to_int(char high, char low)
333
{
334
  return (((unsigned int)high)<<8) + (unsigned int)low;
335
}