Project

General

Profile

Revision 672

merged orbit branch (orbit, bfs) with trunk

View differences:

trunk/code/behaviors/bfs_fsm/bfs_fsm.c
1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_token_ring.h>
4
#include "queue.h"
5
#include <string.h>
6
#include "bfs_fsm.h"
7
#include "smart_run_around_fsm.h"
8
#include "orbit_fsm.h"
9

  
10
/* Used to find a robot (or other object)
11

  
12
  Uses bom and token ring
13
  
14
  Assumes:
15
    robots are already in a token ring
16
*/
17

  
18

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

  
23

  
24
/* init */
25
void bfs_init(int robot) {
26
  range_init();
27
  analog_init(1);
28
  motors_init();
29
  orb_init();
30
  orb_enable();
31
  usb_init();
32
  run_around_init();
33
 
34
  /*Start in the start state, BFS_SEEK */ 
35
  bfs_state = BFS_SEEK;
36
  
37
  bfs_otherRobot = robot; // set which robot to seek
38
  bfs_my_id = wl_get_xbee_id();
39
  bfs_follow_id = -1;
40
  
41
  bfs_pControl=0;
42
  bfs_bom = 0;
43
  bfs_bom_count = 0;
44

  
45
  
46
  /*Initialize distances to zero.*/ 
47
  bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000;
48
  
49
}
50

  
51
/*The main function, call this to update states as frequently as possible.*/
52
void bfs_fsm(void) {
53
  
54
  /*The following lines ensure that undefined (-1) values
55
  will not update the distances.*/ 
56
  int temp;  
57
    
58
  wl_do(); // update wireless
59
  usb_puts("\n\rwl_do");
60
  usb_puts("|state=");
61
  usb_puti(bfs_state);
62
  usb_puts("|");
63
  
64
    
65
  if (bfs_state == BFS_SEEK) {
66
    bfs_follow_id = bfs_follow();
67
    wl_do(); // update wireless
68
    usb_puti(bfs_follow_id);
69
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
70
      bfs_state = BFS_FOLLOW; // move to follow state
71
      bfs_d2=1000;
72
    }
73
  }
74
  if (bfs_state == BFS_FOLLOW) {
75
    // get bom reading
76
    temp = wl_token_get_my_sensor_reading(bfs_follow_id);
77
    if (temp == -1 || temp > 16) {
78
      bfs_bom_count++;
79
      if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
80
        bfs_state = BFS_SEEK;
81
        bfs_bom = 255;
82
        bfs_bom_count = 0;
83
      }
84
    }
85
    else {
86
      bfs_bom = temp;
87
          
88
      // modify bom reading so right is negative, left is positive
89
      if (bfs_bom <= 12)
90
        bfs_bom -= 4;
91
      else
92
        bfs_bom -= 20;
93
      
94
      bfs_pControl = bfs_bom*4;
95
      usb_puti(bfs_bom);
96
      
97
      
98
      // get range reading for front
99
      temp=range_read_distance(IR2);
100
      bfs_d2=(temp == -1) ? bfs_d2 : temp;
101
      usb_puti(bfs_d2);
102
      
103
      if (bfs_d2 < BFS_ORBIT_DISTANCE) {
104
        bfs_state = BFS_ORBIT; // move to orbit state
105
        orbit_init(bfs_follow_id);
106
        bfs_d2=1000;
107
      }
108
    }  
109
  }
110
  if (bfs_state == BFS_ORBIT) {   
111
    usb_puts("orbiting\n");
112
    while(orbit_state != ORBIT_STOP)
113
    { orbit_fsm(); }
114
    bfs_state = BFS_STOP; // move to stop state  
115
  }
116
  
117
  // evaluate state
118
  bfs_evaluate_state();
