Project

General

Profile

Revision 733

Added by Chris Mar almost 16 years ago

reverted projects/template/ directory to pre-demo state. moved run_around demo code into behaviors/smart_run_around_fsm/ directory.

View differences:

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

  
4
int main(void) {
5
	dragonfly_init(ALL_ON);
6
    range_init();
7
	//orb_enable();
8
	usb_puts("Turned on!\n");
9
    run_around_init();
10
    while(1) {
11
        run_around_FSM();
12
    }
13
	return 0;
14
}
trunk/code/behaviors/smart_run_around_fsm/smart_run_around_fsm.c
11 11
void run_around_init(void)
12 12
{
13 13
  range_init();
14
  analog_init();
15
  motors_init();
16
  orb_init();
17
  orb_enable();
18
  usb_init();
19 14
 
20 15
  /*Start in the default state, MOVING*/ 
21 16
  avoid_state=MOVING;
22 17
  /*Set timers to their maximum values.*/
23 18
  crazy_count=CRAZY_MAX;
24
  backup_count=0; 
19
  backup_count=BACKUP_MAX; 
25 20
  pControl=0;
26 21
  
27 22
  /*Initialize distances to zero.*/ 
......
123 118
      move(STRAIT_SPEED,-pControl);
124 119
      break;
125 120
    
126
    case(BACKWARDS): orb_set_color(ORANGE);
127
      move(-STRAIT_SPEED,0);
121
    case(BACKWARDS): orb_set_color(MAGENTA);
122
      move(-STRAIT_SPEED-50,0);
128 123
      break;
129 124
      
130 125
    case(CRAZY): orb_set_color(RED);
trunk/code/behaviors/smart_run_around_fsm/smart_run_around_fsm.h
13 13
#define LEFT 37             //Left
14 14
#define RIGHT 39            //Right
15 15

  
16
#define BACKUP_MAX 20
16
#define BACKUP_MAX 60
17 17
#define CRAZY_MAX 200       //The number of counts between "crazy moments"
18 18
#define STRAIT_SPEED 185    //The speed when going strait or backing up.
19 19
#define TURN_CONSTANT 2
20 20
#define PCONTROL_CRAZY_LIMIT 80
21 21

  
22
int avoid_state;    /*State machine variable.*/
22
int avoid_state;
23
int prev_state;    /*State machine variable.*/
23 24
int crazy_count;    /*Counter for a 'get unstuck' behavior.*/
24 25

  
25 26
int backup_count;	/*Counter for backup duration.*/
trunk/code/behaviors/smart_run_around_fsm/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 = com4
18
#
19
#
20
###################################
21

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

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

  
67
# MCU name
68
MCU = atmega128
69

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

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

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

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

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

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

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

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

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

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

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

  
150

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

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

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

  
163

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

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

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

  
175
MATH_LIB = -lm
176

  
177
#---------------- External Memory Options ----------------
178

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

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

  
187
EXTMEMOPTS =
188

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

  
201

  
202

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

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

  
213
# programmer connected to serial device
214

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

  
218

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

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

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

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

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

  
241

  
242

  
243
#---------------- Debugging Options ----------------
244

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

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

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

  
256
# GDB Init Filename.
257
GDBINIT_FILE = __avr_gdbinit
258

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

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

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

  
270

  
271

  
272
#============================================================================
273

  
274

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

  
288

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

  
307

  
308

  
309

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

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

  
316

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

  
320

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

  
326

  
327

  
328

  
329

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

  
333
build: elf hex eep lss sym
334

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

  
341

  
342

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

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

  
354

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

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

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

  
368

  
369

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

  
374

  
375

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

  
380

  
381
# Generate avr-gdb config/init file which does the following:
382
#     define the reset signal, load the target file, connect to target, and set
383
#     a breakpoint at main().
384
gdb-config:
385
	@$(REMOVE) $(GDBINIT_FILE)
386
	@echo define reset >> $(GDBINIT_FILE)
387
	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)
388
	@echo end >> $(GDBINIT_FILE)
389
	@echo file $(TARGET).elf >> $(GDBINIT_FILE)
390
	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)
391
ifeq ($(DEBUG_BACKEND),simulavr)
392
	@echo load  >> $(GDBINIT_FILE)
393
endif
394
	@echo break main >> $(GDBINIT_FILE)
