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