119
}
120

  
121

  
122
//Acts on state change.
123
void bfs_evaluate_state(){
124
    switch(bfs_state){
125
    case(BFS_SEEK): orb_set_color(RED);
126
      // move around
127
      run_around_FSM(); // note: better to just incorporate into this file later one
128
      break;
129
    
130
    case(BFS_FOLLOW): orb_set_color(GREEN);
131
      // follow another robot
132
      move(BFS_STRAIGHT_SPEED,bfs_pControl);
133
      break;
134
      
135
    case(BFS_ORBIT): //orb_set_color(BLUE);
136
      // orbit a robot
137
      //orbit_fsm();
138
      //move(0,0);
139
      break;
140
      
141
    case(BFS_STOP): orb_set_color(YELLOW);
142
      // stop
143
      move(0,0);
144
      break;
145
      
146
    default: orb_set_color(MAGENTA);
147
      /*Should never get here, so stop.*/
148
      move(0,0);
149
      break;
150
  }
151
}
152

  
153
/* find a robot to follow using BFS 
154
  ported from colonybfs by Felix
155
*/
156
int bfs_follow()
157
{
158
  
159
/* pseudocode for BFS
160

  
161
procedure bfs(v)
162
    q := make_queue()
163
    enqueue(q,v)
164
    mark v as visited
165
    while q is not empty
166
        v = dequeue(q)
167
        process v
168
        for all unvisited vertices v' adjacent to v
169
            mark v' as visited
170
            enqueue(q,v')
171
*/
172

  
173
  Queue* q = queue_create();
174

  
175
  //int num_current_robots = wl_token_get_robots_in_ring();
176

  
177
  // keep track of which nodes you have visited.  Indexed by robot #
178
  // 1 if visited, 0 if not
179
  unsigned char visited_nodes[BFS_MAX_ROBOTS];
180
  // keep track of the distance from the start robot to other robots
181
  // also indexed by robot#
182
  unsigned char node_distances[BFS_MAX_ROBOTS];
183
  // this is the path you take
184
  unsigned char path[BFS_MAX_ROBOTS];
185
    
186
  //variables you will need
187
  unsigned char current_node;                 //current node
188
  unsigned char current_neighbour_node;       //neighbor to the current node
189
  unsigned char current_neighbour_val;        //value of that neighbour's sensors
190
  unsigned char current_distance;              //keep track of current distance to the start
191
    
192
  unsigned char large_number = 255;
193
  
194
  unsigned char* add_value;
195
  
196
  
197

  
198
  //set visited nodes to all 0
199
  memset(visited_nodes, 0, BFS_MAX_ROBOTS);
200
  
201
  //set all the distances to a very large number
202
  //this isn't technically necessary, but it's probably a good thing -- just in case
203
  memset(node_distances, large_number, BFS_MAX_ROBOTS);
204
  
205
  //set the path to all LARGE_NUMBER as well
206
  memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);
207

  
208
  //queue_remove_all(q);
209

  
210
  //add the start node
211
  add_value = (unsigned char*)malloc(sizeof(char));
212
  (*add_value) = bfs_my_id;
213
  queue_add(q, add_value);
214
  visited_nodes[bfs_my_id] = 1;
215
  node_distances[bfs_my_id] = 0;
216
  
217
  while(!queue_is_empty(q)){
218
    add_value = queue_remove(q);
219
    current_node = (*add_value);
220
    
221
    //get the current distance from you to the start
222
    current_distance = node_distances[current_node];
223
    
224
    //this node is on your 'path'
225
    path[current_distance] = current_node;
226
    //note: it's OK if this path leads nowhere -- the path will be
227
    //overwritten by a node that does lead to the goal
228
    //(if there is no path, we set path to null anyways)
229
    
230
    //from now on, all further nodes will be one further away -- increment
231
    current_distance++;
232
    
233
    
234
    //process node -- basically check if it's the goal
235
    if(current_node == bfs_otherRobot) {
236
      //you reach the goal
237
      //return the first robot in the path, which is the one
238
      //to follow
239
      /*
240
      lcd_putchar('.');
241
      lcd_putchar('.');
242
      for(i = 0; i < MAX_ROBOTS; i++)
243
          lcd_putchar(path[i] +48);           //print out the path for fun -- remove this later
244
      lcd_putchar('.');
245
      lcd_putchar('.');
246
      */
247
      return path[1];         //path[0] is you.  path[1] is who you want to follow
248
    }
249
    
250
    // go through all nodes adjacent to current
251
    // in effect, this means going through the current node's row in the matrix
252
    
253
    wl_token_iterator_begin();
254
    while(wl_token_iterator_has_next()) {
255
      //the robot # is actually just the index
256
      current_neighbour_node = wl_token_iterator_next();
257
      //the value is what is stored in the matrix (this is important)
258
      current_neighbour_val = wl_token_get_sensor_reading(current_node,current_neighbour_node);
259
      
260
      //check for connected-ness and that it was not visited
261
      //            unconnected                           visited
262
      if( (current_neighbour_val == BFS_NO_VAL) || (visited_nodes[current_neighbour_node]) ) {
263
        //if it is either unconnected or previously visited, exit
264
        continue;   //go to the next node
265
      }
266
      
267
      //update the distance from the start node to this node
268
      node_distances[current_neighbour_node] = current_distance;
269
      //also mark it as visited
270
      visited_nodes[current_neighbour_node] = 1;
271
      
272
      //also enqueue each neighbour
273
      add_value = (unsigned char*)malloc(sizeof(char));
274
      (*add_value) = current_neighbour_node;
275
      queue_add(q, add_value);
276
    
277
    }
278
  }
279
  
280
  //if we get to here, there is no path
281
  memset(path, 0, BFS_MAX_ROBOTS);
282
  return BFS_NO_VAL;
283

  
284
}
285

  
286

  
trunk/code/behaviors/bfs_fsm/decoy/decoy.c
1
/** driver for orbit code
2
	sit and activate bom, let other robot orbit this
3
*/
4

  
5
#include <dragonfly_lib.h>
6
#include <wireless.h>
7
#include <wl_token_ring.h>
8

  
9

  
10

  
11
int main(void) {
12
  // enable everything
13
  dragonfly_init(ALL_ON);
14
  orb_enable();
15
  orb_init();
16
  orb_set_color(PURPLE);
17
  wl_init();
18
  wl_token_ring_register();
19
  wl_token_ring_join(); // join token ring
20
  usb_init();
21
  usb_puts("start");
22
  usb_puti(wl_get_xbee_id());
23
  usb_puts("end");
24
 
25
  
26
  
27
  while(1) {
28
    wl_do();
29
  }
30
  
31
  orb_set_color(RED);
32

  
33
  return 0;
34
}
trunk/code/behaviors/bfs_fsm/decoy/Makefile
1
########Update This Section########
2
#
3
#
4

  
5
# Relative path to the root directory (containing lib directory)
6
ifndef COLONYROOT
7
COLONYROOT = ../../../..
8
endif
9

  
10
# Target file name (without extension).
11
TARGET = template
12

  
13
# Uncomment this to use the wireless library
14
USE_WIRELESS = 1
15

  
16
# com1 = serial port. Use lpt1 to connect to parallel port.
17
AVRDUDE_PORT = com1
18
#/dev/tty.usbserial*
19
#
20
#
21
###################################
22

  
23
# Hey Emacs, this is a -*- makefile -*-
24
#----------------------------------------------------------------------------
25
# WinAVR Makefile Template written by Eric B. Weddington, J?rg Wunsch, et al.
26
#
27
# Released to the Public Domain
28
#
29
# Additional material for this makefile was written by:
30
# Peter Fleury
31
# Tim Henigan
32
# Colin O'Flynn
33
# Reiner Patommel
34
# Markus Pfaff
35
# Sander Pool
36
# Frederik Rouleau
37
#
38
#----------------------------------------------------------------------------
39
# On command line:
40
#
41
# make all = Make software.
42
#
43
# make clean = Clean out built project files.
44
#
45
# make coff = Convert ELF to AVR COFF.
46
#
47
# make extcoff = Convert ELF to AVR Extended COFF.
48
#
49
# make program = Download the hex file to the device, using avrdude.
50
#                Please customize the avrdude settings below first!
51
#
52
# make debug = Start either simulavr or avarice as specified for debugging, 
53
#              with avr-gdb or avr-insight as the front end for debugging.
54
#
55
# make filename.s = Just compile filename.c into the assembler code only.
56
#
57
# make filename.i = Create a preprocessed source file for use in submitting
58
#                   bug reports to the GCC project.
59
#
60
# To rebuild project do "make clean" then "make all".
61
#----------------------------------------------------------------------------
62

  
63
#if you want your code to work on the Firefly++ and not Firefly+
64
#then add the -DFFPP line to CDEFS
65
CDEFS = 
66
#-DFFPP
67

  
68
# MCU name
69
MCU = atmega128
70

  
71
# Processor frequency.
72
#     This will define a symbol, F_CPU, in all source code files equal to the 
73
#     processor frequency. You can then use this symbol in your source code to 
74
#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done
75
#     automatically to create a 32-bit value in your source code.
76
F_CPU = 8000000
77

  
78
# Output format. (can be srec, ihex, binary)
79
FORMAT = ihex
80

  
81
# List C source files here. (C dependencies are automatically generated.)
82
SRC = $(wildcard *.c)
83

  
84
# List Assembler source files here.
85
#     Make them always end in a capital .S.  Files ending in a lowercase .s
86
#     will not be considered source files but generated files (assembler
87
#     output from the compiler), and will be deleted upon "make clean"!
88
#     Even though the DOS/Win* filesystem matches both .s and .S the same,
89
#     it will preserve the spelling of the filenames, and gcc itself does
90
#     care about how the name is spelled on its command-line.
91
ASRC = 
92

  
93
# Optimization level, can be [0, 1, 2, 3, s]. 
94
#     0 = turn off optimization. s = optimize for size.
95
#     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
96
OPT = s
97

  
98
# Debugging format.
99
#     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
100
#     AVR Studio 4.10 requires dwarf-2.
101
#     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.
102
DEBUG = 
103

  
104
# Compiler flag to set the C Standard level.
105
#     c89   = "ANSI" C
106
#     gnu89 = c89 plus GCC extensions
107
#     c99   = ISO C99 standard (not yet fully implemented)
108
#     gnu99 = c99 plus GCC extensions
109
CSTANDARD = -std=gnu99
110

  
111
# Place -D or -U options here
112
CDEFS += -DF_CPU=$(F_CPU)UL 
113
CDEFS += -DFFP
114
# for wireless library
115
ifdef USE_WIRELESS
116
	CDEFS += -DROBOT
