Project

General

Profile

Statistics
| Revision:

root / branches / encoders / code / behaviors / spline / slave / slave.c @ 1217

History | View | Annotate | Download (1.44 KB)

1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3

    
4
#define GROUP 1
5
#define TYPE 0
6

    
7
#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
void packet_receive (char type, int source, unsigned char* packet, int length);
18

    
19
PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};
20

    
21
/* receive motor speed from master: 
22
 * left_speed left_dir right_speed right_dir
23
 * then sent raw encoder values back to master: 
24
 * high_left low_left high_right low_right 
25
 */
26
void packet_receive (char type, int source, unsigned char* packet, int length) {
27
        orb_set_color(WHITE);
28
        char buf[32];
29
        sprintf(buf, "%d %d %d %d\n", packet[0], packet[1], packet[2], packet[3]);
30
        usb_puts(buf);
31
        motor1_set(packet[LD],packet[LS]);
32
        motor2_set(packet[RD],packet[RS]);
33

    
34
        int encoder_left = encoder_read(LEFT);
35
        int encoder_right = encoder_read(RIGHT);
36
        sprintf(buf, "%d %d\n", encoder_left, encoder_right);
37
        usb_puts(buf);
38
        unsigned char encoder[4];
39
        encoder[H0] = (encoder_left >> 8) & 0xFF;
40
        encoder[L0] = (encoder_left) & 0xFF;
41
        encoder[H1] = (encoder_right >> 8) & 0xFF;
42
        encoder[L1] = (encoder_right) & 0xFF;
43
        wl_send_global_packet(GROUP, TYPE, encoder, 4, 0);
44
        orb_set_color(GREEN);
45
}
46

    
47
int main (void) {
48
        dragonfly_init(ALL_ON);
49
        usb_init();
50
        encoders_init();
51
        //usb_puts("xbee id ");
52
        //usb_putc(wl_get_xbee_id());
53
        wl_init();
54

    
55
        wl_register_packet_group(&packetHandler);
56
        orb_set_color(RED);
57

    
58
        while (1) {
59
                wl_do();
60
        }
61
}
62