Project

General

Profile

Revision 1736

joystick demo - not working yet

View differences:

bom.c
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26

  
27
/**
28
 * @file bom.c
29
 * @brief Implementation for using the BOM
30
 *
31
 * Contains functions for using the Bearing and Orientation Module (BOM)
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

  
36
#include "bom.h"
37
#include "dio.h"
38
#include "serial.h"
39
#include "analog.h"
40

  
41
//On the original BOM1.0, the emmitter angular order does not match the analog mux order
42
//so you need to iterate through the mux index in the following order if you want to get
43
//the detector readings in order:
44
static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
45

  
46

  
47
/* *****************************
48
 * BOM Vector Component Tables *
49
 **************************** **/
50

  
51
/*
52
 * The x component of each BOM detector (indexed from 0 to 15)
53
 * was calculated using the following formula:
54
 *
55
 *		x_comp[i] = fix(25 * cos ( 2 * pi / 16 * i) )
56
 *
57
 * where "fix" rounds towards 0. If the BOM detectors were superimposed
58
 * onto a 2 dimensional Cartesian space, this effectively calculates the
59
 * x component of the emitter vector where emitter 0 corresponds to an
60
 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
61
 */
62
static const signed int x_comp[16] = {
63
	25,
64
	23,
65
	17,
66
	9,
67
	0,
68
	-9,
69
	-17,
70
	-23,
71
	-25,
72
	-23,
73
	-17,
74
	-9,
75
	0,
76
	9,
77
	17,
78
	23
79
};
80

  
81

  
82
/*
83
 * The y component of each BOM detector (indexed from 0 to 15)
84
 * was calculated using the following formula:
85
 *
86
 *		y_comp[i] = fix(25 * sin ( 2 * pi / 16 * i) )
87
 *
88
 * where "fix" rounds towards 0. If the BOM detectors were superimposed
89
 * onto a 2 dimensional Cartesian space, this effectively calculates the
90
 * x component of the emitter vector where emitter 0 corresponds to an
91
 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
92
 */
93
static signed int y_comp[16] = {
94
	0,
95
	9,
96
	17,
97
	23,
98
	25,
99
	23,
100
	17,
101
	9,
102
	0,
103
	-9,
104
	-17,
105
	-23,
106
	-25,
107
	-23,
108
	-17,
109
	-9
110
};
111

  
112
// internal function prototypes
113
static void bom_select(char which);
114

  
115
/*
116
 Bk R Y (Analog)
117
---------
118
 Green
119
 Blue
120
 White
121
---------
122
 Blue
123
 White
124
*/
125

  
126

  
127
/*
128
the analog pin definitions from dio.h DO NOT work here,
129
so we must use PF0 from avrgcc (as opposed to _PIN_F0).
130
BUT the dio pin definitions from dio.h must be used (no PE...).
131

  
132
also, _PIN_E2 is initialized to high for some reason,
133
which turns the BOM on when the robot is turned on.
134
WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
135

  
136
*/
137

  
138
#define MONKI PF0         //analog (yellow)
139
//------------------------//
140
#define MONKL _PIN_E2     //green
141
#define MONK1 _PIN_E3     //blue
142
#define MONK0 _PIN_E4     //white
143
//------------------------//
144
#define MONK3 _PIN_E6     //blue
145
#define MONK2 _PIN_E7     //white
146

  
147
#define BOM_VALUE_THRESHOLD 150 //200
148
#define NUM_BOM_LEDS 16
149

  
150
/*
151
  *The following pin definitions are for the BOM v1.5
152
  */