117
endif
118

  
119
# Place -I, -L options here
120
CINCS = -I$(COLONYROOT)/code/lib/include/libdragonfly 
121
CINCS += -L$(COLONYROOT)/code/lib/bin
122
ifdef USE_WIRELESS
123
	CINCS += -I$(COLONYROOT)/code/lib/include/libwireless #-I$(COLONYROOT)/code/projects/colonet/lib -I$(COLONYROOT)/code/projects/colonet/lib/colonet_dragonfly
124
endif
125

  
126
#---------------- Compiler Options ----------------
127
#  -g*:          generate debugging information
128
#  -O*:          optimization level
129
#  -f...:        tuning, see GCC manual and avr-libc documentation
130
#  -Wall...:     warning level
131
#  -Wa,...:      tell GCC to pass this to the assembler.
132
#    -adhlns...: create assembler listing
133
CFLAGS =
134
# CFLAGS = -g$(DEBUG)
135
CFLAGS += $(CDEFS) $(CINCS)
136
CFLAGS += -O$(OPT)
137
CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums
138
CFLAGS += -Wall -Wstrict-prototypes
139
CFLAGS += -Wa,-adhlns=$(<:.c=.lst)
140
CFLAGS += $(CSTANDARD)
141

  
142
#---------------- Assembler Options ----------------
143
#  -Wa,...:   tell GCC to pass this to the assembler.
144
#  -ahlms:    create listing
145
#  -gstabs:   have the assembler create line number information; note that
146
#             for use in COFF files, additional information about filenames
147
#             and function names needs to be present in the assembler source
148
#             files -- see avr-libc docs [FIXME: not yet described there]
149
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs 
150

  
151

  
152
#---------------- Library Options ----------------
153
# Minimalistic printf version
154
PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min
155

  
156
# Floating point printf version (requires MATH_LIB = -lm below)
157
PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt
158

  
159
# If this is left blank, then it will use the Standard printf version.
160
PRINTF_LIB = 
161
#PRINTF_LIB = $(PRINTF_LIB_MIN)
162
#PRINTF_LIB = $(PRINTF_LIB_FLOAT)
163

  
164

  
165
# Minimalistic scanf version
166
SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min
167

  
168
# Floating point + %[ scanf version (requires MATH_LIB = -lm below)
169
SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt
170

  
171
# If this is left blank, then it will use the Standard scanf version.
172
SCANF_LIB = 
173
#SCANF_LIB = $(SCANF_LIB_MIN)
174
#SCANF_LIB = $(SCANF_LIB_FLOAT)
175

  
176
MATH_LIB = -lm
177

  
178
#---------------- External Memory Options ----------------
179

  
180
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
181
# used for variables (.data/.bss) and heap (malloc()).
182
#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff
183

  
184
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
185
# only used for heap (malloc()).
186
#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff
187

  
188
EXTMEMOPTS =
189

  
190
#---------------- Linker Options ----------------
191
#  -Wl,...:     tell GCC to pass this to linker.
192
#    -Map:      create map file
193
#    --cref:    add cross reference to  map file
194
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
195
LDFLAGS += $(EXTMEMOPTS)
196
LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)
197
ifdef USE_WIRELESS
198
	LDFLAGS += -lwireless #-lcolonet_dragonfly
199
endif
200
LDFLAGS += -ldragonfly
201

  
202

  
203

  
204
#---------------- Programming Options (avrdude) ----------------
205

  
206
# Programming hardware: alf avr910 avrisp bascom bsd 
207
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
208
#
209
# Type: avrdude -c ?
210
# to get a full listing.
211
#
212
AVRDUDE_PROGRAMMER = avrisp
213

  
214
# programmer connected to serial device
215

  
216
AVRDUDE_WRITE_FLASH = -b 57600 -U flash:w:$(TARGET).hex
217
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
218

  
219

  
220
# Uncomment the following if you want avrdude's erase cycle counter.
221
# Note that this counter needs to be initialized first using -Yn,
222
# see avrdude manual.
223
#AVRDUDE_ERASE_COUNTER = -y
224

  
225
# Uncomment the following if you do /not/ wish a verification to be
226
# performed after programming the device.
227
#AVRDUDE_NO_VERIFY = -V
228

  
229
# Increase verbosity level.  Please use this when submitting bug
230
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> 
231
# to submit bug reports.
232
#AVRDUDE_VERBOSE = -v -v
233

  
234
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
235
AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)
236
AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)
237
AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)
238

  
239
#don't check for device signature
240
AVRDUDE_FLAGS += -F
241

  
242

  
243

  
244
#---------------- Debugging Options ----------------
245

  
246
# For simulavr only - target MCU frequency.
247
DEBUG_MFREQ = $(F_CPU)
248

  
249
# Set the DEBUG_UI to either gdb or insight.
250
# DEBUG_UI = gdb
251
DEBUG_UI = insight
252

  
253
# Set the debugging back-end to either avarice, simulavr.
254
DEBUG_BACKEND = avarice
255
#DEBUG_BACKEND = simulavr
256

  
257
# GDB Init Filename.
258
GDBINIT_FILE = __avr_gdbinit
259

  
260
# When using avarice settings for the JTAG
261
JTAG_DEV = /dev/com1
262

  
263
# Debugging port used to communicate between GDB / avarice / simulavr.
264
DEBUG_PORT = 4242
265

  
266
# Debugging host used to communicate between GDB / avarice / simulavr, normally
267
#     just set to localhost unless doing some sort of crazy debugging when 
268
#     avarice is running on a different computer.
269
DEBUG_HOST = localhost
270

  
271

  
272

  
273
#============================================================================
274

  
275

  
276
# Define programs and commands.
277
SHELL = sh
278
CC = avr-gcc
279
OBJCOPY = avr-objcopy
280
OBJDUMP = avr-objdump
281
SIZE = avr-size
282
NM = avr-nm
283
AVRDUDE = avrdude
284
REMOVE = rm -f
285
REMOVEDIR = rm -rf
286
COPY = cp
287
WINSHELL = cmd
288

  
289

  
290
# Define Messages
291
# English
292
MSG_ERRORS_NONE = Errors: none
293
MSG_BEGIN = -------- begin --------
294
MSG_END = --------  end  --------
295
MSG_SIZE_BEFORE = Size before: 
296
MSG_SIZE_AFTER = Size after:
297
MSG_COFF = Converting to AVR COFF:
298
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
299
MSG_FLASH = Creating load file for Flash:
300
MSG_EEPROM = Creating load file for EEPROM:
301
MSG_EXTENDED_LISTING = Creating Extended Listing:
302
MSG_SYMBOL_TABLE = Creating Symbol Table:
303
MSG_LINKING = Linking:
304
MSG_COMPILING = Compiling:
305
MSG_ASSEMBLING = Assembling:
306
MSG_CLEANING = Cleaning project:
307

  
308

  
309

  
310

  
311
# Define all object files.
312
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) 
313

  
314
# Define all listing files.
315
LST = $(SRC:.c=.lst) $(ASRC:.S=.lst) 
316

  
317

  
318
# Compiler flags to generate dependency files.
319
GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
320

  
321

  
322
# Combine all necessary flags and optional flags.
323
# Add target processor to flags.
324
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)
325
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
326

  
327

  
328

  
329

  
330

  
331
# Default target.
332
all: begin gccversion sizebefore build sizeafter end
333

  
334
build: elf hex eep lss sym
335

  
336
elf: $(TARGET).elf
337
hex: $(TARGET).hex
338
eep: $(TARGET).eep
339
lss: $(TARGET).lss 
340
sym: $(TARGET).sym
341

  
342

  
343

  
344
# Eye candy.
345
# AVR Studio 3.x does not check make's exit code but relies on
346
# the following magic strings to be generated by the compile job.
347
begin:
348
	@echo
349
	@echo $(MSG_BEGIN)
350

  
351
end:
352
	@echo $(MSG_END)
353
	@echo
354

  
355

  
356
# Display size of file.
357
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
358
ELFSIZE = $(SIZE) -A $(TARGET).elf
359
AVRMEM = avr-mem.sh $(TARGET).elf $(MCU)
360

  
361
sizebefore:
362
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \
363
	$(AVRMEM) 2>/dev/null; echo; fi
364

  
365
sizeafter:
366
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \
367
	$(AVRMEM) 2>/dev/null; echo; fi
368

  
369

  
370

  
371
# Display compiler version information.
372
gccversion : 
373
	@$(CC) --version
374

  
375

  
376

  
377
# Program the device.  
378
program: $(TARGET).hex $(TARGET).eep
379
	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
380

  
381

  
382
# Generate avr-gdb config/init file which does the following:
383
#     define the reset signal, load the target file, connect to target, and set 
384
#     a breakpoint at main().
385
gdb-config: 
386
	@$(REMOVE) $(GDBINIT_FILE)
387
	@echo define reset >> $(GDBINIT_FILE)
388
	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)
389
	@echo end >> $(GDBINIT_FILE)
390
	@echo file $(TARGET).elf >> $(GDBINIT_FILE)
391
	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)
392
ifeq ($(DEBUG_BACKEND),simulavr)
393
	@echo load  >> $(GDBINIT_FILE)
394
endif	
395
	@echo break main >> $(GDBINIT_FILE)
396
	
397
debug: gdb-config $(TARGET).elf
398
ifeq ($(DEBUG_BACKEND), avarice)
399
	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.
400
	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \
401
	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)
402
	@$(WINSHELL) /c pause
403
	
