Project

General

Profile

Revision 591

BFS worked in 2 out of many trials with 2 robots. However, it dropped wireless a lot and changes states more than it should.

View differences:

branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.c
1 1
#include <dragonfly_lib.h>
2 2
#include <wireless.h>
3 3
#include <wl_token_ring.h>
4
#include <queue.h>
4
#include "queue.h"
5 5
#include <string.h>
6 6
#include "bfs_fsm.h"
7 7
#include "smart_run_around_fsm.h"
......
17 17

  
18 18

  
19 19
/* private function prototypes */
20
void evaluate_state(void);
20
void bfs_evaluate_state(void);
21 21
int bfs_follow(void);
22 22

  
23 23

  
......
40 40
  
41 41
  bfs_pControl=0;
42 42
  bfs_bom = 0;
43
  bfs_bom_count = 0;
43 44

  
44 45
  
45 46
  /*Initialize distances to zero.*/ 
......
57 58
  wl_do(); // update wireless
58 59
  
59 60
    
60
  if (bfs_state == BFS_SEEK || bfs_state == BFS_FOLLOW) {
61
  if (bfs_state == BFS_SEEK) {
61 62
    bfs_follow_id = bfs_follow();
62
    if (bfs_follow_id == BFS_NO_VAL)
63
      bfs_state = BFS_SEEK; // move to seek state
63
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS)
64
      bfs_state = BFS_FOLLOW; // move to follow state
65
  }
66
  if (bfs_state == BFS_FOLLOW) {
67
    // get bom reading
68
    temp = wl_token_get_my_sensor_reading(bfs_follow_id);
69
    if (temp == -1) {
70
      bfs_bom_count++;
71
      if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
72
        bfs_state = BFS_SEEK;
73
        bfs_bom = 255;
74
        bfs_bom_count = 0;
75
      }
76
    }
64 77
    else {
65
      // found robot to follow to goal
66
      bfs_state = BFS_FOLLOW;
67
      
68
      // get bom reading
69
      temp = wl_token_get_my_sensor_reading(bfs_follow_id);
70
      bfs_bom = (temp == -1) ? bfs_bom : temp;
71
      
78
      bfs_bom = temp;
79
    
72 80
      // modify bom reading so right is negative, left is positive
73 81
      if (bfs_bom <= 12)
74 82
        bfs_bom -= 4;
......
99 107
  }
100 108
  
101 109
  // evaluate state
102
  evaluate_state();
110
  bfs_evaluate_state();