153

  
154
#define BOM_MODE	_PIN_E2	//dio0
155
#define BOM_STROBE	_PIN_E3	//dio1
156

  
157
#define BOM_DATA	_PIN_A0 //servo0
158
#define BOM_CLOCK	_PIN_A1	//servo1
159

  
160
#define BOM_S0		_PIN_E5	//dio3
161
#define BOM_S1		_PIN_E4	//dio2
162
#define BOM_S2		_PIN_E7	//dio4
163
#define BOM_S3		_PIN_E6	//dio5
164
#define BOM_OUT		PF0		//analog(yellow)
165

  
166
/**
167
 * @defgroup bom BOM (Bearing and Orientation Module)
168
 * @brief Functions for dealing with the BOM.
169
 *
170
 * The Bearing and Orientation Module / Barrel of Monkeys / BOM
171
 * is a custom sensor designed and built by the Colony Project.
172
 * It consists of a ring of 16 IR emitters and 16 IR detectors.
173
 * The BOM is most often use to determine the direction of other
174
 * robots. This module contains functions for controlling the BOM.
175
 *
176
 * Include bom.h to access these functions.
177
 *
178
 * @{
179
 **/
180

  
181
static unsigned int bom_val[NUM_BOM_LEDS];
182
static volatile char bom_type = BOM10;
183
static int select_pins[4];
184
static int analog_pin;
185

  
186
/**
187
 * Initializes the BOM.
188
 * Call bom_init before reading bom values or turning bom leds.
189
 *
190
 * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out
191
 * until Cornell and Pras return
192
 * 
193
 * @see bom_refresh, bom_leds_on, bom_leds_off
194
 **/
195
void bom_init(char type) {
196
    bom_type = type;
197
    
198
    switch(bom_type) {
199
    case BOM10:
200
		select_pins[0] = MONK0; 
201
		select_pins[1] = MONK1;
202
		select_pins[2] = MONK2;
203
		select_pins[3] = MONK3;
204
		analog_pin = MONKI;
205
        break;
206
    case BOM15:
207
        //Sets BOM1.5 to normal [BOM] mode
208
        digital_output(BOM_MODE, 0);
209
		select_pins[0] = BOM_S0; 
210
		select_pins[1] = BOM_S1;
211
		select_pins[2] = BOM_S2;
212
		select_pins[3] = BOM_S3;
213
		bom_set_leds(BOM_ALL);
214
		analog_pin = BOM_OUT;
215
        break;
216
    case RBOM:
217
        break;
218
    //default:
219
    }
220
}
221

  
222
/**
223
 * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
224
 *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
225
 *    must be called for this to work. Must call this before reading BOM values!
226
 *
227
 *
228
 * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
229
 *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
230
 *
231
 * @see bom_get
232
 **/
233
void bom_refresh(int bit_field) {
234
    int i;
235
	int loop_was_running = 0;
236
    
237
	//Check analog loop status
238
    if(analog_loop_status() == ADC_LOOP_RUNNING) {
239
		loop_was_running = 1;
240
		analog_stop_loop();
241
	}
242
    
243
	//Read BOM values
244
    for(i = 0; i < NUM_BOM_LEDS; i++) {
245
        if(bit_field & 0x1) {
246
            bom_select(i);
247
            bom_val[i] = analog_get8(analog_pin);
248
        }
249
        bit_field = bit_field >> 1;
250
    }
251
    
252
	//Restore analog loop status
253
	if(loop_was_running)
254
		analog_start_loop();
255
}
256

  
257
/**
258
 * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
259
 *
260
 * @pre must call bom refresh first
261
 *
262
 * @param which which bom value to return
263
 *
264
 * @return the bom value
265
 *
266
 * see bom_refresh
267
 **/
268
int bom_get(int which) {
269
	//return bom_val[which];      deleted because we got rid of the analog loop
270
	bom_select(which);
271
    return analog_get8(analog_pin);
272
}
273

  
274
/** 
275
 * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
276
 *
277
 * @pre must call bom refresh
278
 * @return index to the lowest (max) bom value element.  -1 if no value is lower than
279
 *    BOM_VALUE_THRESHOLD
280
 **/