404
else
405
	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \
406
	$(DEBUG_MFREQ) --port $(DEBUG_PORT)
407
endif
408
	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)
409
	
410

  
411

  
412

  
413
# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.
414
COFFCONVERT=$(OBJCOPY) --debugging \
415
--change-section-address .data-0x800000 \
416
--change-section-address .bss-0x800000 \
417
--change-section-address .noinit-0x800000 \
418
--change-section-address .eeprom-0x810000 
419

  
420

  
421
coff: $(TARGET).elf
422
	@echo
423
	@echo $(MSG_COFF) $(TARGET).cof
424
	$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
425

  
426

  
427
extcoff: $(TARGET).elf
428
	@echo
429
	@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
430
	$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
431

  
432

  
433

  
434
# Create final output files (.hex, .eep) from ELF output file.
435
%.hex: %.elf
436
	@echo
437
	@echo $(MSG_FLASH) $@
438
	$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
439

  
440
%.eep: %.elf
441
	@echo
442
	@echo $(MSG_EEPROM) $@
443
	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
444
	--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
445

  
446
# Create extended listing file from ELF output file.
447
%.lss: %.elf
448
	@echo
449
	@echo $(MSG_EXTENDED_LISTING) $@
450
	$(OBJDUMP) -h -S $< > $@
451

  
452
# Create a symbol table from ELF output file.
453
%.sym: %.elf
454
	@echo
455
	@echo $(MSG_SYMBOL_TABLE) $@
456
	$(NM) -n $< > $@
457

  
458

  
459

  
460
# Link: create ELF output file from object files.
461
.SECONDARY : $(TARGET).elf
462
.PRECIOUS : $(OBJ)
463
%.elf: $(OBJ)
464
	@echo
465
	@echo $(MSG_LINKING) $@
466
	$(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS)
467

  
468

  
469
# Compile: create object files from C source files.
470
%.o : %.c
471
	@echo
472
	@echo $(MSG_COMPILING) $<
473
	$(CC) -c $(ALL_CFLAGS) $< -o $@ 
474

  
475

  
476
# Compile: create assembler files from C source files.
477
%.s : %.c
478
	$(CC) -S $(ALL_CFLAGS) $< -o $@
479

  
480

  
481
# Assemble: create object files from assembler source files.
482
%.o : %.S
483
	@echo
484
	@echo $(MSG_ASSEMBLING) $<
485
	$(CC) -c $(ALL_ASFLAGS) $< -o $@
