Revision 1848

View differences:

trunk/code/behaviors/hunter_prey/james/main.c
1
/*HUNTER_PREY by jamesCarroll 10/26/10*/
1 2
#include <dragonfly_lib.h>
2 3
#include <wl_basic.h>
3 4
#include "hunter_prey.h"
......
2 3

  
3
#define WL_CHANNEL 15
4
#define HUNTER 3
5
#define PREY 1
6
#define PREHUNTER 2
7
#define PREPREY 0
8
#define WAITING 4
4
#define WL_CHANNEL 15 
9 5

  
......
14 10
int data_length;
15 11

  
16 12
/*  Data buffer used to send packets */
17
char send_buffer[2];
13
char send_buffer[3];
18 14

  
19 15
int main(void)
20 16
{
21 17

  
22
	/* Initialize dragonfly board */
23
	dragonfly_init(ALL_ON);
24
	/* Initialize the XBEE */
25
	xbee_init();
26
	/* Initialize the basic wireless library */
27
	wl_basic_init_default();
28
	/* Set the XBee channel to your assigned channel */
29
	wl_set_channel(WL_CHANNEL);
30
	/* Initialize Orbs*/
31
	orb_init();
32
	/* Initialize Clock*/
33
	rtc_init(SIXTEENTH_SECOND, NULL);
34
	/* Initialize motors*/
35
	motors_init();
18
    /* Initialize dragonfly board */
19
    dragonfly_init(ALL_ON);
20
    /* Initialize the basic wireless library */
21
    wl_basic_init_default();
22
    /* Set the XBee channel to your assigned channel */
23
    wl_set_channel(WL_CHANNEL);
36 24

  
37
	int state=PREPREY, time, max_bom_reading, front_rangefinder_reading, angle, d1=80, d2=80, d3=80, d4=80, d5=80, speed=0;
38
	unsigned char id=get_robotid(), tag;
25
    /* ****** CODE HERE ******* */
26
	int yes= 1;
27
	int no = 3;
28
	int speed = 200;
29
	int isHunter = yes;
30
	int t1 = 0;
31
	int t2 = 0;
32
	int timechecked = 0;
33
	int beginning = 1;
34
	send_buffer[2] = get_robotid();
35
	if(wheel()<100)
36
		isHunter=yes;
37
	else
38
		isHunter=no;
39 39

  
40
	if(wheel() < 100)
41
		state=HUNTER;
42

  
43
	while(1){
44
		switch(state)
40
	while(1)
41
	{
42
		if(isHunter == yes)
45 43
		{
46
			case PREPREY:
44
			bom_off();
45
			orb_set_color(RED);
46
			while(1)
47
			{
48
				int front = 0;
49
				front = range_read_distance(IR2);
50
				bom_refresh(BOM_ALL);
51
				int bomNum = 0;
52
				bomNum = bom_get_max();
53
				if(hunter_prey_tagged(bomNum, front))
47 54
				{
48
					orb1_set_color(YELLOW);
49
					bom_on();
50
					state=PREY;
51
					motor_r_set(BACKWARD,200);
52
					motor_l_set(BACKWARD,200);
53
					delay_ms(2000);
54
					while(wl_basic_do_default(&data_length));
55
					orb1_set_color(GREEN);
56
					rtc_reset();
57
					break;
55
					send_buffer[0] = HUNTER_PREY_ACTION_TAG;
56
					send_buffer[1] = get_robotid();
57
					wl_basic_send_global_packet(42, send_buffer, 3);
58 58
				}
59
			case PREY:
59
				packet_data = wl_basic_do_default(&data_length);
60
				if(packet_data != 0)
60 61
				{
61
					int t1 = 0;
62
					int t2 = 0;
63
					int timechecked = 0;
64
					int beginning = 1;
65
					orb1_set_color(GREEN);
66
					orb2_set_color(GREEN);
67

  
68
					while(1)
62
					if(data_length == 3 && packet_data[0] == HUNTER_PREY_ACTION_ACK)
69 63
					{
70
						int front = range_read_distance(IR2);
71
						packet_data = wl_basic_do_default(&data_length);
72
						if(packet_data != 0)
64
						if(packet_data[1] == get_robotid())
73 65
						{
74
							if(data_length == 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
75
							{
76
								send_buffer[0] = HUNTER_PREY_ACTION_ACK;
77
								send_buffer[1] = packet_data[1];
78
								wl_basic_send_global_packet(42, send_buffer, 2);
79
								state=PREHUNTER;
80
								motor_r_set(0,0);motor_l_set(0,0);
81
								break;
82
							}
66
							orb_set_color(GREEN);
67
							isHunter = no;
68
							motor_l_set(1, 200);
69
							motor_r_set(0,200);delay_ms(1000);break;
83 70
						}
84
						int side1 = range_read_distance(IR1);
85
						int side2 = range_read_distance(IR3);
86
						t2 = rtc_get();
87
						if(front < 200 && front > 50 && !(timechecked == 1))
88
						{
89
							orb2_set_color(RED);
90
							timechecked = 1;
91
							t1 = rtc_get();
92
							motor_l_set(1, 255);
93
							motor_r_set(0, 0);
71
						else{
72
							orb_set_color(BLUE);motor_r_set(0,0);motor_l_set(0,0);delay_ms(3000);break;
94 73
						}
95
						else if(side1 < 300 && side1 > 0 && timechecked != 1)
96
						{
97
							motor_r_set(0,0);
98
						}
99
						else if(side2 < 300 && side2 > 0 && timechecked != 1)
100
						{
101
							motor_l_set(0,0);
102
						}
103
						else
104
						{
105
							if(t2 - t1 > 2 || beginning == 1)
106
							{
107
								orb2_set_color(BLUE);
108
								timechecked = 0;
109
								motor_l_set(1, 255);
110
								motor_r_set(1, 255);
111
								beginning = 0;
112
							}
113
						}
114 74
					}
115
					break;
116 75
				}
117
			case PREHUNTER: /*new hunter mode (5 second pause)*/
118
				{
119
					orb1_set_color(BLUE);
120
					orb2_set_color(BLUE);
121
					motors_off();
122
					state=HUNTER;
123
					rtc_reset();
124
					bom_off();
125
					delay_ms(3000);		
126
					while(wl_basic_do_default(&data_length));
127
					orb1_set_color(RED);
128
					break;
76
				bom_refresh(BOM_ALL);
77
				bomNum = bom_get_max();
78
				if(bomNum == 4)
79
				{	
80
					motor_l_set(1, speed);
81
					motor_r_set(1, speed);
129 82
				}
130
			case HUNTER:
83
				else
131 84
				{
132
					orb1_set_color(RED);
133
					orb2_set_color(RED);
134
					/*HUNT*/
135
					angle=0; speed=FULL_SPD;
136
					if(max_bom_reading==4){
137
						motor_l_set(FORWARD, 255);
138
						motor_r_set(FORWARD, 255);
85
					if(bomNum == -1){}
86
					else if((bomNum >= 12) || (bomNum < 4))
87
					{
88
						motor_l_set(FORWARD, speed);
89
						motor_r_set(BACKWARD, speed);
139 90
					}
140
					if(max_bom_reading>=1 && max_bom_reading<=3){
141
						motor_l_set(FORWARD, 255);
142
						motor_r_set(FORWARD, 190);
91
					else
92
					{
93
						motor_l_set(BACKWARD, speed);
94
						motor_r_set(FORWARD, speed);
143 95
					}
144
					if(max_bom_reading>=5 && max_bom_reading<=7){
145
						motor_l_set(FORWARD, 190);
146
						motor_r_set(FORWARD, 255);
96
				}
97
			}
98
		}
99
		else
100
		{
101
			orb_set_color(GREEN);
102
			bom_on();
103
			while(1)
104
			{
105
				int front = range_read_distance(IR2);
106
				packet_data = wl_basic_do_default(&data_length);
107
				if(packet_data != 0)
108
				{	
109
					if(data_length == 3 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
110
					{
111
						send_buffer[0] = HUNTER_PREY_ACTION_ACK;
112
						send_buffer[1] = packet_data[1];
113
						wl_basic_send_global_packet(42, send_buffer, 3);
114
						isHunter=yes;
115
						orb_set_color(BLUE);
116
						motor_r_set(0,0);motor_l_set(0,0);
117
						bom_off();
118
						delay_ms(3000);
119
						break;
147 120
					}
148
					if((max_bom_reading>=8 && max_bom_reading<=11) || max_bom_reading<0){
149
						motor_l_set(BACKWARD, 170);
150
						motor_r_set(FORWARD, 255);
151
						orb2_set_color(RED);
152
					}
153
					if((max_bom_reading>=12 && max_bom_reading<=15) || max_bom_reading==0){
154
						motor_l_set(FORWARD, 255);
155
						motor_r_set(BACKWARD, 170);
156
					}
157
					/*Tagging*/
158
					packet_data = wl_basic_do_default(&data_length);
159
					if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]!=id){
160
						state=PREHUNTER;
161
					}
162
					bom_refresh(BOM_ALL);  
163
					front_rangefinder_reading = -1;
164
					while(front_rangefinder_reading == -1){
165
						front_rangefinder_reading = range_read_distance(IR2);
166
					}
167
					delay_ms(10);
168
					max_bom_reading = bom_get_max();
169
					tag = hunter_prey_tagged(max_bom_reading, front_rangefinder_reading);  
170
					time = rtc_get();
171
					if(button2_read()==1 || (tag && time>16)){
172
						send_buffer[0]=HUNTER_PREY_ACTION_TAG;
173
						send_buffer[1]=id;
174
						wl_basic_send_global_packet( 42, send_buffer, 2 );
175
						rtc_reset();
176
						state=WAITING;
177
						orb1_set_color(PURPLE);
178
					}
179
					break;
180 121
				}
181
			case WAITING: /*Waiting for ack after sending tag*/
122
				int side1 = range_read_distance(IR1);
123
				int side2 = range_read_distance(IR3);
124
				t2 = rtc_get();
125
				if(front < 200 && front > 50 && !(timechecked == 1))
182 126
				{
183
					time = rtc_get();
184
					packet_data = wl_basic_do_default(&data_length);
185
					if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]==id){
186
						state=PREPREY;
187
						rtc_reset();
127
					orb2_set_color(RED);
128
					timechecked = 1;
129
					t1 = rtc_get();
130
					motor_l_set(1, speed);
131
					motor_r_set(0, 0);
132
				}
133
				else if(side1 < 300 && side1 > 0 && timechecked != 1)
134
				{
135
					motor_r_set(0,0);
136
				}
137
				else if(side2 < 300 && side2 > 0 && timechecked != 1)
138
				{
139
					motor_l_set(0,0);
140
				}
141
				else
142
				{
143
					if(t2 - t1 > 2 || beginning == 1)
144
					{
145
						orb2_set_color(BLUE);
146
						timechecked = 0;
147
						motor_l_set(1, speed);
148
						motor_r_set(1, speed);
149
						beginning = 0;
188 150
					}
189
					if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]!=id){
190
						state=PREHUNTER;
191
					}
192
					if(time>16){
193
						state=HUNTER;
194
						orb1_set_color(RED);
195
					}
196
					break;
197 151
				}
152
			}
198 153
		}
199 154
	}
200 155

  
201
	while(1);
202 156

  
203
	return 0;
157
    /* ****** END HERE ******* */
204 158

  
159
    while(1);
160

  
161
    return 0;
162

  
205 163
}
206 164

  

Also available in: Unified diff