395

  
396
debug: gdb-config $(TARGET).elf
397
ifeq ($(DEBUG_BACKEND), avarice)
398
	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.
399
	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \
400
	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)
401
	@$(WINSHELL) /c pause
402

  
403
else
404
	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \
405
	$(DEBUG_MFREQ) --port $(DEBUG_PORT)
406
endif
407
	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)
408

  
409

  
410

  
411

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

  
419

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

  
425

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

  
431

  
432

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

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

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

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

  
457

  
458

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

  
467

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

  
474

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

  
479

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

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

  
490

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

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

  
510

  
511

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

  
515

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

  
trunk/code/projects/template/smart_run_around_fsm.h
1
//Obstacle Avoid Numbers
2

  
3

  
4
#ifndef _RUN_AROUND_FSM_H_
5
#define _RUN_AROUND_FSM_H_
6

  
7
//The States: 
8
#define MOVING 12           //Move strait.
9
#define BACKWARDS 15        //Move backwards. (Front close to wall.)
10
#define STOP 16             //Stop.  The default state, (Something broke).
11
#define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)
12

  
13
#define LEFT 37             //Left
14
#define RIGHT 39            //Right
15

  
16
#define BACKUP_MAX 60
17
#define CRAZY_MAX 200       //The number of counts between "crazy moments"
18
#define STRAIT_SPEED 185    //The speed when going strait or backing up.
19
#define TURN_CONSTANT 2
20
#define PCONTROL_CRAZY_LIMIT 80
21

  
22
int avoid_state;
23
int prev_state;    /*State machine variable.*/
24
int crazy_count;    /*Counter for a 'get unstuck' behavior.*/
25

  
26
int backup_count;	/*Counter for backup duration.*/
27
int pControl;		/*Proportional control variable, determines turn direction.*/
28
int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/
29

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

  
34
#endif
trunk/code/projects/template/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

  
11
void run_around_init(void)
12
{
13
  range_init();
14
 
15
  /*Start in the default state, MOVING*/ 
16
  avoid_state=MOVING;
17
  /*Set timers to their maximum values.*/
18
  crazy_count=CRAZY_MAX;
19
  backup_count=BACKUP_MAX; 
20
  pControl=0;
21
  
22
  /*Initialize distances to zero.*/ 
23
  d1=0; d2=0; d3=0; d4=0; d5=0;
24
  
25
  orb_set_color(GREEN);
26

  
27
}
28

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

  
94
  /*Debug stuff:*/
95
  /*usb_puts("pControl evaluating: ");
96
  usb_puti(pControl);
97
  usb_puts("\n\r");
98
  usb_puts("IR1: ");
99
  usb_puti(d1);
100
  usb_puts(" IR2: ");
101
  usb_puti(d2);
102
  usb_puts(" IR3: ");
103
  usb_puti(d3);
104
  usb_puts(" IR4: ");
105
  usb_puti(d4);
106
  usb_puts(" IR5: ");
107
  usb_puti(d5);
108
  usb_puts("\n\r");*/
109
  
110
  evaluate_state();
111
}
112

  
113

  
114
//Acts on state change.
115
void evaluate_state(){
116
    switch(avoid_state){
117
    case(MOVING): orb_set_color(GREEN);
118
      move(STRAIT_SPEED,-pControl);
119
      break;
120
    
121
    case(BACKWARDS): orb_set_color(MAGENTA);
122
      move(-STRAIT_SPEED-50,0);
123
      break;
124
      
125
    case(CRAZY): orb_set_color(RED);
126
      /*TODO: Implement a crazy state.*/
127
      move(STRAIT_SPEED,-pControl);
128
      break;
129
      
130
    default:
131
      /*Should never get here, go strait.*/
132
      move(100,0); orb_set_color(BLUE);
133
      break;
134
  }
135
}
136

  
137

  
trunk/code/projects/template/main.c
1 1
#include <dragonfly_lib.h>
2
#include "smart_run_around_fsm.h"
3 2

  
4 3
int main(void) {
5 4
	dragonfly_init(ALL_ON);
6 5
    range_init();
7 6
	//orb_enable();
8 7
	usb_puts("Turned on!\n");
9
    run_around_init();
10
    while(1) {
11
        run_around_FSM();
12
    }
13 8
	return 0;
14 9
}

Also available in: Unified diff