486

  
487
# Create preprocessed source for use in sending a bug report.
488
%.i : %.c
489
	$(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ 
490

  
491

  
492
# Target: clean project.
493
clean: begin clean_list end
494

  
495
clean_list :
496
	@echo
497
	@echo $(MSG_CLEANING)
498
	$(REMOVE) $(TARGET).hex
499
	$(REMOVE) $(TARGET).eep
500
	$(REMOVE) $(TARGET).cof
501
	$(REMOVE) $(TARGET).elf
502
	$(REMOVE) $(TARGET).map
503
	$(REMOVE) $(TARGET).sym
504
	$(REMOVE) $(TARGET).lss
505
	$(REMOVE) $(OBJ)
506
	$(REMOVE) $(LST)
507
	$(REMOVE) $(SRC:.c=.s)
508
	$(REMOVE) $(SRC:.c=.d)
509
	$(REMOVEDIR) .dep
510

  
511

  
512

  
513
# Include the dependency files.
514
-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
515

  
516

  
517
# Listing of phony targets.
518
.PHONY : all begin finish end sizebefore sizeafter gccversion \
519
build elf hex eep lss sym coff extcoff \
520
clean clean_list program debug gdb-config
521

  
trunk/code/behaviors/bfs_fsm/bfs_fsm.h
1
// BFS FSM header file
2

  
3

  
4
#ifndef _BFS_FSM_H_
5
#define _BFS_FSM_H_
6

  
7
//The States: 
8
#define BFS_SEEK      12           //do run around
9
#define BFS_FOLLOW    13           //follow other robots to location
10
#define BFS_ORBIT     15           //Orbit robot
11
#define BFS_STOP      16           //Stop.  The default and ending state
12

  
13

  
14
#define BFS_STRAIGHT_SPEED 160
15

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

  
18
#define BFS_STOP_DISTANCE 100 /* distance to stop for object */
19

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

  
22
#define BFS_NO_VAL 255
23

  
24

  
25

  
26
int bfs_state;    /*State machine variable.*/
27

  
28
int bfs_otherRobot; /* the robot we are seeking */
29
int bfs_my_id; /* my wireless id */
30
int bfs_follow_id; /* robot to follow */
31

  
32

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

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

  
40
/* bfs_init
41
   argument: robot_id that you want to find
42
   notes: must call before bfs_fsm
43
*/
44
void bfs_init(int robot);
45

  
46

  
47
/* bfs_fsm
48
   argument: none
49
   notes: call in a while loop to perform FSM action
50
*/
51
void bfs_fsm(void);
52

  
53
#endif
trunk/code/behaviors/bfs_fsm/orbit_fsm.c
1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_token_ring.h>
4
#include "orbit_fsm.h"
5
#include "bfs_fsm.h"
6

  
7
/* Used to orbit a robot
8

  
9
  Must be within BOM range of robot before activating
10
  
11
  orbit_theta_stop requires encoders, and is incomplete
12
  
13
  Assumes:
14
    both robots are already in a token ring
15
*/
16

  
17

  
18
/* private function prototype */
19
void orbit_evaluate_state(void);
20

  
21

  
22
/* primary init */
23
void orbit_init(int robot) {
24
  //range_init();
25
  //analog_init(1);
26
  //motors_init();
27
  //orb_init();
28
  //orb_enable();
29
  //usb_init();
30
 
31
  /*Start in the start state, ORBIT_SEEK */ 
32
  orbit_state = ORBIT_INSERTION;
33
  
34
  orbit_otherRobot = robot; // set which robot to seek and orbit
35
  
36
  orbit_pControl=0;
37
  orbit_bom = bfs_bom;
38
  orbit_theta = 0;
39
  orbit_theta_stop = 1000;
40
  
41
  /*Initialize distances to zero.*/ 
42
  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
43
  
44
}
45
/* secondary init */
46
void orbit_init_theta(int robot,int theta) {
47
  //range_init();
48
  //analog_init(1);
49
  //motors_init();
50
  //orb_init();
51
  //orb_enable();
52
  //usb_init();
53
 
54
  /*Start in the start state, ORBIT_INSERTION */ 
55
  orbit_state = ORBIT_INSERTION;
56
  
57
  orbit_otherRobot = robot; // set which robot to seek and orbit
58
  
59
  orbit_pControl=0;
60
  orbit_bom = bfs_bom;
61
  orbit_theta = 0;
62
  orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop
63
  
64
  /*Initialize distances to zero.*/ 
65
  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
66
  
67
}
68

  
69
/*The main function, call this to update states as frequently as possible.*/
70
void orbit_fsm(void) {
71
  
72
  /*The following lines ensure that undefined (-1) values
73
  will not update the distances.*/ 
74
  int temp;  
75
    
76
  //temp=range_read_distance(IR1);
77
  //orbit_d1=(temp == -1) ? orbit_d1 : temp;
78
  
79
  temp=range_read_distance(IR2);
80
  orbit_d2=(temp == -1) ? orbit_d2 : temp;
81
  
82
  //temp=range_read_distance(IR3);
83
  //orbit_d3=(temp == -1) ? orbit_d3 : temp;
84
  
85
  //temp=range_read_distance(IR4);
86
  //orbit_d4=(temp == -1) ? orbit_d4 : temp;
87
  
88
  //temp=range_read_distance(IR5);
89
  //orbit_d5=(temp == -1) ? orbit_d5 : temp;
90
  
91
  wl_do(); // update wireless
92
  
93
  // get bom reading
94
  temp = wl_token_get_my_sensor_reading(orbit_otherRobot);
95
  orbit_bom = (temp == -1) ? orbit_bom : temp;
96
  
97
  // modify bom reading so right is negative, left is positive
98
  if (orbit_bom <= 12)
99
    orbit_bom -= 4;
100
  else
101
    orbit_bom -= 20;
102
  
103
    
104
  if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)
105
    orbit_state = ORBIT_INSERTION; // begin to orbit
106
  if (orbit_state == ORBIT_INSERTION &&
107
    (orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION))
108
    orbit_state = ORBIT_ORBITING; // orbit achieved
109
  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE) 
110
    || (orbit_theta >= orbit_theta_stop)))
111
    orbit_state = ORBIT_STOP; // orbit obstructed
112
  
113
  // evaluate state
114
  orbit_evaluate_state();
115
}
116

  
117

  
118
//Acts on state change.
119
void orbit_evaluate_state(){
120
    switch(orbit_state){
121
    case(ORBIT_SEEK): orb_set_color(RED);
122
      // move towards robot
123
      orbit_pControl = orbit_bom*10;      
124
      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
125
      break;
126
    
127
    case(ORBIT_INSERTION): orb_set_color(GREEN);
128
      // rotate into orbit, perpendicular to other robot
129
      orbit_pControl = -ORBIT_DIRECTION*10;
130
      move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);
131
      break;
132
      
133
    case(ORBIT_ORBITING): orb_set_color(BLUE);
134
      // go straight with slight rotation
135
      if (orbit_bom == ORBIT_DIRECTION)
136
        orbit_pControl = ORBIT_DIRECTION;
137
      else if (orbit_bom < ORBIT_DIRECTION)
138
        orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION;
139
      else
140
        orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION;
141
      
142
      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
143
      break;
144
      
145
    case(ORBIT_STOP): orb_set_color(YELLOW);
146
      move(0,0);
147
      break;
148
      
149
    default: orb_set_color(YELLOW);
150
      /*Should never get here, so stop.*/
151
      move(0,0);
152
      break;
153
  }
