Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / robot / colonet_dragonfly / colonet_dragonfly.c @ 521

History | View | Annotate | Download (8.78 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 <battery.h>
14
#include <string.h>
15
#include <wireless.h>
16

    
17
#include <colonet_dragonfly.h>
18

    
19
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD 15
20

    
21
typedef struct {
22
  unsigned char msgId; //is this necessary?
23
  void (*handler)(void);
24
} UserHandler;
25

    
26
/* Globals (internal) */
27
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
28

    
29
static int robot_x, robot_y;
30
static int updated_robot_pos_ready;
31

    
32
/* Internal function prototypes */
33
static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf);
34
static unsigned int two_bytes_to_int(char high, char low);
35
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length);
36
static void move_to_position_routine(int x, int y);
37

    
38
static PacketGroupHandler colonet_pgh;
39

    
40
/* Public functions */
41
int colonet_init() {
42
  colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
43
  colonet_pgh.timeout_handler = NULL;
44
  colonet_pgh.handle_response = NULL;
45
  colonet_pgh.handle_receive = colonet_handle_receive;
46
  colonet_pgh.unregister = NULL;
47

    
48
  // TODO this should return an error if wl_init has not been called yet.
49
  wl_register_packet_group(&colonet_pgh);
50

    
51
  robot_x = 0;
52
  robot_y = 0;
53
  updated_robot_pos_ready = 1;
54

    
55
  return 0;
56
}
57

    
58
/* colonet_add_message
59
 * Adds a user-defined message
60
 */
61
int colonet_add_message(unsigned char msgId, void (*handler)(void)) {
62
  if(msgId < USER_DEFINED_MSG_ID_START  /* || msgId > USER_DEFINED_MSG_ID_END */){
63
    return -1;
64
  }
65

    
66
  /* Register this function in the array */
67
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
68
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
69

    
70
  return 0;
71
}
72

    
73
void get_absolute_position(int* x, int* y) {
74
  *x = robot_x;
75
  *y = robot_y;
76
}
77

    
78
void request_abs_position() {
79
  //usb_puts("requesting_abs_position\n");
80

    
81
  ColonetRobotServerPacket pkt;
82
  pkt.client_id = -1;
83
  pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;
84
  wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), 0);