103 111
}
104 112

  
105 113

  
106 114
//Acts on state change.
107
void evaluate_state(){
115
void bfs_evaluate_state(){
108 116
    switch(bfs_state){
109 117
    case(BFS_SEEK): orb_set_color(RED);
110 118
      // move around
......
187 195
  memset(node_distances, large_number, BFS_MAX_ROBOTS);
188 196
  
189 197
  //set the path to all LARGE_NUMBER as well
190
  memset(path, large_number, BFS_MAX_ROBOTS);
198
  memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);
191 199

  
192 200
  //queue_remove_all(q);
193 201

  
branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.h
13 13

  
14 14
#define BFS_STRAIGHT_SPEED 160
15 15

  
16
#define BFS_ORBIT_DISTANCE 150 /* distance to start orbit around robot */
16
#define BFS_ORBIT_DISTANCE 120 /* distance to start orbit around robot */
17 17

  
18 18
#define BFS_MAX_ROBOTS 20 /* max id of robot in project */
19 19

  
......
31 31
int bfs_pControl;		/*Proportional control variable, determines turn direction.*/
32 32
int bfs_d1,bfs_d2,bfs_d3,bfs_d4,bfs_d5;	/*The five distances taken in by IR.*/
33 33
int bfs_bom; /* bom data */
34
int bfs_bom_count;
34 35

  
36
#define BFS_MAX_BOM_COUNT 5 /* number of missing bom packets before reverting to seek state */
37

  
35 38
/* bfs_init
36 39
   argument: robot_id that you want to find
37 40
   notes: must call before bfs_fsm
branches/orbit/code/behaviors/bfs_fsm/queue.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
 * @file queue.c
28
 * @brief Queue Implementation
29
 *
30
 * An implementation of a queue for the colony robots.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
35
#include "queue.h"
36
#include <wl_defs.h>
37

  
38
#include <stdlib.h>
39
#include <stdio.h>
40
#ifndef ROBOT
41
#include <pthread.h>
42
#endif
43

  
44
/**
45
 * @struct node_def
46
 * A node in the queue.
47
 **/
48
typedef struct node_def
49
{
50
	/** The item at this point in the queue. **/
51
	void* val;
52
	/** The next node in the queue. **/
53
	struct node_def* next;
54
} Node;
55

  
56
/**
57
 * Create a queue.
58
 *
59
 * @return the newly created queue.
60
 **/
61
Queue* queue_create()
62
{
63
	Queue* q = (Queue*)malloc(sizeof(Queue));
64
	
65
	if (q == NULL)
66
	{
67
		WL_DEBUG_PRINT("Out of memory.\r\n");
68
		return NULL;
69
	}
70
	
71
	q->head = NULL;
72
	q->size = 0;
73

  
74
	#ifndef ROBOT
75
	pthread_mutex_init(&(q->lock), NULL);
76
	#endif
77

  
78
	return q;
79
}
80

  
81
/**
82
 * Destroys a queue, freeing memory.
83
 *
84
 * @param q the queue to destroy
85
 **/
86
void queue_destroy(Queue* q)
87
{
88
	Node* t = q->head;
89
	while (t != NULL)
90
	{
91
		Node* t2 = t->next;
92
		free(t);
93
		t = t2;
94
	}
95

  
96
	#ifndef ROBOT
97
	pthread_mutex_destroy(&(q->lock));
98
	#endif
99

  
100
	free(q);
101
}
102

  
103
/**
104
 * Add an element to a queue.
105
 *
106
 * @param q the queue to add an element to
107
 * @param item the item to add to the queue
108
 **/
109
int queue_add(Queue* q, void* item)
110
{
111
	#ifndef ROBOT
112
	pthread_mutex_lock(&(q->lock));
113
	#endif
114

  
115
	Node* n = (Node*)malloc(sizeof(Node));
116
	if (n == NULL)
117
	{
118
		#ifndef ROBOT
119
		pthread_mutex_unlock(&(q->lock));
120
		#endif
121
		return -1;
122
	}
123

  
124
	n->val = item;
125
	n->next = NULL;
126
	
127
	if (q->head == NULL)
128
	{
129
		q->head = n;
130
		q->tail = n;
131
	}
132
	else
133
	{
134

  
135
		q->tail->next = n;
136
		q->tail = n;
137
	}
138
	
139
	q->size++;
140

  
141
	#ifndef ROBOT
142
	pthread_mutex_unlock(&(q->lock));
143
	#endif
144

  
145
	return 0;
146
}
147

  
148
/**
149
 * Remove an element from the front of a queue.
150
 *
151
 * @param q the queue to remove the element from
152
 *
153
 * @return the element which was removed
154
 **/
155
void* queue_remove(Queue* q)
156
{
157
	#ifndef ROBOT
158
	pthread_mutex_lock(&(q->lock));
159
	#endif
160
	
161
	Node* first = q->head;
162
	if (first == NULL)
163
	{
164
		WL_DEBUG_PRINT("Attempting to remove item \
165
			from empty queue.\r\n");
166
		WL_DEBUG_PRINT_INT(queue_size(q));
167
		#ifndef ROBOT
168
		pthread_mutex_unlock(&(q->lock));
169
		#endif
170
		return NULL;
171
	}
172
	void* ret = first->val;
173
	q->head = first->next;
174
	if (q->tail == first)
175
		q->tail = NULL;
176
	free(first);
177
	q->size--;
178

  
179
	#ifndef ROBOT
180
	pthread_mutex_unlock(&(q->lock));
181
	#endif
182

  
183
	return ret;
184
}
185

  
186
/**
187
 * Remove all instances of a given element from a queue.
188
 *
189
 * @param q the queue to remove the elements from
190
 * @param item the element to remove all instances of
191
 **/
192
void queue_remove_all(Queue* q, void* item)
193
{
194
	#ifndef ROBOT
195
	pthread_mutex_lock(&(q->lock));
196
	#endif
197

  
198
	Node* curr = q->head;
199
	Node* prev = NULL;
200
	
201
	while (curr != NULL)
202
	{
203
		Node* next = curr->next;
204
		if (curr->val == item)
205
		{
206
			if (q->head == curr)
207
				q->head = next;
208
			if (q->tail == curr)
209
				q->tail = prev;
210
			if (prev != NULL)
211
				prev->next = next;
212
			free(curr);
213
			q->size--;
214
		}
215
		else
216
			prev = curr;
217
		curr = next;
218
	}
219
	
220
	#ifndef ROBOT
221
	pthread_mutex_unlock(&(q->lock));
222
	#endif
223
}
224

  
225
/**
226
 * Get the number of elements in the queue.
227
 *
228
 * @param q the queue to get the size of
229
 * @return the size of the queue
230
 **/
231
int queue_size(Queue* q)
232
{
233
	int size;
234

  
235
	#ifndef ROBOT
236
	pthread_mutex_lock(&(q->lock));
237
	#endif
238

  
239
	size = q->size;
240
 
241
	#ifndef ROBOT
242
	pthread_mutex_unlock(&(q->lock));
243
	#endif
244

  
245
	return size;
246
}
247

  
248
/**
249
 * Check if the queue is empty.
250
 * 
251
 * @param q the queue to check
252
 * @return nonzero if the queue is empty, 0 otherwise
253
 **/
254
int queue_is_empty(Queue* q)
255
{
256
	int result;
257

  
258
	#ifndef ROBOT
259
	pthread_mutex_lock(&(q->lock));
260
	#endif
261

  
262
	result = q->size == 0;
263

  
264
	#ifndef ROBOT
265
	pthread_mutex_unlock(&(q->lock));
266
	#endif
267

  
268
	return result;
269
}
270

  
branches/orbit/code/behaviors/bfs_fsm/smart_run_around_fsm.c
7 7
Latest revision only has two accessible states: move and reverse.
8 8
*/
9 9

  
10
/* private function */
11
void run_around_evaluate_state(void);
10 12

  
11 13
void run_around_init(void)
12 14
{
13
  range_init();
14
  analog_init(1);
15
  motors_init();
16
  orb_init();
17
  orb_enable();
15
  //range_init();
16
  //analog_init(1);
17
  //motors_init();
18
  //orb_init();
19
  //orb_enable();
18 20
  usb_init();
19 21
 
20 22
  /*Start in the default state, MOVING*/ 
......
27 29
  /*Initialize distances to zero.*/ 
28 30
  d1=0; d2=0; d3=0; d4=0; d5=0;
29 31
  
30
  orb_set_color(GREEN);
32
  //orb_set_color(GREEN);
31 33

  
32 34
}
33 35

  
......
63 65
    
64 66
    if(crazy_count<0) crazy_count=CRAZY_MAX;
65 67
    
66
    evaluate_state();
68
    run_around_evaluate_state();
67 69
    return;
68 70
  }
69 71
  
......
71 73
  if((d2!=-1)&&(d2 < 150)){
72 74
      backup_count=BACKUP_MAX;
73 75
      avoid_state=BACKWARDS;
74
      evaluate_state();
76
      run_around_evaluate_state();
75 77
      return;
76 78
  }
77 79
  /*
78 80
  if(d1 < 120 || d3 < 120) {
79 81
		avoid_state = BACKWARDS;
80 82
		backup_count = BACKUP_MAX;
81
		evaluate_state();
83
		run_around_evaluate_state();
82 84
		return;
83 85
  }
84 86
  */
......
86 88
    avoid_state=BACKWARDS; 
87 89
    if(backup_count<0)
88 90
      backup_count=BACKUP_MAX;
89
    evaluate_state();
91
    run_around_evaluate_state();
90 92
    return;
91 93
  }
92 94
  
......
112 114
  usb_puti(d5);
113 115
  usb_puts("\n\r");*/
114 116
  
115
  evaluate_state();
117
  run_around_evaluate_state();
116 118
}
117 119

  
118 120

  
119 121
//Acts on state change.
120
void evaluate_state(){
122
void run_around_evaluate_state(){
121 123
    switch(avoid_state){
122
    case(MOVING): orb_set_color(GREEN);
124
    case(MOVING): //orb_set_color(GREEN);
123 125
      move(STRAIT_SPEED,-pControl);
124 126
      break;
125 127
    
126
    case(BACKWARDS): orb_set_color(ORANGE);
128
    case(BACKWARDS): //orb_set_color(ORANGE);
127 129
      move(-STRAIT_SPEED,0);
128 130
      break;
129 131
      
130
    case(CRAZY): orb_set_color(RED);
132
    case(CRAZY): //orb_set_color(RED);
131 133
      /*TODO: Implement a crazy state.*/
132 134
      move(STRAIT_SPEED,-pControl);
133 135
      break;
134 136
      
135 137
    default:
136 138
      /*Should never get here, go strait.*/
137
      move(100,0); orb_set_color(BLUE);
139
      move(100,0); //orb_set_color(BLUE);
138 140
      break;
139 141
  }
140 142
}
branches/orbit/code/behaviors/bfs_fsm/queue.h
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
 * @file queue.h
28
 * @brief A queue implementation
29
 * 
30
 * Implements a queue, a first in, first out data structure.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
35
#ifndef WIRELESS_QUEUE_H
36
#define WIRELESS_QUEUE_H
37

  
38
#ifndef ROBOT
39
#include <pthread.h>
40
#endif
41

  
42
struct node_def;
43

  
44
/**
45
 * @defgroup queue Queue
46
 * @brief A queue implementation
47
 * 
48
 * A queue implementation.
49
 *
50
 * @{
51
 **/
52

  
53
/**
54
 * @struct Queue
55
 * Represents a queue, a first in, first out data structure.
56
 **/
57
typedef struct
58
{
59
	/**
60
	 * The head of the queue, the next item to be removed.
61
	 **/
62
	struct node_def* head;
63
	/**
64
	 * The tail of the queue, the last item added.
65
	 **/
66
	struct node_def* tail;
67
	/**
68
	 * The number of elements in the queue.
69
	 **/
70
	int size;
71
	
72
#ifndef ROBOT
73
	pthread_mutex_t lock;
74
#endif
75
} Queue;
76

  
77
/** @brief Create a new queue **/
78
Queue* queue_create(void);
79
/** @brief Destroy a queue **/
80
void queue_destroy(Queue* q);
81
/** @brief Add an element to a queue **/
82
int queue_add(Queue* q, void* item);
83
/** @brief Remove an element from a queue **/
84
void* queue_remove(Queue* q);
85
/** @brief Remove all instances of a given element from a queue **/
86
void queue_remove_all(Queue* q, void* item);
87
/** @brief Get the size of a queue **/
88
int queue_size(Queue* q);
89
/** @brief Check if the queue is empty **/
90
int queue_is_empty(Queue* q);
91

  
92
/** @} **/
93

  
94

  
95
#endif
branches/orbit/code/behaviors/bfs_fsm/smart_run_around_fsm.h
28 28

  
29 29
void run_around_init(void);
30 30
void run_around_FSM(void);
31
void evaluate_state(void);
32 31

  
33 32
#endif

Also available in: Unified diff