root / branches / encoders / code / behaviors / spline / slave / slave.c @ 1221
History | View | Annotate | Download (1.57 KB)
1 | 1190 | chihsiuh | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | |||
4 | #define GROUP 1 |
||
5 | #define TYPE 0 |
||
6 | |||
7 | 1217 | chihsiuh | #define LS 0 |
8 | #define LD 1 |
||
9 | #define RS 2 |
||
10 | #define RD 3 |
||
11 | |||
12 | #define H0 0 |
||
13 | #define L0 1 |
||
14 | #define H1 2 |
||
15 | #define L1 3 |
||
16 | |||
17 | 1190 | chihsiuh | void packet_receive (char type, int source, unsigned char* packet, int length); |
18 | |||
19 | PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL}; |
||
20 | |||
21 | 1221 | chihsiuh | char buf[32]; |
22 | int count = 0; |
||
23 | |||
24 | 1217 | chihsiuh | /* receive motor speed from master:
|
25 | * left_speed left_dir right_speed right_dir
|
||
26 | * then sent raw encoder values back to master:
|
||
27 | * high_left low_left high_right low_right
|
||
28 | */
|
||
29 | 1190 | chihsiuh | void packet_receive (char type, int source, unsigned char* packet, int length) { |
30 | 1221 | chihsiuh | //orb_set_color(WHITE);
|
31 | usb_puts("receiving\n");
|
||
32 | sprintf(buf, "%d %d %d %d\n", packet[LS], packet[LD], packet[RS], packet[RD]);
|
||
33 | 1190 | chihsiuh | usb_puts(buf); |
34 | 1217 | chihsiuh | motor1_set(packet[LD],packet[LS]); |
35 | motor2_set(packet[RD],packet[RS]); |
||
36 | 1221 | chihsiuh | //orb_set_color(GREEN);
|
37 | 1190 | chihsiuh | } |
38 | |||
39 | int main (void) { |
||
40 | 1217 | chihsiuh | dragonfly_init(ALL_ON); |
41 | usb_init(); |
||
42 | encoders_init(); |
||
43 | //usb_puts("xbee id ");
|
||
44 | //usb_putc(wl_get_xbee_id());
|
||
45 | 1190 | chihsiuh | wl_init(); |
46 | 1217 | chihsiuh | |
47 | 1190 | chihsiuh | wl_register_packet_group(&packetHandler); |
48 | 1221 | chihsiuh | //orb_set_color(RED);
|
49 | 1217 | chihsiuh | |
50 | while (1) { |
||
51 | wl_do(); |
||
52 | 1221 | chihsiuh | |
53 | int encoder_left = encoder_read(LEFT);
|
||
54 | int encoder_right = encoder_read(RIGHT);
|
||
55 | sprintf(buf, "%d %d\n", encoder_left, encoder_right);
|
||
56 | usb_puts(buf); |
||
57 | unsigned char encoder[4]; |
||
58 | encoder[H0] = (encoder_left >> 8) & 0xFF; |
||
59 | encoder[L0] = (encoder_left) & 0xFF;
|
||
60 | encoder[H1] = (encoder_right >> 8) & 0xFF; |
||
61 | encoder[L1] = (encoder_right) & 0xFF;
|
||
62 | wl_send_global_packet(GROUP, TYPE, encoder, 4, 0); |
||
63 | usb_puts("Sent ");
|
||
64 | usb_puti(count++); |
||
65 | usb_puts("\n");
|
||
66 | |||
67 | delay_ms(200);
|
||
68 | 1217 | chihsiuh | } |
69 | 1190 | chihsiuh | } |