Project

General

Profile

Revision 1800

Robots still oscillate while facing in wrong direction

View differences:

trunk/code/behaviors/formation_control/circle/circle.c
207 207
{
208 208
	int inverse = (goal + 8) % 16;	// the inverse of the goal direction, across the BOM.
209 209
	int bomNum = -1;
210
	int speed;	// speed with which to turn
210 211

  
211 212
	orb1_set_color(BLUE);			// BLUE and PURPLE
212
	left(220);
213
	left(180);
213 214
	while(bomNum != goal)
214 215
	{
215 216
		// bomNum is the current maximum reading
......
228 229
		else if (goal > inverse)
229 230
		{
230 231
			if (bomNum < goal && bomNum > inverse)	// goal clockwise of position
231
				right(220);
232
			{
233
				speed = 160 + 10 * (goal - bomNum);
234
				right(180);
235
			}
232 236
			else	// goal counterclockwise of position
233
				left(220);
237
			{
238
				speed = 160 + 10 * ((bomNum > goal) ? bomNum - goal : goal - bomNum);
239
				left(180);
240
			}
234 241
			timeout = 0;
235 242
		}
236 243
		else	// goal < inverse
237 244
		{
238 245
			if (bomNum > goal && bomNum < inverse)	// goal counterclockwise of position
239
				left(220);
246
			{
247
				speed = 160 + 10 * (bomNum - goal);
248
				left(180);
249
			}
240 250
			else
241
				right(220);
251
			{
252
				speed = 160 + 10 * ((bomNum < goal) ? goal - bomNum : bomNum - goal);
253
				right(180);
254
			}
242 255
			timeout = 0;
243 256
		}
244 257
	}

Also available in: Unified diff