Project

General

Profile

Revision 1103

Reverted changes accidentally committed

View differences:

trunk/code/behaviors/smart_run_around_fsm/main.c
1 1
#include <dragonfly_lib.h>
2
//#include "smart_run_around_fsm.h"
2
#include "smart_run_around_fsm.h"
3 3

  
4
#include "drive.h"
5

  
6
// Martin "deffi" Herrmann Test
7

  
8
uint8_t motors_on=0;
9

  
10
static void periodic_call ()
11
{
12
	if (button1_read ())
13
	{
14
		motors_on=~motors_on;
15
	}
16

  
17
	if (motors_on)
18
	{
19
		int s=(wheel ()/2+127)&0xFF;
20
		motor1_set (FORWARD, s);
21
		motor2_set (FORWARD, s);
22
	}
23
	else
24
		motors_off ();
25
		
26
	while (button1_read ()); // Wait for button release *after* setting new motor state
27
}
28

  
29
static void spirograph ()
30
{
31
	uint8_t left=255;
32
	uint8_t right_min=160;
33
	uint8_t right_max=200;
34
	uint8_t delay=100;
35
	uint8_t step=2;
36
	
37
	uint8_t dir=step, right=right_min;
38

  
39
	orb_set (0,255,0);
40
	
41
	while (1)
42
	{
43
		if (right<right_min) dir=step;
44
		if (right>right_max) dir=-step;
45
		
46
		right+=dir;
47
		
48
		motor1_set (FORWARD, left);
49
		motor2_set (FORWARD, right);
50
		
51
		delay_ms (delay);
52
	}
53
}
54

  
55
static void color_circle ()
56
{
57
   while(1)
58
   {
59
		for (uint8_t part=0; part<6; ++part)
60
		{
61
			for (uint8_t up=0; up<255; ++up)
62
			{
63
				uint8_t dn=255-up;
64
				uint8_t on=255;
65
				uint8_t of=0;
66
				
67
				if (1)
68
				{
69
					// Maximum brightness mode
70
					switch (part)
71
					{
72
						case 0: orb_set (on, up, of); break;
73
						case 1: orb_set (dn, on, of); break;
74
						case 2: orb_set (of, on, up); break;
75
						case 3: orb_set (of, dn, on); break;
76
						case 4: orb_set (up, of, on); break;
77
						case 5: orb_set (on, of, dn); break;
78
					}
79
				}
80
				else
81
				{
82
					// Constant brightness mode (not very constant though)
83
					switch (part%3)
84
					{
85
						case 0: orb_set (dn, up, of); break;
86
						case 1: orb_set (of, dn, up); break;
87
						case 2: orb_set (up, of, dn); break;
88
					}
89
				}
90
				
91
				delay_ms (2);
92
				periodic_call (); 
93
			}
94
		}
95
	}
96
}
97

  
98
static void startup_sequence()
99
{
100
	orb_set (255, 0, 0); delay_ms (200);
101
	orb_set (0, 0, 0); delay_ms (200);
102
	orb_set (255, 0, 0); delay_ms (200);
103
	orb_set (0, 0, 0); delay_ms (200);
104
	orb_set (255, 0, 0); delay_ms (200);
105
	orb_set (0, 0, 0); delay_ms (500);
106
}
107

  
108
static void right_controller()
109
{
110
	encoders_init ();
111
	encoder_rst_dx (0); encoder_rst_dx (1);
112

  
113
	while (1)
114
	{
115
		int el=encoder_get_dx (0);
116
		int er=encoder_get_dx (1);
117
		
118
		int diff=el-er;
119
		uint8_t direction=(diff>0)?FORWARD:BACKWARD;
120
		
121
		if (diff>2)
122
			diff+=180;
123
		else if (diff<-2)
124
			diff-=180;
125
		else
126
			diff=0;
127

  
128
		if (diff>255) diff=255;
129
		if (diff<-255) diff=-255;
130
		
131
		motor2_set(direction, abs (diff));
132
		
133
		usb_puti (el); usb_puts (" ");
134
		usb_puti (er); usb_puts (" ");
135
		usb_puti (diff); usb_puts (" ");
136
		usb_puts ("            \r");
137
	}		
138
}
139

  
140
static void acl()
141
{
142
	#define redval 255
143
	#define greenval 150
144
	#define interval 1300
145
	#define flash 20
146
	#define pause 200
147

  
148
	#define def do { orb1_set(redval,0,0); orb2_set(0,greenval,0); } while (0)
149
	#define wht do { orb_set(255,255,255); } while (0)
150
	
151
	while (1)
152
	{
153
		periodic_call (); 
154
		wht; delay_ms (flash);
155
		def; delay_ms (pause-flash);
156
		periodic_call (); 
157
		wht; delay_ms (flash);
158
		def; delay_ms (pause-flash);
159
		def; delay_ms (interval-2*pause-2*flash);
160
	}
161
}
162

  
163
static void redlight ()
164
{
165
	uint8_t r=255;
166
	int8_t d=-1;
167
	
168
	while (1)
169
	{
170
		if (r==0) d=1;
171
		if (r==255) d=-1;
172
		r+=d;
173
		
174
		orb_set(r,0,0);
175
		delay_ms (5);
176
	}
177
}
178

  
179
static uint8_t hex_digit(uint8_t x)
180
{
181
	if (x>15) return '?';
182
	return "0123456789ABCDEF"[x];
183
}
184

  
185

  
186
static void usb_puth(int i)
187
{
188
	usb_putc(hex_digit((i>>12)&0xF));
189
	usb_putc(hex_digit((i>>8)&0xF));
190
	usb_putc(hex_digit((i>>4)&0xF));
191
	usb_putc(hex_digit(i&0xF));
192
}
193

  
194
static void usb_puth8(uint8_t i)
195
{
196
	usb_putc(hex_digit((i>>4)&0xF));
197
	usb_putc(hex_digit(i&0xF));
198
}
199

  
200
static void bom_beacon ()
201
{
202
	//bom_init (BOM15); // Done by dragonfly init
203

  
204
	//bom_set_leds(1<<4);
205
	//bom_set_leds(0xAAAA);
206
	bom_set_leds(0xFFFF);
207
	bom_on ();
208
	redlight ();
209

  
210
}
211

  
212
static int get_color(int x)
213
{
214
	switch (x)
215
	{
216
		case 0: return RED;
217
		case 1: return YELLOW;
218
		case 2: return GREEN;
219
		case 3: return CYAN;
220
		case 4: return BLUE;
221
		case 5: return MAGENTA;
222
		default: return WHITE;
223
	}
224
}
225

  
226
static void set_color_code(int x)
227
{
228
	if (0<=x && x<6)
229
	{
230
		orb1_set_color (get_color (x));
231
		orb2_set_color (ORB_OFF);
232
	}
233
	else if (6<=x && x<12)
234
	{
235
		orb1_set_color (ORB_OFF);
236
		orb2_set_color (get_color (x-6));
237
	}
238
	else if (12<=x && x<18)
239
	{
240
		orb1_set_color (get_color (x-12));
241
		orb2_set_color (get_color (x-12));
242
	}
243
	else
244
	{
245
		orb1_set_color (ORB_OFF);
246
		orb2_set_color (ORB_OFF);
247
	}
248
}
249

  
250
static void bom_test()
251
{
252
	analog_init (0/*ADC_START*/);
253
	//bom_init (BOM15); // Done by dragonfly init
254
	bom_off ();
255

  
256
	uint8_t num=0;
257

  
258
	#define selected 9
259

  
260
	while (1)
261
	{
262
		// Each 100 ms, print all values, hex, on one line
263
		if (0)
264
		{
265
			bom_refresh(BOM_ALL);
266
			for (uint8_t i=0; i<16; ++i)
267
			{
268
				usb_puti (i);
269
				usb_putc (':');
270
				usb_puth8 ((uint8_t)(bom_get(i)));
271
				usb_putc (' ');
272
			}
273
			
274
			usb_puts ("\r\n");
275
			delay_ms (100);
276
		}
277
		
278
		if (0)
279
		{
280
			// Average 10 #selected values over 1 second, print dec, one per line
281
			uint16_t val=0;
282
			for (int i=0; i<10; ++i)
283
			{
284
				bom_refresh (1<<selected);
285
				val+=bom_get(selected);
286
				delay_ms (100);
287
			}
288
			usb_puti(val);
289
			usb_puts("\r\n");
290
		}
291

  
292
		// Read #selected, print dec//, 16 values per line.
293
		if (0)
294
		{
295
			bom_refresh (1<<selected);
296
			usb_puti (selected);
297
			usb_puts (": ");
298
			usb_puti (bom_get (selected));
299
//			num++;
300
//			if (num>15)
301
//			{
302
				usb_puts ("\r\n");
303
//				num=0;
304
//			}
305
//			else
306
//				usb_putc (' ');
307
			delay_ms (100);
308
		}
309
		
310
		// Print maximum of all, one per line
311
		if (1)
312
		{
313
			bom_refresh (0xFFFF);
314
			usb_puti (bom_get_max ());
315
			set_color_code(bom_get_max ());
316
			usb_puts ("\r\n");
317
			delay_ms (100);
318
		}
319
	}
320
}
321

  
322
void rtc_callback ()
323
{
324
	static uint8_t x=0;
325
	
326
	motor1_set(x?FORWARD:BACKWARD, 180);
327
	motor2_set(x?BACKWARD:FORWARD, 180);
328

  
329
	x=!x;
330
}
331

  
332
static void rtc_test ()
333
{
334
	rtc_init (HALF_SECOND, rtc_callback);
335
	while (1);
336
}
337

  
338
void drive_controller_test ()
339
{
340

  
341
	// Drive straight ahead
342
	drive_controller dc;
343
	drive_controller_set_parameters_shift (&dc, 0, -16);
344
	drive_controller_init (&dc);
345

  
346
	while (1)
347
	{
348
		// Use the wheel for velocity control
349
		drive_controller_set_target_velocity (&dc, wheel ()/2+127);
350
		
351
		// Do the control
352
		drive_controller_control (&dc);
353
		
354
		delay_ms (1);
355
	}
356
}
357

  
358
void encoder_timing_test ()
359
{
360
	//bom_init(BOM15);
361
	//bom_set_leds(0xAAAA);
362
	//bom_on ();
363

  
364
	DDRF |= 1<<1;
365
	//encoders_init ();
366

  
367

  
368
	while (1)
369
	{
370
		PORTF|=0x02;
371
		PORTF&=0xFD;
372
		PORTF|=0x02;
373
		PORTF&=0xFD;
374
		PORTF|=0x02;
375
		PORTF&=0xFD;
376
		PORTF|=0x02;
377
		PORTF&=0xFD;
378
		PORTF|=0x02;
379
		PORTF&=0xFD;
380
		PORTF|=0x02;
381
		PORTF&=0xFD;
382
		//bom_set_leds(0x0001);
383
		//bom_set_leds(0x0003);
384
	}
385

  
386
}
387

  
388
static void duty_cycle_test ()
389
{
390
	DDRF=0xFF;
391
	while (1)
392
	{
393
		if (button1_read ())
394
		{
395
			// Button 1 => 1/20 (5%)
396
			PORTF=0x02; delay_ms (1);
397
			PORTF=0x00; delay_ms (19);
398
		}
399
		else if (button2_read ())
400
		{
401
			// Button 2 => 3/20 (15%)
402
			PORTF=0x02; delay_ms (3);
403
			PORTF=0x00; delay_ms (17);
404
		}
405
		else
406
		{
407
			// No button (or both) => Alternating, avg 2/20 (10%)
408
			PORTF=0x02; delay_ms (1);
409
			PORTF=0x00; delay_ms (19);
410
			PORTF=0x02; delay_ms (3);
411
			PORTF=0x00; delay_ms (17);
412
		}
413
	}
414
}	
415

  
416 4
int main(void) {
417
//	dragonfly_init(ALL_ON);
418
	dragonfly_init(0);
419
	
420
	//encoders_init ();
421

  
422
	analog_init(0);
423
//    usb_init();
424

  
425
//	encoder_timing_test ();
426

  
427

  
428
	DDRF=0x02;
429

  
430
	if (button2_read ())
431
		duty_cycle_test ();
432
	else
433
	{
434
		orb_init ();
435
		orb_enable ();
436
		color_circle ();
437
	}
438
	
439
	orb_init ();
440
	orb_enable ();
441

  
442
//	while (1)
443
//	{
444
//		
445
//		orb_set (128,128,0);
446
//		delay_ms (1000);
447
//
448
//
449
//		orb1_set (128,128,0);
450
//		orb2_set (128,128,0);
451
//		delay_ms (1000);
452
//	}
453

  
454
//	orb1_set (128,0,0);
455
//	orb2_set (255,0,0);
456
//	while (1);
457
	
458
    //analog_init(ADC_START);
459

  
460

  
461
    //range_init();
462
	
463
	usb_puts ("Startup\r\n");
464

  
465

  
466
	//bom_beacon ();
467
	//bom_test ();
468
	//rtc_test ();
469

  
470
	// Do some lighting and wheel controlled motors on BTN1
471
	if (!button2_read ())
472
		acl ();
473
	else
474
		color_circle ();
475

  
476

  
477
	startup_sequence ();
478
	drive_controller_test ();
5
	dragonfly_init(ALL_ON);
6
    range_init();
7
	//orb_enable();
8
	usb_puts("Turned on!\n");
9
    run_around_init();
10
    while(1) {
11
        run_around_FSM();
12
    }
13
	return 0;
479 14
}

Also available in: Unified diff