Project

General

Profile

Revision 1240

Added by Rich Hong almost 12 years ago

sending velocity instead of raw encoder values

View differences:

slave.c
21 21

  
22 22
char buf[32];
23 23
int count = 0;
24
int left_dir = FORWARD;
25
int right_dir = FORWARD;
26
int left_speed = 0;
27
int right_speed = 0;
24 28

  
25 29
/* receive motor speed from master: 
26 30
 * left_speed left_dir right_speed right_dir
......
30 34
void packet_receive (char type, int source, unsigned char* packet, int length) {
31 35
	if (type != MOTOR) return;
32 36
	//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]);
37
	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]);
35 42
	usb_puts(buf);
36
	motor1_set(packet[LD],packet[LS]);
37
	motor2_set(packet[RD],packet[RS]);
43
	//motor1_set(packet[LD],packet[LS]);
44
	//motor2_set(packet[RD],packet[RS]);
38 45
	//orb_set_color(GREEN);
39 46
}
40 47

  
......
48 55

  
49 56
	wl_register_packet_group(&packetHandler);
50 57
	//orb_set_color(RED);
58
	//motor1_set(FORWARD,230);
59
	//motor2_set(FORWARD,230);
51 60

  
52 61
	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);
62
		int encoder_left = encoder_get_v(LEFT);
63
		int encoder_right = encoder_get_v(RIGHT);
59 64
		unsigned char encoder[4];
60 65
		encoder[H0] = (encoder_left >> 8) & 0xFF;
61 66
		encoder[L0] = (encoder_left) & 0xFF;
62 67
		encoder[H1] = (encoder_right >> 8) & 0xFF;
63 68
		encoder[L1] = (encoder_right) & 0xFF;
64 69
		wl_send_global_packet(GROUP, ENCODER, encoder, 4, 0);
65
		usb_puts("Sent ");
66
		usb_puti(count++);
67
		usb_puts("\n");
70
		sprintf(buf, "Sent (%d): %d %d\n", count++, encoder_left, encoder_right);
71
		usb_puts(buf);
68 72

  
69
		delay_ms(10);
73
		wl_do();
74
		motor1_set(left_dir, left_speed);
75
		motor2_set(right_dir, right_speed);
76
		delay_ms(100);
70 77
	}
71 78
}
72 79

  

Also available in: Unified diff