Project

General

Profile

Revision 1345

Added by Rich Hong almost 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

slave.c
18 18
volatile int left_ref;
19 19
volatile int right_ref;
20 20

  
21
volatile int left_old;
22
volatile int right_old;
21
volatile int left_err_old;
22
volatile int right_err_old;
23 23

  
24
volatile int left_vel_old = 0;
25
volatile int right_vel_old = 0;
26

  
24 27
volatile int left_i = 0;
25 28
volatile int right_i = 0;
26 29

  
27 30
volatile int left_u;
28 31
volatile int right_u;
29 32

  
30
volatile int left_vel = 0;
31
volatile int right_vel = 0;
33
volatile int left_vel;
34
volatile int right_vel;
32 35

  
33 36
#define PL	1
34 37
#define DL	0.1
35 38
#define IL	0.1
36 39

  
40
/*
37 41
#define PR	1.5
38 42
#define DR	0.2
39 43
#define IR	0.1
44
*/
45
#define PR	1
46
#define DR	0.1
47
#define IR	0.1
40 48

  
41 49

  
42
#define INTERVAL	50
50
#define INTERVAL	20
43 51

  
44 52
/* receive motor speed from master: 
45 53
 * left_speed left_dir right_speed right_dir
......
50 58
	if (type != MOTOR) return;
51 59
	
52 60
	left_ref = packet[0];
61
	if (left_ref > 127)
62
		left_ref -= 256;
53 63
	right_ref = packet[1];
64
	if (right_ref > 127)
65
		right_ref -= 256;
54 66

  
55
	sprintf(buf, "received: %d %d\n", left_ref, right_ref);
56
	usb_puts(buf);
67
//	sprintf(buf, "received: %d %d\n", left_ref, right_ref);
68
//	usb_puts(buf);
57 69
	//orb_set_color(GREEN);
58 70
}
59 71

  
......
61 73
	int left_err = left_ref - left_vel;
62 74
	int right_err = right_ref - right_vel;
63 75
	
64
	left_err = left_vel == 2048 ? left_old : left_err;
65
	right_err = right_vel == 2048 ? right_old : right_err;
76
	if (!left_ref)
77
		left_err = 0;
78
	if (!right_ref)
79
		right_err = 0;
80
	
81
//	left_err = left_vel == 2048 ? left_err_old : left_err;
82
//	right_err = right_vel == 2048 ? right_err_old : right_err;
66 83

  
67 84
	left_i += left_err;
68
	left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_old) + IL*(float)left_i);
69
	left_old = left_err;
85
	left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_err_old) + IL*(float)left_i);
86
	left_err_old = left_err;
70 87
	
71 88
	right_i += right_err;
72
	right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_old) + IR*(float)right_i);
73
	right_old = right_err;
89
	right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_err_old) + IR*(float)right_i);
90
	right_err_old = right_err;
74 91

  
92
//	sprintf(buf, "u: %d %d\n", left_u, right_u);
93
//	usb_puts(buf);
94
	
75 95
	if (left_u < 0) {
76 96
		left_dir = 0;
77 97
		left_u = -left_u;
......
91 111

  
92 112
	left_u = left_u > 210 ? 210 : left_u;
93 113
	right_u = right_u > 210 ? 210 : right_u;
114

  
115
	if (!left_ref)
116
		left_u = 0;
117
	if (!right_ref)
118
		right_u = 0;
94 119
}
95 120

  
96 121
int main (void) {
......
101 126
	//usb_puts("xbee id ");
102 127
	//usb_putc(wl_get_xbee_id());
103 128
	wl_init();
104
    left_ref = 20;
105
	right_ref = 20;
129

  
106 130
	wl_register_packet_group(&packetHandler);
107 131
	//orb_set_color(RED);
108 132
	//motor1_set(FORWARD,230);
109 133
	//motor2_set(FORWARD,230);
110 134
	
111 135
	while (1) {
112
		usb_puts("We Win\n");
113
	    //left_vel = encoder_get_v(0);
114
		//right_vel = encoder_get_v(1);
115
		 delay_ms(1000);
116
		//if (!(count % INTERVAL)) {
117
		//	wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
136
		
137
		left_vel = encoder_get_v(0);
138
		right_vel = encoder_get_v(1);
139
		// err handling
140
		if (left_vel == 2048)
141
			left_vel = left_vel_old;
142
		else 
143
			left_vel_old = left_vel;
144
		
145
		if (right_vel == 2048)
146
			right_vel = right_vel_old;
147
		else 
148
			right_vel_old = right_vel;
149
		
150
		
151
		if (!(count % INTERVAL)) {
152
			wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
118 153
			encoder[0] = left_vel;
119 154
			encoder[1] = right_vel;
120
		
121
		   // usb_puti(left_vel);
122
		  //  sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
123
		   // usb_puts(buf);
124
		   // usb_puti(encoder[0]);
125
	  //  }
126
		
155
			//sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
156
			//usb_puts(buf);
157
		}	
127 158

  
128
		//ctrl();
159
		ctrl();
129 160
	
130
	  //  if (!(count % INTERVAL))
131
	  //  	wl_do();
161
		if (!(count % INTERVAL))
162
			wl_do();
132 163
		
133 164
//		sprintf(buf, "motor (%d): %d %d %d %d\n", count, left_dir, left_ref, right_dir, right_ref);
134 165
//		usb_puts(buf);
135 166

  
136
	    //motor1_set(left_dir, left_u);
137
		//motor2_set(right_dir, right_u);
167
		motor1_set(left_dir, left_u);
168
		motor2_set(right_dir, right_u);
138 169
		
139 170
		count++;
140
	    delay_ms(1000);
171
//		delay_ms(100);
141 172
	}
142 173
}
174

  

Also available in: Unified diff