Revision 1450

View differences:

branches/analog/code/tools/eeprom/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 = program_eeprom
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 = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
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

  
branches/analog/code/tools/eeprom/program_eeprom.c
1
#include <dragonfly_lib.h>
2
#include <eeprom.h>
3

  
4
int main()
5
{
6
  char str[5];
7
  int i=0,id;
8
  char c;
9

  
10
  dragonfly_init(ALL_ON);
11

  
12
  usb_puts("Here is the current setup:\r\n  ID: ");
13
  itoa(get_robotid(), str, 10);
14
  usb_puts(str);
15
  usb_puts("\r\n  BOM type: ");
16
  itoa(get_bom_type(), str, 10);
17
  usb_puts(str);
18
  usb_puts("\r\n\r\nEnter new ID:");
19

  
20
  while((c = usb_getc()) != '\n' && c != '\r')
21
    {
22
      usb_putc(c);
23
      if(i>=4)
24
	{
25
	  usb_puts("ERROR: filled buffer\n");
26
	  while(1);
27
	}
28
      str[i++] = c;
29
    }
30
  while(i<5)
31
    str[i++] = 0;
32

  
33
  id = atoi(str);
34

  
35
  usb_puts("\r\nsetting new id to: ");
36
  usb_puti(id);
37

  
38

  
39
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR, 'I');
40
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+1, 'D');
41
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+2, id);
42

  
43

  
44
  usb_puts("\r\nEnter BOM type:\r\b"
45
	   "1 for BOM 1.0\r\n"
46
	   "5 for BOM 1.5\r\n"
47
	   "r for RBOM\r\n>");
48

  
49
  c = usb_getc();
50
  usb_putc(c);
51

  
52
  switch(c)
53
    {
54
    case '1': id = BOM10; break;
55
    case '5': id = BOM15; break;
56
    case 'r': id = RBOM; break;
57
    default:
58
      usb_puts("ERROR: invalid input");
59
      while(1);
60
    }
61

  
62
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR, 'B');
63
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+1, 'O');
64
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+2, 'M');
65
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+3, id);
66

  
67

  
68

  
69
  usb_puts("\r\ndone! have a nice day\r\n");
70

  
71

  
72
  while(1);
73

  
74
  return 0;
75
}
76
  