281
int bom_get_max(void) {
282
    int i, lowest_val, lowest_i, i_val;
283
    lowest_i = -1;
284
    lowest_val = 255;
285
    for(i = 0; i < NUM_BOM_LEDS; i++) {
286
		i_val = bom_get(i);
287
        if(i_val < lowest_val) {
288
            lowest_val = i_val;
289
            lowest_i = i;
290
        }
291
    }
292
    
293
    if(lowest_val < BOM_VALUE_THRESHOLD)
294
        return lowest_i;
295
    else
296
        return -1;
297
}
298

  
299
/** 
300
 * Compute the net resultant BOM IR vector by scaling each IR unit vector by its intensity
301
 *		and summing over all IR LEDs.
302
 *
303
 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
304
 *		component.
305
 *
306
 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
307
 *		has already collected BOM information. Otherwise, leave as NULL and a new set of data
308
 *		will be collected and used.
309
 *
310
 * @return  Exit status - Zero for success; negative on error.
311
 **/
312
int bom_get_vector(Vector* v, int* usrBOMvals) {
313

  
314
	/* Store current BOM readings and use them as a weighting factor */
315
	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
316

  
317
	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
318
	 * components. Calculated by multiplying the intensity by the x and y
319
	 * component respectively (x and y components are stored in the tables
320
	 * above). */
321
	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
322
	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
323
	
324
	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
325
	 * components for the entire robot. */
326
	long net_x_comp = 0;
327
	long net_y_comp = 0;
328

  
329
	int i = 0;
330

  
331
	/* BOM intensity is actually measured as more intense = closer to 0 */
332
	if (usrBOMvals) {
333
		/* Use BOM values collected by user */
334
		for (i = 0; i < 16; i++) {
335
			intensity[i] = 255 - usrBOMvals[i];
336
		}
337
	} else {
338
		/* Collect new set of BOM data */
339
		bom_refresh(BOM_ALL);
340
		for (i = 0; i < 16; i++) {
341
			intensity[i] = 255 - bom_get(i);
342
		}
343
	}
344

  
345
	/* Calculate weighted vector components and accumulate vector sum */
346
	for (i = 0; i < 16; i++) {
347
		weighted_x_comp[i] = intensity[i] * x_comp[i];
348
		weighted_y_comp[i] = intensity[i] * y_comp[i];
349
		net_x_comp += weighted_x_comp[i];
350
		net_y_comp += weighted_y_comp[i];
351
	}
352

  
353
	/* Fill the Vector struct */
354
	v->x = net_x_comp;
355
	v->y = net_y_comp;
356

  
357
	return 0;
358

  
359
}
360

  
361
/** 
362
 * Compute the normalized net resultant BOM IR vector by scaling each IR unit vector by its
363
 *		intensity and summing over all IR LEDs.
364
 *
365
 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
366
 *		component.
367
 *
368
 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
369
 *		has already collected BOM information. Otherwise, leave as NULL and a new set of data
370
 *		will be collected and used.
371
 *
372
 * @return  Exit status - Zero for success; negative on error.
373
 **/
