Project

General

Profile

Revision 1217

Added by Rich Hong almost 12 years ago

spline slave

View differences:

slave.c
4 4
#define GROUP 1
5 5
#define TYPE 0
6 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

  
7 17
void packet_receive (char type, int source, unsigned char* packet, int length);
8 18

  
9 19
PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};
10 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
 */
11 26
void packet_receive (char type, int source, unsigned char* packet, int length) {
12
	int lv, ls, rv, rs;
27
	orb_set_color(WHITE);
13 28
	char buf[32];
14 29
	sprintf(buf, "%d %d %d %d\n", packet[0], packet[1], packet[2], packet[3]);
15 30
	usb_puts(buf);
16
	lv = packet[0];
17
	ls = packet[1];
18
	rv = packet[2];
19
	rs = packet[3];
20
	motor1_set(ls,lv);
21
	motor2_set(rs,rv);
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);
22 45
}
23 46

  
24 47
int main (void) {
25
  dragonfly_init(SERIAL | USB | COMM | ORB | ANALOG | MOTORS | BUZZER);
26
	ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
27
	usb_puts("xbee id ");
28
	usb_putc(wl_get_xbee_id());
48
	dragonfly_init(ALL_ON);
49
	usb_init();
50
	encoders_init();
51
	//usb_puts("xbee id ");
52
	//usb_putc(wl_get_xbee_id());
29 53
	wl_init();
30
	
54

  
31 55
	wl_register_packet_group(&packetHandler);
32
	orb_set_color(PURPLE);
33
  
34
  while (1) {
35
	wl_do();
36
  }
37
  
56
	orb_set_color(RED);
57

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

  

Also available in: Unified diff