85
}
86

    
87
static int distsquared(int x1, int y1,int x2, int y2) {
88
  return (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1);
89
}
90

    
91
/* cubic approximation of arctan. */
92
static void arctan(float t) {
93
  float t3 = t*t*t;
94
  return -0.039 * t3 + 0.74 * t + 0.00011;
95
}
96

    
97
static void move_to_position_routine(int target_x, int target_y) {
98
  int last_x, last_y;
99

    
100
  do {
101
    while (!updated_robot_pos_ready) { // ghetto condition variable
102
      wl_do();
103
    }
104

    
105
    int x_error = target_x - robot_x
106
    int y_error = target_y - robot_y;
107

    
108
    int x_diff = robot_x - last_x;
109
    int y_diff = robot_y - last_y;
110

    
111
    //float dir = arctan(y/x);
112

    
113

    
114
    int sum_x = 
115
    
116

    
117
    last_x = robot_x;
118
    last_y = robot_y;
119

    
120
    motor1_set(1, 200);
121
    motor2_set(1, 200);
122

    
123
  while (distsquared(robot_x, robot_y, target_x, target_y) >
124
    TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD);
125
}
126

    
127
/* Private functions */
128

    
129
/** @brief Handles colonet packets.  Should be called by parse_buffer
130
 * when it is determined that a colonet message has been received.
131
 *
132
 * @param robot_id The robot id
133
 * @param pkt_buf The packet buffer (e.g. wl_buf)
134
 *
135
 * @return -1 on error (invalid msgId), 0 on success
136
 */
137
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
138
  ColonetRobotServerPacket pkt;
139
  unsigned char* args; //up to 7 char args
140
  unsigned int int_args[3]; //up to 3 int (2-byte) args
141

    
142
  usb_puts("Packet received.\n");
143
  char buf[40];
144
  sprintf(buf, "length=%d\n", length);
145

    
146
  int i;
147
  for (i = 0; i < length; i++) {
148
    sprintf(buf, "%d: %d ", i, packet[i]);
149
    usb_puts(buf);
150
  }
151
  usb_puts("\n");
152

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

    
155
  ///assert(length == sizeof(ColonetRobotServerPacket));
156
  packet_string_to_struct(&pkt, packet);
157

    
158
  char buf2[40];
159
  sprintf(buf2, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
160
  usb_puts(buf2);
161

    
162
  args = pkt.data;
163

    
164
  int_args[0] = two_bytes_to_int(args[0], args[1]);
165
  int_args[1] = two_bytes_to_int(args[2], args[3]);
166
  int_args[2] = two_bytes_to_int(args[4], args[5]);
167

    
168
  if (type == COLONET_REQUEST) {
169
    usb_puts("type is colonet request\n");
170

    
171
    /* TODO - send back data! */
172

    
173
    switch (pkt.msg_code) {
174
      //Sharp
175
    case READ_DISTANCE:
176
      break;
177
    case LINEARIZE_DISTANCE:
178
      break;
179
    case LOG_DISTANCE:
180
      break;
181

    
182
      //Analog
183
    case CALL_ANALOG8:
184
      break;
185
    case CALL_ANALOG10:
186
      break;
187
    case WHEEL:
188
      break;
189

    
190
    case BATTERY:
191
      sprintf((char*)pkt.data, "%d", (int)battery8());
192
      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), wl_source, 0);
193
      break;
194

    
195
      //BOM
196
    case GETMAXBOM:
197
      break;
198

    
199
      //Dio
200
    case DIGITAL_INPUT:
201
      break;
202
    case BUTTON1_READ:
203
      break;
204
    case BUTTON2_READ:
205
      break;
206

    
207
      //Bumper
208
    case DETECT_BUMP:
209
      break;
210
    }
211
  } else if (type == COLONET_COMMAND) {
212
    usb_puts("colonet command...\n");
213

    
214
/* TODO uncomment this stuff */
215
/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
216
/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
217
/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
218
/*         /\* Call the user's handler function if it the function's address */
219
/*          * is non-zero (has been set) *\/ */
220
/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
221
/*       } */
222
/*     } */
223

    
224
    switch (pkt.msg_code) {
225
/*     default: */
226
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
227
/*              pkt.msg_code); */
228
/*       break; */
229

    
230
    case SERVER_REPORT_POSITION_TO_ROBOT:
231
      robot_x = int_args[0];
232
      robot_y = int_args[1];
233

    
234
      updated_robot_pos_ready;
235

    
236
      sprintf(buf, "pos is: %d %d\n", robot_x, robot_y);
237
      usb_puts(buf);
238
      break;
239

    
240
    case MOVE_TO_ABSOLUTE_POSITION:
241
      move_to_position_routine(int_args[0], int_args[1]);
242
      break;
243

    
244
      //Buzzer
245
    case BUZZER_INIT:
246
      buzzer_init();
247
      break;
248
    case BUZZER_SET_VAL:
249
      buzzer_set_val(args[0]);
250
      break;
251
    case BUZZER_SET_FREQ:
252
      buzzer_set_freq(args[0]);
253
      break;
254
    case BUZZER_CHIRP:
255
      buzzer_chirp(int_args[0], int_args[1]);
256
      break;
257
    case BUZZER_OFF:
258
      buzzer_off();
259
      break;
260
    case ORB_INIT:
261
      orb_init();
262
      break;
263
    //Orb
264
    case ORB_SET:
265
      orb_set(args[0], args[1], args[2]);
266
      break;
267
    case ORB_SET_COLOR:
268
      orb_set_color(int_args[0]);
269
      break;
270
    case ORB_DISABLE:
271
      orb_disable();
272
      break;
273
    case ORB_ENABLE:
274
      orb_enable();
275
      break;
276
    case LED_INIT:
277
      break;
278
    case LED_USER:
279
      break;
280
    case ORB_SEND:
281
      //orb_send();
282
      break;
283
      //Motors
284
    case MOTORS_INIT:
285
      usb_puts("calling motors_init\n");
286
      motors_init();
287
      break;
288
    case MOTOR1_SET:
289
      sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
290
      usb_puts(buf);
291
      motor1_set(args[0], args[1]);
292
      break;
293
    case MOTOR2_SET:
294
      sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
295
      usb_puts(buf);
296
      motor2_set(args[0], args[1]);
297
      break;
298
    case MOTORS_OFF:
299
      usb_puts("calling motors_off\n");
300
      motors_off();
301
      break;
302
    case MOVE:
303
      sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);
304
      usb_puts(buf);
305
      move(args[0], args[1]);
306
      break;
307
    case XBEE_INIT:
308
      xbee_init();
309
      break;
310

    
311
    case XBEE_PUTC:
312
      xbee_putc((char)args[0]);
313
      break;
314

    
315
    case USB_INIT:
316
      usb_init();
317
      break;
318

    
319
    case USB_PUTC:
320
      usb_putc((char)args[0]);
321
      break;
322

    
323
    case PRINTF:
324
      printf("%s", pkt.data);
325
      break;
326
    case KILL_ROBOT:
327
      motors_off();
328
      buzzer_off();
329
      exit(0); //kill the robot
330
      break;
331
      //Time
332
    case DELAY_MS:
333
      delay_ms(int_args[0]);
334
      break;
335

    
336
      //Analog
337
    case ANALOG_INIT:
338
      //TODO: how do we really init the analog? this doesn't work:
339
      //analog_init();
340
      break;
341

    
342
      //BOM
343
    case BOM_ON:
344
      bom_on();
345
      break;
346
    case BOM_OFF:
347
      bom_off();
348
      break;
349

    
350
    //Dio
351
    case DIGITAL_OUTPUT:
352
      digital_output(int_args[0], int_args[1]);
353
      break;
354
    }
355
  } else {
356
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
357
    usb_puts(buf);
358
  }
359
}
360

    
361
static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf) {
362
  memcpy(dest_pkt, pkt_buf, sizeof(ColonetRobotServerPacket));
363
}
364

    
365
/* two_bytes_to_int(char a, char b)
366
 * Returns int of form [high][low]
367
 */
368
static unsigned int two_bytes_to_int(char high, char low) {
369
  return (((unsigned int)high)<<8) + (unsigned int)low;
370
}