374
int bom_get_norm_vector(Vector* v, int* usrBOMvals) {
375

  
376
	/* Store current BOM readings and use them as a weighting factor */
377
	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
378

  
379
	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
380
	 * components. Calculated by multiplying the intensity by the x and y
381
	 * component respectively (x and y components are stored in the tables
382
	 * above). */
383
	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
384
	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
385
	
386
	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
387
	 * components for the entire robot. */
388
	long net_x_comp = 0;
389
	long net_y_comp = 0;
390

  
391
	/* Variables used to normalize the net component values */
392
	int total_intensity = 0;
393
	int normalized_net_x_comp = 0;
394
	int normalized_net_y_comp = 0;
395

  
396
	int i = 0;
397

  
398
	/* BOM intensity is actually measured as more intense = closer to 0 */
399
	if (usrBOMvals) {
400
		/* Use BOM values collected by user */
401
		for (i = 0; i < 16; i++) {
402
			intensity[i] = 255 - usrBOMvals[i];
403
		}
404
	} else {
405
		/* Collect new set of BOM data */
406
		bom_refresh(BOM_ALL);
407
		for (i = 0; i < 16; i++) {
408
			intensity[i] = 255 - bom_get(i);
409
		}
410
	}
411

  
412
	/* Calculate weighted vector components and accumulate vector sum */
413
	for (i = 0; i < 16; i++) {
414
		weighted_x_comp[i] = intensity[i] * x_comp[i];
415
		weighted_y_comp[i] = intensity[i] * y_comp[i];
416
		net_x_comp += weighted_x_comp[i];
417
		net_y_comp += weighted_y_comp[i];
418
		total_intensity += intensity[i];
419
	}
420

  
421
	/* Normalize the resultant vector components by the total intensity */
422
	if (total_intensity > 0) {
423
		normalized_net_x_comp = net_x_comp / total_intensity;
424
		normalized_net_y_comp = net_y_comp / total_intensity;
425
	}
426

  
427
	/* Fill the Vector struct */
428
	v->x = normalized_net_x_comp;
429
	v->y = normalized_net_y_comp;
430

  
431
	return 0;
432

  
433
}
434

  
435
/** 
436
 * Print a histogram which shows the current BOM intensity values for each of the 16 BOM IR
437
 *		sensors. The function will attempt to send the histogram data over USB.
438
 *
439
 * @param curBOMvals  Pointer to an array of the current BOM values (the array must have
440
 *		length 16). Use this to print values you have already collected. Otherwise pass in NULL
441
 *		and bom_refresh() will be called and the current BOM intensity values will be collected.
442
 * @return  Exit status - Zero for success; negative on error.
443
 **/
444
int bom_print_usb(int* usrBOMvals) {
445

  
446
	int i, j, max = -1;
447
	int curVals[16];
448
	int* prtValPtr;
449

  
450
	if (usrBOMvals) {
451
		/* Use BOM values collected by user */
452
		prtValPtr = usrBOMvals;
453

  
454
		/* Find max BOM value from users values */
455
		for (i = 0; i < 16; i++) {
456
			if (max < prtValPtr[i])
457
				max = prtValPtr[i];
458
		}
459
	} else {
460
		/* Refresh and make sure the table is updated */
461
		bom_refresh(BOM_ALL);
462

  
463
		/* Record values into an array */
464
		for (i = 0; i < 16; i++) {
465
			curVals[i] = bom_get(i);
466
			if (max < curVals[i])
467
				max = curVals[i];
468
		}
469

  
470
		/* Use the current set of collected values */
471
		prtValPtr = curVals;
472
	}
473

  
474
	/* Display results */
475
	for (i = 0; i < 16; i++) {
476
		
477
		usb_puti(i);
478
		usb_puts(": ");
479
		usb_puti(prtValPtr[i]);
480
		usb_putc('\t');
481

  
482
		for (j = 0; j < (int)((max - prtValPtr[i]) / 5); j++) {
483
			usb_putc('#');
484
		}
485
		usb_puts("\r\n");
486
	}
487
	usb_puts("\r\n");
488

  
489
	return 0;
490

  
491
}
492

  
493
/** 
494
 * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.
495
 *
496
 * @pre must call bom refresh
497
 * @param dist  pointer to int in which to return the estimated distance to the other robot
498
 * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the
499
 *        index of the max bom value.  -1 if no value is lower than BOM_VALUE_THRESHOLD.
500
 **/
501
int bom_get_max10(int *dist) {
502
    int i, max;
503
    long long mean = 0, sum = 0;
504

  
505
    max = bom_get_max();
506
    if (max < 0)
507
    {
508
        if (dist)
509
        {
510
            *dist = -1;
511
        }
512
        return -1;
513
    }
514
    /* Record values into an array */
515
    for (i = 0; i < NUM_BOM_LEDS; i++) {
516
        int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max);
517
        int val = 255 - bom_get(i);
518
        mean += idx * val;
519
        sum += val;
520
    }
521
    mean = (mean * 10) / sum;
522
    mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10);
523

  
524
    if (dist)
525
    {
526
        *dist = 50 - sum/48;
527
    }
528

  
529
    return mean;
530
}
531

  
532
/**
533
 * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will
534
 *    be enabled to turn on when bom_on() is called.
535
 *    bom_init must be called for this to work. Does nothing if a BOM1.0 is installed
536
 *
537
 * @param bit_field specifies which leds should be turned on when bom_on is called.  Use BOM_ALL to turn on all bom leds.
538
 *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
539
 **/
540
void bom_set_leds(int bit_field) {
541
    int i;
542
	unsigned int mask = 1<<(NUM_BOM_LEDS-1);
543
	switch(bom_type) {
544
    case BOM10:
545
        //TODO: put an assert here to alert the user that this should not be called
546
        break;
547
		
548
    case BOM15:
549
	    for(i=NUM_BOM_LEDS; i>0; i--)
550
	    {
551
		    //set the current bit, sending MSB first
552
		    digital_output(BOM_DATA, bit_field&mask);
553
		    //then pulse the clock
554
		    digital_output(BOM_CLOCK, 1);
555
		    digital_output(BOM_CLOCK, 0);
556
			mask = mask>>1;
557
	    }
558
        break;
559
		
560
    case RBOM:
561
        //add rbom code here
562
        break;
563
    }
564
}
565

  
566

  
567
/**
568
 * (DEPRECATED) Returns the direction of the maximum BOM reading,
569
 * as an integer in the range 0-15. 0 indicates to the
570
 * robot's right, while the rest of the sensors are
571
 * numbered counterclockwise. This is useful for determining
572
 * the direction of a robot flashing its BOM, of only one
573
 * robot is currently doing so. analog_init must be called
574
 * before this function can be used.
575
 *
576
 * @return the direction of the maximum BOM reading
577
 *
578
 * @see analog_init
579
 **/
580
int get_max_bom(void) {
581
    bom_refresh(BOM_ALL);
582
    return bom_get_max();
583
}
584

  
585
/**
586
 * Flashes the BOM.  If using a BOM1.5, only the emitters that have been enabled using
587
 * bom_set_leds will turn on.
588
 * 
589
 * @see bom_off, bom_set_leds
590
 **/
591
void bom_on(void)
592
{
593
  switch(bom_type) {
594
  case BOM10:
595
	digital_output(MONKL, 1);
596
	break;
597
  case BOM15:
598
	digital_output(BOM_STROBE, 1);
599
	break;
600
  case RBOM:
601
	break;
602
  }
603
}
604

  
605
/**
606
 * Turns off all bom leds.
607
 * 
608
 * @see bom_on
609
 **/
610
void bom_off(void)
611
{
612
  switch(bom_type) {
613
  case BOM10:
614
	digital_output(MONKL, 0);
615
	break;
616
  case BOM15:
617
	digital_output(BOM_STROBE, 0);
618
	break;
619
  case RBOM:
620
	break;
621
  }
622
}
623

  
624
/** @} **/ //end group
625

  
626
//select a detector to read
627
static void bom_select(char which) {
628
	if(bom_type == BOM10)
629
	  which = lookup[(int)which];
630
	
631
    if (which&8)
632
      digital_output(select_pins[3], 1);
633
    else
634
      digital_output(select_pins[3], 0);
635

  
636
    if (which&4)
637
      digital_output(select_pins[2], 1);
638
    else
639
      digital_output(select_pins[2], 0);
640

  
641
    if (which&2)
642
      digital_output(select_pins[1], 1);
643
    else
644
      digital_output(select_pins[1], 0);
645

  
646
    if (which&1)
647
      digital_output(select_pins[0], 1);
648
    else
649
      digital_output(select_pins[0], 0);
650
	
651
}

Also available in: Unified diff