Project

General

Profile

Revision 1221

Added by Rich Hong almost 12 years ago

spline slave

View differences:

branches/encoders/code/behaviors/spline/slave/slave.c
18 18

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

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

  
21 24
/* receive motor speed from master: 
22 25
 * left_speed left_dir right_speed right_dir
23 26
 * then sent raw encoder values back to master: 
24 27
 * high_left low_left high_right low_right 
25 28
 */
26 29
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
	//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]);
30 33
	usb_puts(buf);
31 34
	motor1_set(packet[LD],packet[LS]);
32 35
	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);
36
	//orb_set_color(GREEN);
45 37
}
46 38

  
47 39
int main (void) {
......
53 45
	wl_init();
54 46

  
55 47
	wl_register_packet_group(&packetHandler);
56
	orb_set_color(RED);
48
	//orb_set_color(RED);
57 49

  
58 50
	while (1) {
59 51
		wl_do();
52

  
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);
60 68
	}
61 69
}
62 70

  

Also available in: Unified diff