branches/analog/code/behaviors/library_test/main.c
1
#include <dragonfly_lib.h>
2
//#include "smart_run_around_fsm.h"
3

  
4
// Martin "deffi" Herrmann Test
5

  
6
static void oset(uint8_t r,uint8_t g,uint8_t b)
7
{
8
    orbs_set(r,g,b,255-r,255-g,255-b);
9
}
10

  
11
static void color_circle (void)
12
{
13
    while(1)
14
    {
15
        for (uint8_t part=0; part<6; ++part)
16
        {
17
            for (uint8_t up=0; up<255; ++up)
18
            {
19
                uint8_t dn=255-up;
20
                uint8_t on=255;
21
                uint8_t of=0;
22

  
23
                if (1)
24
                {
25
                    // Maximum brightness mode
26
                    switch (part)
27
                    {
28
                        case 0: oset (on, up, of); break;
29
                        case 1: oset (dn, on, of); break;
30
                        case 2: oset (of, on, up); break;
31
                        case 3: oset (of, dn, on); break;
32
                        case 4: oset (up, of, on); break;
33
                        case 5: oset (on, of, dn); break;
34
                    }
35
                }
36
                else
37
                {
38
                    // Constant brightness mode (not very constant though)
39
                    switch (part%3)
40
                    {
41
                        case 0: oset (dn, up, of); break;
42
                        case 1: oset (of, dn, up); break;
43
                        case 2: oset (up, of, dn); break;
44
                    }
45
                }
46

  
47
                delay_ms (2);
48
            }
49
        }
50
    }
51
}
52

  
53
static void acl(void)
54
{
55
#define redval 255
56
#define greenval 150
57
#define interval 1300
58
#define flash 20
59
#define pause 200
60

  
61
#define def do { orb1_set(redval,0,0); orb2_set(0,greenval,0); } while (0)
62
#define wht do { orb_set(255,255,255); } while (0)
63

  
64
    while (1)
65
    {
66
        wht; delay_ms (flash);
67
        def; delay_ms (pause-flash);
68
        wht; delay_ms (flash);
69
        def; delay_ms (pause-flash);
70
        def; delay_ms (interval-2*pause-2*flash);
71
    }
72
}
73

  
74
int main(void) {
75
    //  dragonfly_init(ALL_ON);
76
    dragonfly_init(0);
77

  
78
    usb_init ();
79
    usb_puts ("Startup\r\n");
80

  
81

  
82
    //encoders_init ();
83

  
84
    //analog_init(ADC_START);
85
    //analog_init(0);
86

  
87
    //range_init();
88

  
89
    //DDRF=2;
90

  
91
    //motors_init ();
92
    //motor2_set (FORWARD, 64);
93

  
94
    //orb_init_binary ();
95
    orb_init_pwm ();
96

  
97
    if (false)
98
    {
99
        orbs_set (255, 0, 0, 0, 0, 0); delay_ms (500);
100
        orbs_set (0, 255, 0, 0, 0, 0); delay_ms (500);
101
        orbs_set (0, 0, 255, 0, 0, 0); delay_ms (500);
102
        orbs_set (0, 0, 0, 255, 0, 0); delay_ms (500);
103
        orbs_set (0, 0, 0, 0, 255, 0); delay_ms (500);
104
        orbs_set (0, 0, 0, 0, 0, 255); delay_ms (500);
105
    }
106

  
107
    if (false)
108
    {
109
        orb_set (1,1,1);
110
        while (1)
111
        {
112
            SYNC
113
            {
114
                for (uint8_t m=0; m<100; ++m)
115
                {
116
                    _delay_us(10);
117
                }
118
            }
119
        }
120
    }
121

  
122
    if (false)
123
    {
124
        while (1)
125
        {
126
            for (uint8_t x=0; x<255; ++x)
127
            {
128
                orbs_set (x, 1, 2, 3, 4, 5);
129

  
130
                for (uint8_t n=0; n<80; ++n)
131
                {
132
                    SYNC
133
                    { for (uint8_t m=0; m<1; ++m) _delay_us(50); }
134
                }
135
            }
136

  
137
            for (uint8_t x=255; x>0; --x)
138
            {
139
                orbs_set (x, 1, 1, 1, 1, 1);
140
                delay_ms (4);
141
            }
142
        }
143
    }
144

  
145
    if (false)
146
    {
147
        //orbs_set (2,4,6,8,10,12);
148
        //orbs_set (32, 64, 96, 128, 160, 192);
149
        orbs_set (128,128,128,128,128,128);
150
        while (1);
151
    }
152

  
153
    if (false)
154
    {
155
        if (!button2_read ())
156
        {
157
            orb_set_mode (orb_mode_pwm);
158
            orb_disable_timer ();
159

  
160
            while (1)
161
            {
162
                // Continuously call the sorting routine for timing test
163
                for (uint8_t i=0; i<200; ++i)
164
                {
165
                    orbs_set (10,20,30,40,50,60);
166
                    delay_ms (10);
167
                }
168

  
169
                for (uint8_t i=0; i<200; ++i)
170
                {
171
                    orbs_set (60,50,40,30,20,10);
172
                    delay_ms (10);
173
                }
174
            }
175
        }
176
        else
177
        {
178
            while (button2_read ());
179
            if (button1_read ())
180
            {
181
                color_circle ();
182
            }
183
            else
184
            {
185
                acl ();
186
            }
187
        }
188
    }
189

  
190

  
191

  
192
    // Do some lighting
193
    if (!button2_read ())
194
        color_circle ();
195
    else
196
        acl ();
197

  
198
}
199

  
branches/analog/code/behaviors/library_test/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
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
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)/lib/include/libdragonfly
121
CINCS += -L$(COLONYROOT)/lib/bin
122
ifdef USE_WIRELESS
123
	CINCS += -I$(COLONYROOT)/lib/include/libwireless
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
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

  
branches/analog/code/behaviors/orbit_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

  
6
/* Used to orbit a robot
7

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

  
16

  
17
/* private function prototype */
18
void evaluate_state(void);
19

  
20

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

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

  
115

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

  
154

  
branches/analog/code/behaviors/orbit_fsm/orbit_fsm.h
1
//Obstacle Avoid Numbers
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff