Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (1.62 KB)

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

    
4
#define GROUP 1
5
#define MOTOR 0
6
#define ENCODER 1
7

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

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

    
22
char buf[32];
23
int count = 0;
24

    
25
/* receive motor speed from master: 
26
 * left_speed left_dir right_speed right_dir
27
 * then sent raw encoder values back to master: 
28
 * high_left low_left high_right low_right 
29
 */
30
void packet_receive (char type, int source, unsigned char* packet, int length) {
31
        if (type != MOTOR) return;
32
        //orb_set_color(WHITE);
33
        usb_puts("receiving\n");
34
        sprintf(buf, "%d %d %d %d\n", packet[LS], packet[LD], packet[RS], packet[RD]);
35
        usb_puts(buf);
36
        motor1_set(packet[LD],packet[LS]);
37
        motor2_set(packet[RD],packet[RS]);
38
        //orb_set_color(GREEN);
39
}
40

    
41
int main (void) {
42
        dragonfly_init(ALL_ON);
43
        usb_init();
44
        encoders_init();
45
        //usb_puts("xbee id ");
46
        //usb_putc(wl_get_xbee_id());
47
        wl_init();
48

    
49
        wl_register_packet_group(&packetHandler);
50
        //orb_set_color(RED);
51

    
52
        while (1) {
53
                wl_do();
54

    
55
                int encoder_left = encoder_read(LEFT);
56
                int encoder_right = encoder_read(RIGHT);
57
                sprintf(buf, "%d %d\n", encoder_left, encoder_right);
58
                usb_puts(buf);
59
                unsigned char encoder[4];
60
                encoder[H0] = (encoder_left >> 8) & 0xFF;
61
                encoder[L0] = (encoder_left) & 0xFF;
62
                encoder[H1] = (encoder_right >> 8) & 0xFF;
63
                encoder[L1] = (encoder_right) & 0xFF;
64
                wl_send_global_packet(GROUP, ENCODER, encoder, 4, 0);
65
                usb_puts("Sent ");
66
                usb_puti(count++);
67
                usb_puts("\n");
68

    
69
                delay_ms(10);
70
        }
71
}
72