Revision 1503
Progress on Push-Pull. Added a divide-by-zero check. Also fixed an
error (added code to clear accumulators each time through the loop).
trunk/code/behaviors/formation_control/push_pull/main.c | ||
---|---|---|
95 | 95 |
|
96 | 96 |
while (1) { |
97 | 97 |
|
98 |
/* Make sure to clear our accumulators */ |
|
98 | 99 |
net_x_comp = 0; |
99 | 100 |
net_y_comp = 0; |
100 | 101 |
total_intensity = 0; |
... | ... | |
109 | 110 |
total_intensity += intensity[i]; |
110 | 111 |
} |
111 | 112 |
|
112 |
normalized_net_x_comp = net_x_comp / total_intensity; |
|
113 |
normalized_net_y_comp = net_y_comp / total_intensity; |
|
113 |
if (total_intensity > 0) { |
|
114 |
normalized_net_x_comp = net_x_comp / total_intensity; |
|
115 |
normalized_net_y_comp = net_y_comp / total_intensity; |
|
116 |
} |
|
114 | 117 |
|
115 |
delay_ms(500); |
|
116 |
|
|
117 | 118 |
usb_puts("x: "); |
118 | 119 |
usb_puti(normalized_net_x_comp); |
119 | 120 |
usb_puts("\ty: "); |
120 | 121 |
usb_puti(normalized_net_y_comp); |
121 |
usb_puts("\n\n");
|
|
122 |
usb_puts("\n"); |
|
122 | 123 |
|
124 |
delay_ms(500); |
|
125 |
|
|
123 | 126 |
} |
124 | 127 |
|
125 | 128 |
while(1); |
Also available in: Unified diff