Revision 1409
Changed BOM threshold to 120, seems to be helping for some robot and causing problems for others
hunter-prey is done except the hunting part
trunk/code/lib/include/libdragonfly/lights.h | ||
---|---|---|
182 | 182 |
|
183 | 183 |
// ***** Setting RGB colors ***** |
184 | 184 |
|
185 |
/** @brief set the specified orb to a specified color **/ |
|
185 |
/** @brief set the specified orb to a specified color |
|
186 |
* @deprecated This function indexes from 0 instead of 1 **/ |
|
186 | 187 |
void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue); |
187 | 188 |
|
188 | 189 |
/** @brief Set both orbs to a specified color **/ |
... | ... | |
200 | 201 |
|
201 | 202 |
// ***** Settings predefined colors ***** |
202 | 203 |
|
203 |
/** @brief set the specified orb to the specified color **/ |
|
204 |
/** @brief set the specified orb to the specified color |
|
205 |
* @deprecated This function indexes from 0 instead of 1 **/ |
|
204 | 206 |
void orb_n_set_color(uint8_t num, uint8_t col); |
205 | 207 |
|
206 | 208 |
/** @brief Set orb1 to a specified color **/ |
trunk/code/lib/include/libdragonfly/analog.h | ||
---|---|---|
36 | 36 |
* to analog. |
37 | 37 |
|
38 | 38 |
* @author Colony Project, CMU Robotics Club, based on firefly |
39 |
* code by Tom Lauwers |
|
39 |
* originally taken from fwr analog file (author: Tom Lauwers) |
|
40 |
* loop code written by Kevin Woo and James Kong |
|
40 | 41 |
*/ |
41 | 42 |
|
42 | 43 |
#ifndef _ANALOG_H |
... | ... | |
91 | 92 |
/** @brief Analog port for the battery voltage detector **/ |
92 | 93 |
#define BATT_PORT AN11 |
93 | 94 |
|
95 |
/** @brief Analog loop status. ADC conversion running. **/ |
|
96 |
#define ADC_LOOP_RUNNING 1 |
|
97 |
/** @brief Analog loop status. No ADC conversion running.**/ |
|
98 |
#define ADC_LOOP_STOPPED 0 |
|
99 |
|
|
100 |
/** @brief Analog init parameter. Start the analog loop. **/ |
|
94 | 101 |
#define ADC_START 1 |
95 |
#define ADC_STOP 0 |
|
102 |
/** @brief Analog init parameter. Don't start the analog loop. **/ |
|
103 |
#define ADC_STOP 0 |
|
96 | 104 |
|
97 | 105 |
#define ADMUX_OPT 0x60 |
98 | 106 |
|
... | ... | |
110 | 118 |
void analog_start_loop(void); |
111 | 119 |
/** @brief Stops the analog loop. Doesn't do anything if the loop is already stopped. **/ |
112 | 120 |
void analog_stop_loop(void); |
121 |
/** @brief Returns the status of the analog loop. **/ |
|
122 |
int analog_loop_status(void); |
|
113 | 123 |
/** @brief Returns an 8-bit analog value from the look up table. Use this instead of analog_get8. **/ |
114 | 124 |
unsigned int analog8(int which); |
115 | 125 |
/** @brief Returns an 10-bit analog value from the look up table. Use this instead of analog_get10. **/ |
trunk/code/projects/hunter_prey/main.c | ||
---|---|---|
20 | 20 |
volatile uint8_t tagger; |
21 | 21 |
uint8_t id; |
22 | 22 |
|
23 |
void test_bom() { |
|
24 |
int i; |
|
25 |
int val; |
|
26 |
|
|
27 |
while(1) { |
|
28 |
bom_refresh(BOM_ALL); |
|
29 |
|
|
30 |
for(i=0; i<16; i++) { |
|
31 |
val = bom_get(i); |
|
32 |
usb_puti(val); |
|
33 |
usb_putc(' '); |
|
34 |
} |
|
35 |
|
|
36 |
usb_putc('\n'); |
|
37 |
delay_ms(500); |
|
38 |
} |
|
39 |
|
|
40 |
} |
|
41 |
|
|
23 | 42 |
int main(void) { |
24 | 43 |
|
25 | 44 |
int frontRange; |
... | ... | |
27 | 46 |
uint8_t i; |
28 | 47 |
int onTarget = 0; |
29 | 48 |
char packet[2]; |
49 |
uint8_t bom = 0; |
|
30 | 50 |
|
31 | 51 |
//intialize the robot and wireless |
32 | 52 |
dragonfly_init(ALL_ON); |
33 | 53 |
|
54 |
//test_bom(); |
|
55 |
|
|
34 | 56 |
id = get_robotid(); |
35 | 57 |
packet[1] = id; |
36 | 58 |
usb_puts("hello, I am robot \n"); |
... | ... | |
45 | 67 |
|
46 | 68 |
run_around_init(); |
47 | 69 |
|
48 |
// bom_init(BOM15); |
|
49 |
|
|
50 | 70 |
usb_puts("channel\n"); |
51 | 71 |
|
52 | 72 |
//use a specific channel to avoid interfering with other tasks |
... | ... | |
60 | 80 |
//if button 2 is held, you are prey |
61 | 81 |
if(button2_read()) { |
62 | 82 |
robotState = ROBOT_PREY; |
83 |
bom_on(); |
|
63 | 84 |
} |
64 | 85 |
|
65 | 86 |
|
... | ... | |
86 | 107 |
|
87 | 108 |
else { //ROBOT_HUNTER |
88 | 109 |
|
89 |
// bom_refresh(BOM_ALL);
|
|
110 |
bom_refresh(BOM_ALL); |
|
90 | 111 |
|
91 | 112 |
frontRange = range_read_distance (IR2); |
92 | 113 |
|
93 |
/* usb_puti(frontRange); */
|
|
94 |
/* usb_putc('\n'); */
|
|
114 |
usb_puti(frontRange);
|
|
115 |
usb_putc(',');
|
|
95 | 116 |
|
96 | 117 |
//BUG: when get_max_bom is called, the rangefinder seems to stop |
97 | 118 |
//working |
98 |
//angle = get_max_bom(); |
|
99 |
angle = 4; |
|
119 |
angle = bom_get_max(); |
|
120 |
usb_puti(angle); |
|
121 |
usb_putc('\n'); |
|
100 | 122 |
|
101 | 123 |
if(angle == 4 && frontRange > 0 && frontRange < TAG_RANGE) { |
102 | 124 |
if(onTarget == 0) { |
... | ... | |
124 | 146 |
usb_puts("ACK!\n"); |
125 | 147 |
// delay_ms(TAG_PAUSE); |
126 | 148 |
robotState = ROBOT_PREY; |
149 |
bom_on(); |
|
127 | 150 |
} |
128 | 151 |
else { |
129 | 152 |
usb_puts("ACK failed!\n"); |
... | ... | |
156 | 179 |
if(packet[0] == 'T') { |
157 | 180 |
if(robotState == ROBOT_PREY) { |
158 | 181 |
robotState = ROBOT_TAGGED; |
182 |
bom_off(); |
|
159 | 183 |
color = BLUE; |
160 | 184 |
orb_set_color(BLUE); |
161 | 185 |
tagger = packet[1]; |
trunk/code/projects/hunter_prey/smart_run_around_fsm.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include "smart_run_around_fsm.h" |
|
3 |
|
|
4 |
void run_around_init(void) |
|
5 |
{ |
|
6 |
range_init(); |
|
7 |
orb_init(); |
|
8 |
|
|
9 |
state = SRA_FORWARD; // start out moving forward |
|
10 |
// initialize rangefinder values to 0 |
|
11 |
d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0; |
|
12 |
} |
|
13 |
|
|
14 |
/* evaluate rangefinder input and update state |
|
15 |
* call this function as often as possible to avoid collisions |
|
16 |
*/ |
|
17 |
void run_around_FSM(void) |
|
18 |
{ |
|
19 |
/* TODO: find a better way to handle rangefinder input |
|
20 |
* robot should deal with -1s (obstacles too close or too far to detect) |
|
21 |
* in a way that keeps it from crashing or driving in circles |
|
22 |
*/ |
|
23 |
// do not update distances when rangefinders return -1 |
|
24 |
// otherwise update distances with new rangefinder values |
|
25 |
int16_t temp; |
|
26 |
|
|
27 |
temp = range_read_distance(IR1); |
|
28 |
d1 = (temp == -1) ? d1 : temp; |
|
29 |
|
|
30 |
temp = range_read_distance(IR2); |
|
31 |
d2 = (temp == -1) ? d2 : temp; |
|
32 |
|
|
33 |
temp = range_read_distance(IR3); |
|
34 |
d3 = (temp == -1) ? d3 : temp; |
|
35 |
|
|
36 |
temp = range_read_distance(IR4); |
|
37 |
d4 = (temp == -1) ? d4 : temp; |
|
38 |
|
|
39 |
temp = range_read_distance(IR5); |
|
40 |
d5 = (temp == -1) ? d5 : temp; |
|
41 |
|
|
42 |
// update state based on rangefinder input |
|
43 |
if (d2 < IR2_DANGER) // avoid frontal collision by turning in place |
|
44 |
state = SRA_SPIN; |
|
45 |
/* nowhere to turn, so don't |
|
46 |
* will probably need to turn around soon, but not yet |
|
47 |
*/ |
|
48 |
else if (d1 < IR13_DANGER && d3 < IR13_DANGER) |
|
49 |
state = SRA_FORWARD; |
|
50 |
// avoid left-side collision by turning right |
|
51 |
else if (d1 < IR13_DANGER || d5 < IR45_DANGER) |
|
52 |
state = SRA_RIGHT; |
|
53 |
// avoid right-side collision by turning left |
|
54 |
else if (d3 < IR13_DANGER || d4 < IR45_DANGER) |
|
55 |
state = SRA_LEFT; |
|
56 |
else if (d2 < IR2_INTEREST) // should turn to avoid obstacle up ahead |
|
57 |
{ |
|
58 |
if (d3 >= d1) // more room on right |
|
59 |
state = SRA_RIGHT; |
|
60 |
else // more room on left |
|
61 |
state = SRA_LEFT; |
|
62 |
} |
|
63 |
else // no obstacles close by, so keep going straight |
|
64 |
state = SRA_FORWARD; |
|
65 |
|
|
66 |
/* Debugging via USB output */ |
|
67 |
/* usb_puts("IR1: "); */ |
|
68 |
/* usb_puti(d1); */ |
|
69 |
/* usb_puts(" IR2: "); */ |
|
70 |
/* usb_puti(d2); */ |
|
71 |
/* usb_puts(" IR3: "); */ |
|
72 |
/* usb_puti(d3); */ |
|
73 |
/* usb_puts(" IR4: "); */ |
|
74 |
/* usb_puti(d4); */ |
|
75 |
/* usb_puts(" IR5: "); */ |
|
76 |
/* usb_puti(d5); */ |
|
77 |
/* usb_puts("\n\r"); */ |
|
78 |
|
|
79 |
evaluate_state(); // take action on updated state |
|
80 |
} |
|
81 |
|
|
82 |
// behave according to current state |
|
83 |
// TODO: adjust speeds after testing |
|
84 |
void evaluate_state(void) |
|
85 |
{ |
|
86 |
switch (state) |
|
87 |
{ |
|
88 |
case SRA_FORWARD: |
|
89 |
orb_set_color(GREEN); |
|
90 |
move(FULL_SPD, 0); // drive straight forward |
|
91 |
break; |
|
92 |
case SRA_LEFT: |
|
93 |
orbs_set_color(GREEN, YELLOW); // green on left side |
|
94 |
move(HALF_SPD, NRML_TURN); // drive forward and to the left |
|
95 |
break; |
|
96 |
case SRA_RIGHT: |
|
97 |
orbs_set_color(YELLOW, GREEN); // green on right side |
|
98 |
move(HALF_SPD, -NRML_TURN); // drive forward and to the right |
|
99 |
break; |
|
100 |
case SRA_SPIN: |
|
101 |
orb_set_color(RED); |
|
102 |
move(0, NRML_TURN); // spin CCW without traveling |
|
103 |
break; |
|
104 |
default: // should never reach this |
|
105 |
orb_set_color(PURPLE); |
|
106 |
move(0, 0); // stop completely |
|
107 |
} |
|
108 |
} |
|
109 |
|
|
0 | 110 |
trunk/code/projects/hunter_prey/smart_run_around_fsm.h | ||
---|---|---|
1 |
// smart_run_around_fsm.h |
|
2 |
// required to run Smart Run Around functions |
|
3 |
// declare functions and global variables |
|
4 |
|
|
5 |
#ifndef _RUN_AROUND_FSM_H_ |
|
6 |
#define _RUN_AROUND_FSM_H_ |
|
7 |
|
|
8 |
// states (robot shall never move in reverse) |
|
9 |
// these constants may be changed (but can't overlap) without effect |
|
10 |
#define SRA_FORWARD 0 // drive straight forward |
|
11 |
#define SRA_LEFT 1 // drive forward and to the left |
|
12 |
#define SRA_RIGHT 2 // drive forward and to the right |
|
13 |
#define SRA_SPIN 3 // turn without traveling (last resort if otherwise stuck) |
|
14 |
|
|
15 |
/* conditions on rangefinders (in mm) informing state changes |
|
16 |
* distances are 50 greater than in real life, cannot be smaller than 100 |
|
17 |
* naming format: SENSOR_URGENCY |
|
18 |
* changing these constants affects robot operation |
|
19 |
*/ |
|
20 |
#define IR2_DANGER 150 // 10 cm: dangerously close to front |
|
21 |
#define IR13_DANGER 200 // 15 cm: dangerously close to sides (looking ahead) |
|
22 |
#define IR2_INTEREST 300 // 25 cm: notably close to front |
|
23 |
#define IR45_DANGER 150 // dangerously close to side (looking sideways) |
|
24 |
|
|
25 |
uint8_t state; // current state of FSM: SRA_FORWARD, SRA_LEFT, SRA_RIGHT, SRA_SPIN |
|
26 |
int16_t d1, d2, d3, d4, d5; // values returned by rangefinders |
|
27 |
|
|
28 |
// initialize rangefinders and global variables |
|
29 |
void run_around_init(void); |
|
30 |
|
|
31 |
/* evaluate rangefinder input and update state |
|
32 |
* call this function as often as possible to avoid collisions |
|
33 |
*/ |
|
34 |
void run_around_FSM(void); |
|
35 |
|
|
36 |
// behave according to current state |
|
37 |
void evaluate_state(void); |
|
38 |
|
|
39 |
#endif |
|
40 |
|
|
0 | 41 |
trunk/code/projects/libdragonfly/bom.c | ||
---|---|---|
79 | 79 |
#define MONK3 _PIN_E6 //blue |
80 | 80 |
#define MONK2 _PIN_E7 //white |
81 | 81 |
|
82 |
#define BOM_VALUE_THRESHOLD 200 |
|
82 |
#define BOM_VALUE_THRESHOLD 120 //200
|
|
83 | 83 |
#define NUM_BOM_LEDS 16 |
84 | 84 |
|
85 | 85 |
/* |
Also available in: Unified diff