154
}
155

  
156

  
trunk/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

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

  
4
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
5
Could be better at not getting stuck.
6

  
7
Latest revision only has two accessible states: move and reverse.
8
*/
9

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

  
13
void run_around_init(void)
14
{
15
  //range_init();
16
  //analog_init(1);
17
  //motors_init();
18
  //orb_init();
19
  //orb_enable();
20
  usb_init();
21
 
22
  /*Start in the default state, MOVING*/ 
23
  avoid_state=MOVING;
24
  /*Set timers to their maximum values.*/
25
  crazy_count=CRAZY_MAX;
26
  backup_count=0; 
27
  pControl=0;
28
  
29
  /*Initialize distances to zero.*/ 
30
  d1=0; d2=0; d3=0; d4=0; d5=0;
31
  
32
  //orb_set_color(GREEN);
33

  
34
}
35

  
36
/*The main function, call this to update states as frequently as possible.*/
37
void run_around_FSM(void) {
38
  /*Default to moving.*/ 
39
  avoid_state=MOVING;
40
  
41
  /*The following lines ensure that undefined (-1) values
42
  will not update the distances.*/ 
43
  int temp;
44
  
45
  temp=range_read_distance(IR1);
46
  d1=(temp == -1) ? d1 : temp;
47
  
48
  temp=range_read_distance(IR2);
49
  d2=(temp == -1) ? d2 : temp;
50
  
51
  temp=range_read_distance(IR3);
52
  d3=(temp == -1) ? d3 : temp;
53
  
54
  temp=range_read_distance(IR4);
55
  d4=(temp == -1) ? d4 : temp;
56
  
57
  temp=range_read_distance(IR5);
58
  d5=(temp == -1) ? d5 : temp;
59
  
60
  /*If the crazy count is in it's >>3 range, it acts crazy.*/
61
  if(crazy_count<=(CRAZY_MAX>>3))
62
  {
63
    avoid_state=CRAZY;
64
    crazy_count--;
65
    
66
    if(crazy_count<0) crazy_count=CRAZY_MAX;
67
    
68
    run_around_evaluate_state();
69
    return;
70
  }
71
  
72
  //Checks the forward distance to see if it should back up, if so...state backwards.
73
  if((d2!=-1)&&(d2 < 150)){
74
      backup_count=BACKUP_MAX;
75
      avoid_state=BACKWARDS;
76
      run_around_evaluate_state();
77
      return;
78
  }
79
  /*
80
  if(d1 < 120 || d3 < 120) {
81
		avoid_state = BACKWARDS;
82
		backup_count = BACKUP_MAX;
83
		run_around_evaluate_state();
84
		return;
85
  }
86
  */
87
  if(backup_count<BACKUP_MAX){
88
    avoid_state=BACKWARDS; 
89
    if(backup_count<0)
90
      backup_count=BACKUP_MAX;
91
    run_around_evaluate_state();
92
    return;
93
  }
94
  
95
  /*Should evaluate an expression from -255 to 255 to pass to move.*/
96
  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;
97
  
98
  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;
99
  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/
100

  
101
  /*Debug stuff:*/
102
  /*usb_puts("pControl evaluating: ");
103
  usb_puti(pControl);
104
  usb_puts("\n\r");
105
  usb_puts("IR1: ");
106
  usb_puti(d1);
107
  usb_puts(" IR2: ");
108
  usb_puti(d2);
109
  usb_puts(" IR3: ");
110
  usb_puti(d3);
111
  usb_puts(" IR4: ");
112
  usb_puti(d4);
113
  usb_puts(" IR5: ");
114
  usb_puti(d5);
115
  usb_puts("\n\r");*/
116
  
117
  run_around_evaluate_state();
118
}
119

  
120

  
121
//Acts on state change.
122
void run_around_evaluate_state(){
123
    switch(avoid_state){
124
    case(MOVING): //orb_set_color(GREEN);
125
      move(STRAIT_SPEED,-pControl);
126
      break;
127
    
128
    case(BACKWARDS): //orb_set_color(ORANGE);
129
      move(-STRAIT_SPEED,0);
130
      break;
131
      
132
    case(CRAZY): //orb_set_color(RED);
133
      /*TODO: Implement a crazy state.*/
134
      move(STRAIT_SPEED,-pControl);
135
      break;
136
      
137
    default:
138
      /*Should never get here, go strait.*/
139
      move(100,0); //orb_set_color(BLUE);
140
      break;
141
  }
142
}
143

  
144

  
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff