TAGS
_compiled
_build
-_infrastructure
*.pyc
cscope.files
cscope.out
[submodule "src/orte"]
path = src/orte
- url = ../orte/eurobot.git
+ url = ssh://git@rtime.felk.cvut.cz/orte/eurobot.git
[submodule "robot-root"]
path = robot-root
- url = ./robot-root.git
+ url = ssh://git@rtime.felk.cvut.cz/eurobot/robot-root.git
[submodule "src/pxmc"]
path = src/pxmc
- url = ../pxmc.git
+ url = ssh://git@rtime.felk.cvut.cz/pxmc.git
[submodule "src/linux-shark"]
path = src/linux-shark
- url = ../shark/linux.git
+ url = ssh://git@rtime.felk.cvut.cz/shark/linux.git
[submodule "src/v4l/v4l-utils"]
path = src/v4l/v4l-utils
url = git://linuxtv.org/v4l-utils.git
[submodule "src/sysless"]
path = src/sysless
- url = ../sysless.git
+ url = ssh://git@rtime.felk.cvut.cz/sysless.git
[submodule "src/3rdparty/opencv"]
path = src/3rdparty/opencv
- url = ../opencv.git
+ url = ssh://git@rtime.felk.cvut.cz/opencv.git
[submodule "src/ulut"]
path = src/ulut
url = git://rtime.felk.cvut.cz/ulut.git
+[submodule "src/ulan-app"]
+ path = src/ulan-app
+ url = git://ulan.git.sourceforge.net/gitroot/ulan/ulan-app
+[submodule "src/3rdparty/qt"]
+ path = src/3rdparty/qt
+ url = git://gitorious.org/qt/qt.git
+[submodule "bin/gumstix"]
+ path = bin/gumstix
+ url = ssh://git@rtime.felk.cvut.cz/eurobot/gumstix-bin
+[submodule "src/3rdparty/libusb"]
+ path = src/3rdparty/libusb
+ url = git://git.libusb.org/libusb.git
+[submodule "openembedded/openembedded-core"]
+ path = openembedded/openembedded-core
+ url = git://git.openembedded.org/openembedded-core
+[submodule "openembedded/meta-openembedded"]
+ path = openembedded/meta-openembedded
+ url = git://git.openembedded.org/meta-openembedded
+[submodule "openembedded/bitbake"]
+ path = openembedded/bitbake
+ url = git://git.openembedded.org/bitbake
+[submodule "openembedded/meta-angstrom"]
+ path = openembedded/meta-angstrom
+ url = git://github.com/Angstrom-distribution/meta-angstrom.git
+[submodule "openembedded/meta-gumstix-community"]
+ path = openembedded/meta-gumstix-community
+ url = git://gitorious.org/schnitzeltony-oe-meta/meta-gumstix-community.git
+[submodule "openembedded/meta-linaro"]
+ path = openembedded/meta-linaro
+ url = git://git.linaro.org/openembedded/meta-linaro.git
+[submodule "openembedded/meta-ti"]
+ path = openembedded/meta-ti
+ url = git://git.yoctoproject.org/meta-ti
+[submodule "openembedded/meta-toradex-community"]
+ path = openembedded/meta-toradex-community
+ url = git://gitorious.org/schnitzeltony-oe-meta/meta-toradex-community.git
--- /dev/null
+Subproject commit 2df582f32878639297dac0fddf7fe9ace7f5623c
-SUBDIRS=linux ppc lpceurobot # spejblarm h8eurobot h8mirosot
+SUBDIRS=host ppc lpceurobot h8eurobot # spejblarm h8mirosot
all := all $(filter-out all Makefile,$(MAKECMDGOALS))
# Makefile.rules - OCERA make framework common project rules -*- makefile-gmake -*- #OMK:base.omk
#
# (C) Copyright 2003, 2006, 2007, 2008, 2009 by Pavel Pisa - OCERA team member
-# (C) Copyright 2006, 2007, 2008, 2009, 2010 by Michal Sojka - Czech Technical University, FEE, DCE
+# (C) Copyright 2006, 2007, 2008, 2009, 2010, 2011 by Michal Sojka - Czech Technical University, FEE, DCE
#
# Homepage: http://rtime.felk.cvut.cz/omk/
-# Version: 0.2-15-g5530d5e
+# Version: 0.2-102-gfe2582a
#
# The OMK build system is distributed under the GNU General Public
# License. See file COPYING for details.
# W .. whole tree - if set to 1, make is always called from the top-level directory
# SUBDIRS .. list of subdirectories intended for make from actual directory
# default_CONFIG .. list of default config assignments CONFIG_XXX=y/n ...
+# wvtest_SCRIPTS .. list of scripts producing wvtest output #OMK:wvtest.omk
+# wvtest_PROGRAMS .. list of the testing programs producing wvtest output
# LN_HEADERS .. if "y", header files are symbolicaly linked instead of copied. #OMK:include.omk
# input variables #OMK:linux.omk
# lib_LIBRARIES .. list of the user-space libraries
# bin_PROGRAMS .. list of the require binary programs
# utils_PROGRAMS .. list of the development utility programs
# test_PROGRAMS .. list of the testing programs
+# bin_SCRIPTS .. list of scripts to be copied to _compiled/bin
# kernel_MODULES .. list of the kernel side modules/applications
# rtlinux_MODULES .. list of RT-Linux the kernel side modules/applications
# xxx_SOURCES .. list of specific target sources
MAKERULES_DIR := $(abspath $(dir $(filter %Makefile.rules,$(MAKEFILE_LIST))))
endif
+# The $(SED4OMK) command for BSD based systems requires -E option to allow
+# extended regular expressions
+
+SED4OMK ?= sed
+ifneq ($(shell ( echo A | $(SED4OMK) -n -e 's/A\|B/y/p' )),y)
+ SED4OMK := $(SED4OMK) -E
+ ifneq ($(shell ( echo A | $(SED4OMK) -n -e 's/A\|B/y/p' )),y)
+ SED4OMK := gsed
+ endif
+ ifneq ($(shell ( echo A | $(SED4OMK) -n -e 's/A\|B/y/p' )),y)
+ SED4OMK := gsed -E
+ endif
+ ifneq ($(shell ( echo A | $(SED4OMK) -n -e 's/A\|B/y/p' )),y)
+ $(error No SED program suitable for OMK found)
+ endif
+endif
+
# OUTPUT_DIR is the place where _compiled, _build and possible other
# files/directories are created. By default is the same as
# $(MAKERULES_DIR).
INVOCATION_DIR := $(INVOCATION_DIR:\\%=%)
endif
-.PHONY: all default check-make-ver omkize
+.PHONY: all default check-make-ver print-hints omkize
ifdef W
ifeq ("$(origin W)", "command line")
endif
ifneq ($(OMK_WHOLE_TREE),1)
-all: check-make-ver default
+all: check-make-ver print-hints default
@echo "Compilation finished"
else
# Run make in the top-level directory
@$(MAKE) -C $(MAKERULES_DIR) OMK_SERIALIZE_INCLUDED=n SOURCES_DIR=$(MAKERULES_DIR) RELATIVE_DIR="" $(MAKECMDGOALS) W=0
endif
-ifdef OMK_TESTSROOT
-# Usage: $(call canttest,<error message>)
-define canttest
- ( echo "$(1)" > $(OUTPUT_DIR)/_canttest; echo "$(1)"; exit 1 )
-endef
-else
-define canttest
- echo "$(1)"
-endef
-endif
+# omk-get-var target allows external scripts/programs to determine the
+# values of OMK variables such as RELATIVE_DIR etc.
+.PHONY: omk-get-var
+omk-get-var:
+ @$(foreach var,$(VAR),echo $(var)=$($(var));)
#=========================
# Include the config file
-# FIXME: I think CONFIG_FILE_OK variable is useless. We have three
-# config files and it is not clearly defined to which file is this
-# variable related.
-ifneq ($(CONFIG_FILE_OK),y)
ifndef CONFIG_FILE
CONFIG_FILE := $(OUTPUT_DIR)/config.omk
endif
-ifneq ($(wildcard $(CONFIG_FILE)-default),)
--include $(CONFIG_FILE)-default
-else
-ifneq ($(MAKECMDGOALS),default-config)
-$(warning Please, run "make default-config" first)
+
+$(CONFIG_FILE)-default:
+ $(MAKE) default-config
+
+ifeq ($(MAKECMDGOALS),default-config)
+export DEFAULT_CONFIG_PASS=1
endif
+
+ifneq ($(DEFAULT_CONFIG_PASS),1)
+include $(CONFIG_FILE)-default
endif
-include $(OUTPUT_DIR)/config.target
ifneq ($(wildcard $(CONFIG_FILE)),)
-include $(CONFIG_FILE)
-CONFIG_FILE_OK = y
endif
-endif #$(CONFIG_FILE_OK)
CONFIG_FILES ?= $(wildcard $(CONFIG_FILE)-default) $(wildcard $(OUTPUT_DIR)/config.target) $(wildcard $(CONFIG_FILE))
-export SOURCES_DIR MAKERULES_DIR RELATIVE_DIR INVOCATION_DIR
+export SED4OMK SOURCES_DIR MAKERULES_DIR RELATIVE_DIR INVOCATION_DIR
export CONFIG_FILE CONFIG_FILES OMK_SERIALIZE_INCLUDED OMK_VERBOSE OMK_SILENT
# OMK_SERIALIZE_INCLUDED has to be exported to submakes because passes
# must to be serialized only in the toplevel make.
override RELATIVE_DIR := $(RELATIVE_DIR:/%=%)
override RELATIVE_DIR := $(RELATIVE_DIR:\\%=%)
#$(warning RELATIVE_DIR = "$(RELATIVE_DIR)")
-override BACK2TOP_DIR := $(shell echo $(RELATIVE_DIR)/ | sed -e 's_//_/_g' -e 's_/\./_/_g' -e 's_^\./__g' -e 's_\([^/][^/]*\)_.._g' -e 's_/$$__')
+#override BACK2TOP_DIR := $(shell echo $(RELATIVE_DIR)/ | $(SED4OMK) -e 's_//_/_g' -e 's_/\./_/_g' -e 's_^\./__g' -e 's_\([^/][^/]*\)_.._g' -e 's_/$$__')
#$(warning BACK2TOP_DIR = "$(BACK2TOP_DIR)")
#$(warning SOURCES_DIR = "$(SOURCES_DIR)")
OMK_INCLUDED := 1
endif
+print-hints:
+ @echo 'Use "make V=1" to see the verbose compile lines.'
+
check-make-ver:
- @GOOD_MAKE_VERSION=`echo $(MAKE_VERSION) | sed -n -e 's/^[4-9]\..*\|^3\.9[0-9].*\|^3\.8[1-9].*/y/p'` ; \
+ @GOOD_MAKE_VERSION=`echo $(MAKE_VERSION) | $(SED4OMK) -n -e 's/^[4-9]\..*\|^3\.9[0-9].*\|^3\.8[1-9].*/y/p'` ; \
if [ x$$GOOD_MAKE_VERSION != xy ] ; then \
- echo "Your make program version is too old and does not support OMK system." ; \
+ echo "Your make program version ($(MAKE_VERSION)) is too old and does not support OMK system." ; \
echo "Please update to make program 3.81beta1 or newer." ; exit 1 ; \
fi
$(pass)-$(3)-subdir: MAKEOVERRIDES:=$(filter-out SUBDIRS=%,$(MAKEOVERRIDES))
$(pass)-$(3)-subdir:
@$(call mkdir_def,$(2)/$(3))
- +@$(MAKE) SOURCES_DIR=$(SOURCES_DIR)/$(3) $(NO_PRINT_DIRECTORY) \
+ +@$(MAKE) --no-builtin-rules SOURCES_DIR=$(SOURCES_DIR)/$(3) $(NO_PRINT_DIRECTORY) \
RELATIVE_DIR=$(RELATIVE_PREFIX)$(3) -C $(2)/$(3) \
-f $(SUBDIR_MAKEFILE) $(pass)-submakes
# In subdirectories we can call submakes directly since passes are
$(pass):
# Submakes have to be called this way and not as dependecies for pass
# serialization to work
- +@$(MAKE) SOURCES_DIR=$(SOURCES_DIR) $(NO_PRINT_DIRECTORY) \
+ +@$(MAKE) --no-builtin-rules SOURCES_DIR=$(SOURCES_DIR) $(NO_PRINT_DIRECTORY) \
RELATIVE_DIR=$(RELATIVE_DIR) \
-f $(SOURCESDIR_MAKEFILE) $(pass)-submakes
$(pass)-submakes:
$(pass)-this-dir: $(foreach subdir,$(SUBDIRS),$(pass)-$(subdir)-subdir)
+@echo "make[omk]: $(pass) in $(RELATIVE_DIR)"
@$(call mkdir_def,$(2))
- +@$(MAKE) $(NO_PRINT_DIRECTORY) SOURCES_DIR=$(SOURCES_DIR) RELATIVE_DIR=$(RELATIVE_DIR) -C $(2) \
+ +@$(MAKE) --no-builtin-rules $(NO_PRINT_DIRECTORY) SOURCES_DIR=$(SOURCES_DIR) RELATIVE_DIR=$(RELATIVE_DIR) -C $(2) \
-f $(SOURCESDIR_MAKEFILE) $(3) $(check-target) $(1:%=%-local)
$(pass)-local: $($(pass)_HOOKS)
endif
# @echo Default config for $(RELATIVE_DIR)
@echo "# Config for $(RELATIVE_DIR)" >> "$(CONFIG_FILE)-default"
@$(foreach x, $(default_CONFIG), echo '$(x)' | \
- sed -e 's/^[^=]*=x$$/#\0/' >> "$(CONFIG_FILE)-default" ; )
+ $(SED4OMK) -e 's/^[^=]*=x$$/#\0/' >> "$(CONFIG_FILE)-default" ; )
omkize:
cp -v Makefile "$${d}/Makefile"; \
fi \
done
+ #OMK:wvtest.omk@Makefile.rules.linux
+ifndef WVTEST_LIBRARY
+WVTEST_LIBRARY = wvtest
+endif
+
+# Documentation: wvtest_PROGRAMS is amost equivalent to test_PROGRAMS.
+# The two differences are that the program is automatically linked
+# with $(WVTEST_LIBRARY) and it is run during "make wvtest".
+test_PROGRAMS += $(wvtest_PROGRAMS)
+
+# Documentation: If your project uses wvtest, it is recomended to put
+# the "test: wvtest" rule to config.target.
+wvtest:
+ $(Q)$(MAKERULES_DIR)/wvtestrun $(MAKE) wvtest-pass
+
+.PHONY: wvtest
+
+$(eval $(call omk_pass_template,wvtest-pass,$$(LOCAL_BUILD_DIR),,$(wvtest_SCRIPTS)$(wvtest_PROGRAMS)))
+
+# Usage: $(call wvtest_template,<shell command>)
+define wvtest_template
+wvtest-pass-local: wvtest-run-$(1)
+.PHONY: wvtest-run-$(1)
+wvtest-run-$(1):
+ $(Q)echo "Testing \"Run $(1)\" in Makefile.rules:"
+ $(Q)mkdir -p $(1).wvtest
+ $(Q)cd $(1).wvtest && \
+ PATH=$$(USER_BIN_DIR):$$$$PATH LD_LIBRARY_PATH=$$(USER_LIB_DIR):$$$$LD_LIBRARY_PATH \
+ $(1)
+$(notdir $(1))_LIBS += $$(WVTEST_LIBRARY)
+endef
+
+# Documentation: Write the test so, that it can be run from arbitrary
+# directory, i.e. in case of a script ensure that the wvtest library
+# is sourced like this:
+#
+# . $(dirname $0)/wvtest.sh
+
+$(foreach script,$(wvtest_SCRIPTS),$(eval $(call wvtest_template,$(SOURCES_DIR)/$(script))))
+# Hack!!!
+USER_TESTS_DIR := $(OUTPUT_DIR)/_compiled/bin-tests
+$(foreach prog,$(wvtest_PROGRAMS),$(eval $(call wvtest_template,$(USER_TESTS_DIR)/$(prog))))
ifeq ($(OMK_VERBOSE),1) #OMK:include.omk@Makefile.rules.linux
CPHEADER_FLAGS += -v
LNHEADER_FLAGS += -v
ifneq ($(LN_HEADERS),y)
define cp_cmd
-( echo " CP $(1:$(OUTPUT_DIR)/%=%) -> $(2:$(OUTPUT_DIR)/%=%)"; cp $(CPHEADER_FLAGS) $(1) $(2) )
+if ! cmp --quiet $(1) $(2); then \
+ echo " CP $(1:$(OUTPUT_DIR)/%=%) -> $(2:$(OUTPUT_DIR)/%=%)"; \
+ install -D $(CPHEADER_FLAGS) $(1) $(2) || exit 1; \
+fi
endef
else
define cp_cmd
-( echo " LN $(1:$(OUTPUT_DIR)/%=%) -> $(2:$(OUTPUT_DIR)/%=%)"; [ -f $(1) ] && ln -sf $(LNHEADER_FLAGS) $(1) $(2) )
+if ! cmp --quiet $(1) $(2); then \
+ echo " LN $(1:$(OUTPUT_DIR)/%=%) -> $(2:$(OUTPUT_DIR)/%=%)"; \
+ if [ -f $(1) ]; then d=$(2); mkdir -p $${d%/*} && ln -sf $(LNHEADER_FLAGS) $(1) $(2) || exit 1; else exit 1; fi; \
+fi
endef
endif
# Syntax: $(call include-pass-template,<include dir>,<keyword>)
define include-pass-template
include-pass-local: include-pass-local-$(2)
-include-pass-local-$(2): $$($(2)_GEN_HEADERS) $$(foreach f,$$(renamed_$(2)_GEN_HEADERS),$$(shell echo '$$(f)' | sed -e 's/^\(.*\)->.*$$$$/\1/'))
- @$$(foreach f, $$($(2)_HEADERS), cmp --quiet $$(SOURCES_DIR)/$$(f) $(1)/$$(notdir $$(f)) \
- || $$(call cp_cmd,$$(SOURCES_DIR)/$$(f),$(1)/$$(notdir $$(f))) || exit 1 ; )
- @$$(foreach f, $$($(2)_GEN_HEADERS), cmp --quiet $$(f) $(1)/$$(notdir $$(f)) \
- || $$(call cp_cmd,$$(LOCAL_BUILD_DIR)/$$(f),$(1)/$$(notdir $$(f))) || exit 1 ; ) # FIXME: Use correct build dir, then document it
- @$$(foreach f, $$(nobase_$(2)_HEADERS), cmp --quiet $$(SOURCES_DIR)/$$(f) $(1)/$$(f) \
- || ( mkdir -p $(1)/$$(dir $$(f)) && $$(call cp_cmd,$$(SOURCES_DIR)/$$(f),$(1)/$$(f)) ) || exit 1 ; )
+include-pass-local-$(2): $$($(2)_GEN_HEADERS) $$(foreach f,$$(renamed_$(2)_GEN_HEADERS),$$(shell f='$$(f)'; echo $$$${f%->*}))
+ @$$(foreach f, $$($(2)_HEADERS),$$(call cp_cmd,$$(SOURCES_DIR)/$$(f),$(1)/$$(notdir $$(f))); )
+# FIXME: Use correct build dir, then document it (in the line bellow)
+ @$$(foreach f, $$($(2)_GEN_HEADERS),$$(call cp_cmd,$$(LOCAL_BUILD_DIR)/$$(f),$(1)/$$(notdir $$(f))); )
+ @$$(foreach f, $$(nobase_$(2)_HEADERS), $$(call cp_cmd,$$(SOURCES_DIR)/$$(f),$(1)/$$(f)); )
@$$(foreach f, $$(renamed_$(2)_HEADERS), \
- srcfname=`echo '$$(f)' | sed -e 's/^\(.*\)->.*$$$$/\1/'` ; destfname=`echo '$$(f)' | sed -e 's/^.*->\(.*\)$$$$/\1/'` ; \
- cmp --quiet $$(SOURCES_DIR)/$$$${srcfname} $(1)/$$$${destfname} \
- || ( mkdir -p `dirname $(1)/$$$${destfname}` && $$(call cp_cmd,$$(SOURCES_DIR)/$$$${srcfname},$(1)/$$$${destfname}) ) || exit 1 ; )
+ f='$$(f)'; srcfname=$$$${f%->*}; destfname=$$$${f#*->}; \
+ $$(call cp_cmd,$$(SOURCES_DIR)/$$$${srcfname},$(1)/$$$${destfname}); )
@$$(foreach f, $$(renamed_$(2)_GEN_HEADERS), \
- srcfname=`echo '$$(f)' | sed -e 's/^\(.*\)->.*$$$$/\1/'` ; destfname=`echo '$$(f)' | sed -e 's/^.*->\(.*\)$$$$/\1/'` ; \
- cmp --quiet $$$${srcfname} $(1)/$$$${destfname} \
- || ( mkdir -p `dirname $(1)/$$$${destfname}` && $$(call cp_cmd,$$(LOCAL_BUILD_DIR)/$$$${srcfname},$(1)/$$$${destfname}) ) || exit 1 ; )
+ f='$$(f)'; srcfname=$$$${f%->*}; destfname=$$$${f#*->}; \
+ $$(call cp_cmd,$$(LOCAL_BUILD_DIR)/$$$${srcfname},$(1)/$$$${destfname}); )
+# Suppress "Nothing to be done for `include-pass-local'" message if no headers are defined in Makefile.omk
+ @$$(if $$($(2)_HEADERS)$$($(2)_GEN_HEADERS)$$(nobase_$(2)_HEADERS)$$(renamed_$(2)_HEADERS)$$(renamed_$(2)_GEN_HEADERS),,true)
endef
#OMK:linux.omk@Makefile.rules.linux
# Hack to check RT-Linux rules
MAKEOVERRIDES := $(filter-out OMK_SERIALIZE_INCLUDED=n,$(MAKEOVERRIDES))
endif
-# Checks for OMK tester
-ifdef OMK_TESTSROOT
-default-config-pass-check include-pass-check:
-library-pass-check binary-pass-check:
- @[ -x "$(shell which $(CC))" ] || $(call canttest,Cannot find compiler: $(CC))
-endif
-
#=====================================================================
# User-space rules and templates to compile programs, libraries etc.
ifdef USER_RULE_TEMPLATES
-USER_SOURCES2OBJS = .o/.c .o/.cc .o/.cxx .o/.S .o/.o
+USER_SOURCES2OBJS = .o/.c .o/.cc .o/.cxx .o/.cpp .o/.S .o/.o
-USER_SOURCES2OBJSLO = .lo/.c .lo/.cc .lo/.cxx .lo/.S .lo/.lo
+USER_SOURCES2OBJSLO = .lo/.c .lo/.cc .lo/.cxx .lo/.cpp .lo/.S .lo/.lo
#%.lo: %.c
# $(CC) -o $@ $(LCFLAGS) -c $<
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -DOMK_FOR_USER
cc_o_COMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
- $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -DOMK_FOR_USER
+ $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) $(CXXFLAGS) -DOMK_FOR_USER
S_o_COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) $(ASFLAGS) -DOMK_FOR_USER
# Check GCC version for user build
ifndef CC_MAJOR_VERSION
-CC_MAJOR_VERSION := $(shell $(CC) -dumpversion | sed -e 's/\([^.]\)\..*/\1/')
+CC_MAJOR_VERSION := $(shell $(CC) -dumpversion | $(SED4OMK) -e 's/\([^.]\)\..*/\1/')
endif
# Prepare suitable define for dependency building
ifeq ($(CC_MAJOR_VERSION),2)
# Bellow, the tricks with redirection are for shells without set -o pipefail
# (see http://www.mail-archive.com/dash@vger.kernel.org/msg00149.html)
$(Q)exec 3>&1; status=`exec 4>&1 >&3; { $(NM) $$<; echo $$$$? >&4; }\
- | sed -n 's/^ *0*\(0[0-9A-Fa-f]*\) *A *_cmetric2cond_\([A-Za-z_0-9]*\) */#define \2 0x\1/p' \
+ | $(SED4OMK) -n 's/^ *0*\(0[0-9A-Fa-f]*\) *A *_cmetric2cond_\([A-Za-z_0-9]*\) */#define \2 0x\1/p' \
| sort >>$$@.tmp` && exit $$$$status
$(Q)echo >>$$@.tmp '/* Defines from the values defined to symbols in hexadecimal format */'
$(Q)exec 3>&1; status=`exec 4>&1 >&3; { $(NM) $$<; echo $$$$? >&4; }\
- | sed -n 's/^ *0*\(0[0-9A-Fa-f]*\) *A *_cmetric2def_\([A-Za-z_0-9]*\) */#define \2 0x\1/p' \
+ | $(SED4OMK) -n 's/^ *0*\(0[0-9A-Fa-f]*\) *A *_cmetric2def_\([A-Za-z_0-9]*\) */#define \2 0x\1/p' \
| sort >>$$@.tmp` && exit $$$$status
$(Q)echo >>$$@.tmp '/* Defines from the values defined to symbols in decimal format */'
$(Q)exec 3>&1; status=`exec 4>&1 >&3; { $(NM) -td $$<; echo $$$$? >&4; }\
- | sed -n 's/^ *0*\(0\|[1-9][0-9]*\) *A *_cmetric2defdec_\([A-Za-z_0-9]*\) */#define \2 \1/p' \
+ | $(SED4OMK) -n 's/^ *0*\(0\|[1-9][0-9]*\) *A *_cmetric2defdec_\([A-Za-z_0-9]*\) */#define \2 \1/p' \
| sort >>$$@.tmp` && exit $$$$status
$(Q)mv $$@.tmp $$@
endef
$(2)/$(1)$(3): $$($(1)_OBJS)
@$(QUIET_CMD_ECHO) " LINK $$@"
- $(Q) $$(if $$(filter %.cc,$$($(1)_SOURCES:%.cxx=%.cc)),$$(CXX),$$(CC)) \
+ $(Q) $$(if $$(filter %.cc,$$($(1)_SOURCES))$$(filter %.cxx,$$($(1)_SOURCES))$$(filter %.cpp,$$($(1)_SOURCES)),$$(CXX),$$(CC)) \
$$($(1)_OBJS) $$($(1)_LIBS:%=-l%) $$(LOADLIBES) $$(LDFLAGS) $$($(1)_LDFLAGS) -Wl,-rpath-link,$(USER_LIB_DIR) -Wl,-Map,$(USER_OBJS_DIR)/$(1).exe.map -o $$@
@echo "$(2)/$(1)$(3): \\" >$(USER_OBJS_DIR)/$(1).exe.d
- @sed -n -e 's|^LOAD \(.*\)$$$$| \1 \&|p' $(USER_OBJS_DIR)/$(1).exe.map|tr '&' '\134' >>$(USER_OBJS_DIR)/$(1).exe.d
+ @$(SED4OMK) -n -e 's|^LOAD \(.*\)$$$$| \1 \&|p' $(USER_OBJS_DIR)/$(1).exe.map|tr '&' '\134' >>$(USER_OBJS_DIR)/$(1).exe.d
@echo >>$(USER_OBJS_DIR)/$(1).exe.d
endef
+# Usage: $(call SCRIPT_template,<target-directory>,<script-name>)
+define SCRIPT_template
+$(2)/$(1): $$(SOURCES_DIR)/$(1)
+ @$(QUIET_CMD_ECHO) " CP $$@"
+ $(Q)cp $$^ $$@
+endef
+
# Syntax: $(call LIBRARY_template,<library-name>)
define LIBRARY_template
library-pass-local: $(addprefix $(USER_INCLUDE_DIR)/,$(cmetric_include_HEADERS)) \
$(lib_LIBRARIES:%=$(USER_LIB_DIR)/lib%.a) $(shared_LIBRARIES:%=$(OMK_WORK_DIR)/lib%.$(SOLIB_EXT).omkvar)
-binary-pass-local: $(bin_PROGRAMS:%=$(USER_BIN_DIR)/%$(EXE_SUFFIX)) $(utils_PROGRAMS:%=$(USER_UTILS_DIR)/%$(EXE_SUFFIX)) $(test_PROGRAMS:%=$(USER_TESTS_DIR)/%$(EXE_SUFFIX))
+binary-pass-local: $(bin_PROGRAMS:%=$(USER_BIN_DIR)/%$(EXE_SUFFIX)) \
+ $(utils_PROGRAMS:%=$(USER_UTILS_DIR)/%$(EXE_SUFFIX)) \
+ $(test_PROGRAMS:%=$(USER_TESTS_DIR)/%$(EXE_SUFFIX)) \
+ $(bin_SCRIPTS:%=$(USER_BIN_DIR)/%)
# Special rules for CMETRIC generated headers
$(foreach prog,$(bin_PROGRAMS),$(eval $(call PROGRAM_template,$(prog),$(USER_BIN_DIR),$(EXE_SUFFIX))))
+$(foreach script,$(bin_SCRIPTS),$(eval $(call SCRIPT_template,$(script),$(USER_BIN_DIR))))
+
+
$(foreach lib,$(lib_LIBRARIES),$(eval $(call LIBRARY_template,$(lib))))
$(foreach lib,$(shared_LIBRARIES),$(eval $(call SOLIB_template,$(lib))))
-include $(USER_OBJS_DIR)/*.d
+#FIXME: This doesn't include dependencies of source files from
+#subdirectories (i.s. *_SOURCES=dir/file.c. I'm currently not sure,
+#how to handle this.
endif # USER_RULE_TEMPLATES
include $(RTL_DIR)/rtl.mk
KERN_CC = $(CC)
-kern_GCCLIB_DIR=$(shell LANG=C LC_ALL=C LC_MESSAGES=C $(CC) -print-search-dirs | sed -n -e 's/^install: \(.*\)$$/\1/p' )
+kern_GCCLIB_DIR=$(shell LANG=C LC_ALL=C LC_MESSAGES=C $(CC) -print-search-dirs | $(SED4OMK) -n -e 's/^install: \(.*\)$$/\1/p' )
INCLUDES := -I $(KERN_INCLUDE_DIR) $(INCLUDE) $(rtlinux_INCLUDES) $(kernel_INCLUDES)
#-DEXPORT_NO_SYMBOLS
c_o_kern_COMPILE = $(KERN_CC) -idirafter $(kern_GCCLIB_DIR)/include $(INCLUDES) $(CFLAGS) -DOMK_FOR_KERNEL -DEXPORT_SYMTAB -nostdinc
ifdef LINUX_CC
KERN_CC = $(LINUX_CC)
-kern_GCCLIB_DIR=$(shell LANG=C LC_ALL=C LC_MESSAGES=C $(LINUX_CC) -print-search-dirs | sed -n -e 's/^install: \(.*\)$$/\1/p' )
+kern_GCCLIB_DIR=$(shell LANG=C LC_ALL=C LC_MESSAGES=C $(LINUX_CC) -print-search-dirs | $(SED4OMK) -n -e 's/^install: \(.*\)$$/\1/p' )
else
KERN_CC = echo KERN_CC not defined - compilation skipped
endif
c_o_kern_COMPILE = $(KERN_CC) $(kernel_INCLUDES) -idirafter $(kern_GCCLIB_DIR)/include $(LINUX_CPPFLAGS) $(LINUX_CFLAGS) $(LINUX_CFLAGS_MODULE) -DOMK_FOR_KERNEL -DEXPORT_SYMTAB -nostdinc
cc_o_kern_COMPILE = $(KERN_CC) $(kernel_INCLUDES) -idirafter $(kern_GCCLIB_DIR)/include $(LINUX_CPPFLAGS) $(LINUX_CFLAGS) $(LINUX_CFLAGS_MODULE) -DOMK_FOR_KERNEL -DEXPORT_SYMTAB
-S_o_kern_COMPILE = $(KERN_CC) $(kernel_INCLUDES) -idirafter $(kern_GCCLIB_DIR)/include $(LINUX_CPPFLAGS) $(LINUX_AFLAGS) $(LINUX_CFLAGS_MODULE) -DOMK_FOR_KERNEL -DEXPORT_SYMTAB -nostdinc
+S_o_kern_COMPILE = $(KERN_CC) $(kernel_INCLUDES) -idirafter $(kern_GCCLIB_DIR)/include $(LINUX_CPPFLAGS) $(LINUX_AFLAGS) $(LINUX_AFLAGS_MODULE) -DOMK_FOR_KERNEL -DEXPORT_SYMTAB -nostdinc
KERN_EXE_SUFFIX := $(LINUX_MODULE_EXT)
-KERN_LDFLAGS = $(LINUX_LDFLAGS)
+KERN_LDFLAGS = $(LINUX_LDFLAGS) $(LINUX_LDFLAGS_MODULE)
ifdef LINUX_ARCH
KERN_ARCH = $(LINUX_ARCH)
else
# Check GCC version for kernel part of build
ifndef kern_CC_MAJOR_VERSION
-kern_CC_MAJOR_VERSION := $(shell $(KERN_CC) -dumpversion | sed -e 's/\([^.]\)\..*/\1/')
+kern_CC_MAJOR_VERSION := $(shell $(KERN_CC) -dumpversion | $(SED4OMK) -e 's/\([^.]\)\..*/\1/')
endif
# Prepare suitable define for dependency building
ifeq ($(kern_CC_MAJOR_VERSION),2)
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.c=%.o))
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cc=%.o))
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cxx=%.o))
+$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cpp=%.o))
$(1)_OBJS := $$(sort $$($(1)_OBJS))
KERN_OBJS += $$($(1)_OBJS)
@$(QUIET_CMD_ECHO) " LD [K] $$@"
$(Q) $$(KERN_LD) $$(KERN_LDFLAGS) -r $$($(1)_OBJS) -L$$(kern_GCCLIB_DIR) $$($(1)_LIBS:%=-l%) $$(KERN_LOADLIBES) -Map $(KERN_OBJS_DIR)/$(1).mod.map -o $$@
@echo "$(2)/$(1)$(KERN_LINK_SUFFIX): \\" >$(KERN_OBJS_DIR)/$(1).mod.d
- @sed -n -e 's/^LOAD \(.*\)$$$$/ \1 \\/p' $(KERN_OBJS_DIR)/$(1).mod.map >>$(KERN_OBJS_DIR)/$(1).mod.d
+ @$(SED4OMK) -n -e 's/^LOAD \(.*\)$$$$/ \1 \\/p' $(KERN_OBJS_DIR)/$(1).mod.map >>$(KERN_OBJS_DIR)/$(1).mod.d
@echo >>$(KERN_OBJS_DIR)/$(1).mod.d
@if [ "$(KERN_EXE_SUFFIX)" = ".ko" ] ; then \
echo $(1) >>$(KERN_MODPOST_DIR)/module-changes ; \
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.c=%.o))
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cc=%.o))
$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cxx=%.o))
+$(1)_OBJS += $$(filter %.o,$$($(1)_SOURCES:%.cpp=%.o))
$(1)_OBJS := $$(sort $$($(1)_OBJS))
KERN_OBJS += $$($(1)_OBJS)
fi
$(eval $(call omk_pass_template, library-pass,$(USER_OBJS_DIR),USER_RULE_TEMPLATES=y,$(lib_LIBRARIES)$(shared_LIBRARIES)))
-$(eval $(call omk_pass_template, binary-pass, $(USER_OBJS_DIR),USER_RULE_TEMPLATES=y,$(bin_PROGRAMS)$(utils_PROGRAMS)$(test_PROGRAMS)))
+$(eval $(call omk_pass_template, binary-pass, $(USER_OBJS_DIR),USER_RULE_TEMPLATES=y,$(bin_PROGRAMS)$(utils_PROGRAMS)$(test_PROGRAMS)$(bin_SCRIPTS)))
$(eval $(call omk_pass_template,clean,$(USER_OBJS_DIR),,always))
$(eval $(call omk_pass_template,install,$(USER_OBJS_DIR),,always))
$(foreach src,$(filter %.cxx,$(USER_SOURCES)),$(eval $(call COMPILE_cc_o_template,$(SOURCES_DIR)/$(src),$(src:%.cxx=%.o),)))
+$(foreach src,$(filter %.cpp,$(USER_SOURCES)),$(eval $(call COMPILE_cc_o_template,$(SOURCES_DIR)/$(src),$(src:%.cpp=%.o),)))
+
$(foreach src,$(filter %.S,$(USER_SOURCES)),$(eval $(call COMPILE_S_o_template,$(SOURCES_DIR)/$(src),$(src:%.S=%.o),)))
$(foreach src,$(filter %.c,$(USER_GEN_SOURCES)),$(eval $(call COMPILE_c_o_template,$(src),$(src:%.c=%.o),)))
$(foreach src,$(filter %.cxx,$(SOLIB_SOURCES)),$(eval $(call COMPILE_cc_o_template,$(SOURCES_DIR)/$(src),$(src:%.cxx=%.lo),$(SOLIB_PICFLAGS))))
+$(foreach src,$(filter %.cpp,$(SOLIB_SOURCES)),$(eval $(call COMPILE_cc_o_template,$(SOURCES_DIR)/$(src),$(src:%.cpp=%.lo),$(SOLIB_PICFLAGS))))
+
$(foreach src,$(filter %.S,$(SOLIB_SOURCES)),$(eval $(call COMPILE_S_o_template,$(SOURCES_DIR)/$(src),$(src:%.S=%.lo),$(SOLIB_PICFLAGS))))
$(foreach src,$(filter %.c,$(SOLIB_GEN_SOURCES)),$(eval $(call COMPILE_c_o_template,$(src),$(src:%.c=%.lo),$(SOLIB_PICFLAGS))))
$(foreach src,$(filter %.cxx,$(KERN_SOURCES)),$(eval $(call COMPILE_cc_o_kern_template,$(SOURCES_DIR)/$(src),$(src:%.cxx=%.o),)))
+$(foreach src,$(filter %.cpp,$(KERN_SOURCES)),$(eval $(call COMPILE_cc_o_kern_template,$(SOURCES_DIR)/$(src),$(src:%.cpp=%.o),)))
+
$(foreach src,$(filter %.S,$(USER_SOURCES)),$(eval $(call COMPILE_S_o_kern_template,$(SOURCES_DIR)/$(src),$(src:%.S=%.o),)))
endif
$(if $(DOXYGEN),@echo "/** @file */" >> "$(2).tmp")
@echo "#ifndef $(4)" >> "$(2).tmp"
@echo "#define $(4)" >> "$(2).tmp"
- @( $(foreach x, $(shell echo '$($(3))' | tr 'x\t ' 'x\n\n' | sed -e 's/^\([^ =]*\)\(=[^ ]\+\|\)$$/\1/' ), \
+ @( $(foreach x, $(shell echo '$($(3))' | tr 'x\t ' 'x\n\n' | $(SED4OMK) -e 's/^\([^ =]*\)\(=[^ ]\+\|\)$$/\1/' ), \
echo '$(x).$($(x))' ; ) echo ; ) | \
- sed -e '/^[^.]*\.n$$$$/d' -e '/^[^.]*\.$$$$/d' -e 's/^\([^.]*\)\.[ym]$$$$/\1.1/' | \
- sed -n -e 's/^\([^.]*\)\.\(.*\)$$$$/#define \1 \2/p' \
+ $(SED4OMK) -e '/^[^.]*\.n$$$$/d' -e '/^[^.]*\.$$$$/d' -e 's/^\([^.]*\)\.[ym]$$$$/\1.1/' | \
+ $(SED4OMK) -n -e 's/^\([^.]*\)\.\(.*\)$$$$/#define \1 \2/p' \
>> "$(2).tmp"
@echo "#endif /*$(4)*/" >> "$(2).tmp"
@touch "$$@"
$(QTDIR:%=QTDIR=%) CC=$(CC) CXX=$(CXX) \
LIBS+="-L$(USER_LIB_DIR)" DESTDIR=$(USER_BIN_DIR) \
INCLUDEPATH+="$(USER_INCLUDE_DIR)" \
+ DEPENDPATH+="$(USER_INCLUDE_DIR)" \
QMAKE_LFLAGS="-Wl,-rpath-link,$(USER_LIB_DIR) $$(QMAKE_LFLAGS)" \
$(SOURCES_DIR)/$(1)
@mv "$(SOURCES_LIST).tmp2" "$(SOURCES_LIST)"
@echo "$(SOURCES_LIST): \\" > "$(SOURCES_LIST_D).tmp2"
@cat "$(SOURCES_LIST_D).tmp"|grep -v "$(SOURCES_LIST_D).tmp"|sort|uniq|\
- sed -e 's/$$/\\/' >> "$(SOURCES_LIST_D).tmp2"
+ $(SED4OMK) -e 's/$$/\\/' >> "$(SOURCES_LIST_D).tmp2"
@rm "$(SOURCES_LIST_D).tmp"
@mv "$(SOURCES_LIST_D).tmp2" "$(SOURCES_LIST_D)"
endif
echo "$(addsuffix /,$(RELATIVE_DIR:$(SOURCES_LIST_DIR)/%=%))$(h)" >> "$(SOURCES_LIST).tmp";)
@$(foreach ch,$(config_include_HEADERS), \
echo "$(USER_INCLUDE_DIR:$(OUTPUT_DIR)/$(addsuffix /,$(SOURCES_LIST_DIR))%=%)/$(ch)" >> "$(SOURCES_LIST).tmp";)
- @$(foreach h,$(renamed_include_HEADERS),echo '$(h)'|sed -e 's|\(.*\)->.*|$(addsuffix /,$(RELATIVE_DIR:$(SOURCES_LIST_DIR)/%=%))\1|' >> "$(SOURCES_LIST).tmp";)
+ @$(foreach h,$(renamed_include_HEADERS),echo '$(h)'|$(SED4OMK) -e 's|\(.*\)->.*|$(addsuffix /,$(RELATIVE_DIR:$(SOURCES_LIST_DIR)/%=%))\1|' >> "$(SOURCES_LIST).tmp";)
@$(foreach bin,$(lib_LIBRARIES) $(shared_LIBRARIES) $(bin_PROGRAMS) $(test_PROGRAMS) $(utils_PROGRAMS) \
$(kernel_LIBRARIES) $(rtlinux_LIBRARIES) $(kernel_MODULES),\
$(foreach src,$(filter-out %.o,$($(bin)_SOURCES)),echo "$(addsuffix /,$(RELATIVE_DIR:$(SOURCES_LIST_DIR)/%=%))$(src)" >> "$(SOURCES_LIST).tmp";))
ifeq ($(MAKECMDGOALS),do-tags)
.PHONY: do-tags
-do-tags: $(shell sed -e '/^\#/d' $(SOURCES_LIST))
+do-tags: $(shell $(SED4OMK) -e '/^\#/d' $(SOURCES_LIST))
@$(QUIET_CMD_ECHO) " TAGS $(SOURCES_LIST_FN)"
$(Q)$(TAGS_CMD) $^
endif
cscope: $(SOURCES_LIST)
@$(QUIET_CMD_ECHO) " CSCOPE < $(SOURCES_LIST_FN)"
- $(Q)sed -e '/^#/d' $(SOURCES_LIST) > cscope.files
+ $(Q)$(SED4OMK) -e '/^#/d' $(SOURCES_LIST) > cscope.files
$(Q)cscope -b -icscope.files
#FIXME: see doc to -i in cscope(1)
set grid
set key left top
EOF
-%authors = ('Michal' => 'sojka',
- 'Martin' => '\(zidek\|mzi\|martin-eee\|martin-nb2\)',
- 'Filip' => 'jares',
- 'Jarda' => 'jarin',
- 'Jirka' => '\(jirka\|kubia\)',
- 'Marek' => '\(marek\|duch\)',
- 'Petr' => 'benes',
+%authors = ('Michal S.' => 'sojka',
+ #'Martin' => '\(zidek\|mzi\|martin-eee\|martin-nb2\)',
+ #'Filip' => 'jares',
+ #'Jarda' => 'jarin',
+ #'Jirka' => '\(jirka\|kubia\)',
+ #'Marek' => '\(marek\|duch\)',
+ #'Petr' => 'benes',
+ 'Matous' => '\(ehiker\|pokor\)',
+ 'Michal V.' => '\(zandar\|vokac\)',
"Celkem" => '.');
foreach $author(sort keys %authors) {
--- /dev/null
+Postup pro uspesnou kompilaci (zatim s puvodnim toolchainem od P. Pisy):
+
+# 1. Nejprve je nutne pridat do PATHu cestu k toolchainu - pro jeho velikost zatim nedavam do gitu, ma totiz 50MB
+2. Je nutne stahnout archiv s Qt: ftp://ftp.trolltech.com/qt/source/qt-everywhere-opensource-src-4.6.3.tar.gz a rozbalit do slozky soft/src/3rdparty/qt a to tak, aby qt byla korenova slozka
+# 3. nastavit platformu hosta v qt/Makefile.omk, pro 64bitu -platform qws/linux-x86_64-g++, pro 32bitu -platform qws/linux-x86-g++
+
+TODO
+1. Aplikace patchu pro opravu prace s pismy a s kurzorem - v Qt v rootfs v Gumstixu je opraveno
+2. Zahrnout cely postup do makefilu ?
\ No newline at end of file
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+SUBDIRS = $(ALL_OMK_SUBDIRS)
--- /dev/null
+../_infrastructure/Makefile.rules
\ No newline at end of file
--- /dev/null
+CROSS_COMPILE = arm-linux-gnueabi-
+CC = $(CROSS_COMPILE)gcc
+CXX = $(CROSS_COMPILE)g++
+AS = $(CROSS_COMPILE)as
+AR = $(CROSS_COMPILE)ar
+STRIP = $(CROSS_COMPILE)strip
+
+CFLAGS = -Wall
+CXXFLAGS = $(CFLAGS)
+
+#INCLUDES = $(-I/home/zandar/programovani/embedded/qt4/target/include)
+
+IDL_COMPILER = orte-idl
+IDL_COMPILER=$(MAKERULES_DIR)/../host/_compiled/bin/orte-idl
+
+CONFIG_QT4_DIR = $(OUTPUT_DIR)/_compiled/qt
+CONFIG_OC_ETH_ORTE_IDL=n
+LN_HEADERS=y
+OMIT_KERNEL_PASSES=y
+CFLAGS = -O2 -g -Wall
+CXXFLAGS = -O2 -g -Wall
+
--- /dev/null
+../../src/display-qt/
\ No newline at end of file
--- /dev/null
+../../src/orte
\ No newline at end of file
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+library-pass_HOOKS = install-qt
+
+config.status: $(SOURCES_DIR)/qt-src/configure
+ cp -a $(SOURCES_DIR)/linux-arm-gumstix-g++ $(SOURCES_DIR)/qt-src/mkspecs/qws
+ cp -arv $(SOURCES_DIR)linux.conf $(SOURCES_DIR)/qt-src/mkspecs/common
+ $< -embedded arm --prefix=$(OUTPUT_DIR)/_compiled/qt -xplatform qws/linux-arm-gumstix-g++ -opensource -confirm-license -little-endian -qt-gfx-linuxfb -qt3support -opengl es2 -depths 16,24,32 -plugin-gfx-powervr -qtlibinfix E -plugin-mouse-tslib -qt-mouse-pc -qt-mouse-linuxinput -qt-kbd-tty -qt-kbd-linuxinput -DQT_KEYPAD_NAVIGATION
+
+lib/libQtCoreE.so: config.status
+ $(MAKE)
+
+$(OUTPUT_DIR)/_compiled/qt/libQtCoreE.so: lib/libQtCoreE.so
+ echo $@: $^
+ $(MAKE) install
+ touch $@
+
+.PHONY: install-qt
+install-qt: $(OUTPUT_DIR)/_compiled/qt/libQtCoreE.so
--- /dev/null
+#
+# qmake configuration for building with arm-linux-gnueabi-g++ - used
+# for Eurobot team Flamingos. It would be better if we use the same
+# settings from OMK's config.target, but I do not know how to achieve
+# that.
+#
+
+include(../../common/g++.conf)
+include(../../common/linux.conf)
+include(../../common/qws.conf)
+
+# modifications to g++.conf
+QMAKE_CC = arm-linux-gnueabi-gcc
+QMAKE_CXX = arm-linux-gnueabi-g++
+QMAKE_LINK = arm-linux-gnueabi-g++
+QMAKE_LINK_SHLIB = arm-linux-gnueabi-g++
+
+# modifications to linux.conf
+QMAKE_AR = arm-linux-gnueabi-ar cqs
+QMAKE_OBJCOPY = arm-linux-gnueabi-objcopy
+QMAKE_STRIP = arm-linux-gnueabi-strip
+
+load(qt_config)
--- /dev/null
+/****************************************************************************
+**
+** Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: Nokia Corporation (qt-info@nokia.com)
+**
+** This file is part of the qmake spec of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** Commercial Usage
+** Licensees holding valid Qt Commercial licenses may use this file in
+** accordance with the Qt Commercial License Agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and Nokia.
+**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 2.1 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 2.1 requirements
+** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
+**
+** In addition, as a special exception, Nokia gives you certain additional
+** rights. These rights are described in the Nokia Qt LGPL Exception
+** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 3.0 as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU General Public License version 3.0 requirements will be
+** met: http://www.gnu.org/copyleft/gpl.html.
+**
+** If you have questions regarding the use of this file, please contact
+** Nokia at qt-info@nokia.com.
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "../../linux-g++/qplatformdefs.h"
--- /dev/null
+#
+# qmake configuration for common linux
+#
+
+QMAKE_CFLAGS_THREAD += -D_REENTRANT
+QMAKE_CXXFLAGS_THREAD += $$QMAKE_CFLAGS_THREAD
+
+QMAKE_INCDIR =
+QMAKE_LIBDIR =
+QMAKE_INCDIR_X11 = /usr/X11R6/include
+QMAKE_LIBDIR_X11 = /usr/X11R6/lib
+QMAKE_INCDIR_QT = $$[QT_INSTALL_HEADERS]
+QMAKE_LIBDIR_QT = $$[QT_INSTALL_LIBS]
+QMAKE_INCDIR_OPENGL = /usr/X11R6/include
+QMAKE_LIBDIR_OPENGL = /usr/X11R6/lib
+QMAKE_INCDIR_OPENGL_ES1 = $$QMAKE_INCDIR_OPENGL
+QMAKE_LIBDIR_OPENGL_ES1 = $$QMAKE_LIBDIR_OPENGL
+QMAKE_INCDIR_OPENGL_ES1CL = $$QMAKE_INCDIR_OPENGL
+QMAKE_LIBDIR_OPENGL_ES1CL = $$QMAKE_LIBDIR_OPENGL
+QMAKE_INCDIR_OPENGL_ES2 = $$QMAKE_INCDIR_OPENGL
+QMAKE_LIBDIR_OPENGL_ES2 = $$QMAKE_LIBDIR_OPENGL
+QMAKE_INCDIR_EGL =
+QMAKE_LIBDIR_EGL =
+QMAKE_INCDIR_OPENVG =
+QMAKE_LIBDIR_OPENVG =
+
+QMAKE_LIBS = -Wl,-rpath-link,SEDME/lib -lglib-2.0
+QMAKE_LIBS_DYNLOAD = -ldl
+QMAKE_LIBS_X11 = -lXext -lX11 -lm
+QMAKE_LIBS_X11SM = -lSM -lICE
+QMAKE_LIBS_NIS = -lnsl
+QMAKE_LIBS_EGL = -lEGL -lIMGegl -lsrv_um
+QMAKE_LIBS_OPENGL = -lEGL -lGLES_CM -lIMGegl -lsrv_um
+QMAKE_LIBS_OPENGL_QT = -lEGL -lGLES_CM -lIMGegl -lsrv_um
+QMAKE_LIBS_OPENGL_ES1 = -lEGL -lGLES_CM -lIMGegl -lsrv_um
+QMAKE_LIBS_OPENGL_ES1CL = -lGLES_CL
+QMAKE_LIBS_OPENGL_ES2 = -lEGL -lGLESv2 -lGLES_CM -lIMGegl -lsrv_um
+QMAKE_LIBS_OPENVG = -lOpenVG
+QMAKE_LIBS_THREAD = -lpthread
+
+QMAKE_MOC = $$[QT_INSTALL_BINS]/moc
+QMAKE_UIC = $$[QT_INSTALL_BINS]/uic
+
+QMAKE_AR = ar cqs
+QMAKE_OBJCOPY = objcopy
+QMAKE_RANLIB =
+
+QMAKE_TAR = tar -cf
+QMAKE_GZIP = gzip -9f
+
+QMAKE_COPY = cp -f
+QMAKE_COPY_FILE = $(COPY)
+QMAKE_COPY_DIR = $(COPY) -r
+QMAKE_MOVE = mv -f
+QMAKE_DEL_FILE = rm -f
+QMAKE_DEL_DIR = rmdir
+QMAKE_STRIP = strip
+QMAKE_STRIPFLAGS_LIB += --strip-unneeded
+QMAKE_CHK_DIR_EXISTS = test -d
+QMAKE_MKDIR = mkdir -p
+QMAKE_INSTALL_FILE = install -m 644 -p
+QMAKE_INSTALL_PROGRAM = install -m 755 -p
+
+DEFINES += QT_QWS_CLIENTBLIT
+
+include(unix.conf)
--- /dev/null
+../../../src/3rdparty/qt
\ No newline at end of file
--- /dev/null
+../../src/robodim
\ No newline at end of file
--- /dev/null
+../../src/robomath
\ No newline at end of file
--- /dev/null
+../../src/types
\ No newline at end of file
+++ /dev/null
-../../src/pxmc
\ No newline at end of file
--- /dev/null
+../../src/pxmc/libs4c/pxmc_bsp/
\ No newline at end of file
--- /dev/null
+../../src/pxmc/libs4c/pxmc_core/
\ No newline at end of file
--- /dev/null
+../../src/barcode/
\ No newline at end of file
CFLAGS += -fpic
endif
CONFIG_OC_ULUTKERN=n
+CONFIG_APP_USB_SENDHEX=y
--- /dev/null
+../../src/display-qt
\ No newline at end of file
+++ /dev/null
-../../src/robodata
\ No newline at end of file
--- /dev/null
+../../src/ulan-app/app-host/usb_sendhex/
\ No newline at end of file
-../../src/sysless/app/arm/eb_ebb
\ No newline at end of file
+../../src/eb_ebb
\ No newline at end of file
--- /dev/null
+../../src/eb_jaws_11/
\ No newline at end of file
--- /dev/null
+/home/ehiker/git_ebot/src/eb_lift
\ No newline at end of file
--- /dev/null
+../../src/eb_lift_11/
\ No newline at end of file
--- /dev/null
+../../src/eb_test
\ No newline at end of file
AS = $(CROSS_COMPILE)as
AR = $(CROSS_COMPILE)ar
+export PKG_CONFIG_PATH=$(OUTPUT_DIR)/_compiled/lib/pkgconfig
#CONFIG_OC_ETH_ORTE_IDL=y
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+include-pass_HOOKS = install-libusb
+
+export CC
+export CFLAGS
+
+PREFIX=$(OUTPUT_DIR)/_compiled
+
+install-libusb: $(PREFIX)/lib/pkgconfig/libusb-1.0.pc
+
+config.status:
+ unset CONFIG_FILES; \
+ $(srcdir)/src/configure --host=powerpc-linux-gnu --prefix=$(PREFIX)
+
+libusb-1.0.pc: config.status
+ $(MAKE)
+
+$(PREFIX)/lib/pkgconfig/libusb-1.0.pc: libusb-1.0.pc
+ $(MAKE) install
--- /dev/null
+../../../src/3rdparty/libusb
\ No newline at end of file
update_submodule src/sysless
update_submodule src/orte
+update_submodule src/ulan-app
update_submodule src/ulut
update_submodule robot-root
update_submodule src/v4l/v4l-utils
-#update_submodule src/pxmc
+update_submodule src/pxmc
+update_submodule src/3rdparty/libusb && ( cd src/3rdparty/libusb && ./autogen.sh && make distclean )
#update_submodule src/linux-shark
echo "make default-config"
make -C $i default-config | grep -v default-config-pass
done
-
--- /dev/null
+#!/bin/sh
+myip=$(ip a|grep -o 'inet 10\.1\.1\.[0-9]\+'|cut -d ' ' -f 2)
+mypath=$(readlink --canonicalize $PWD/$(dirname $0)/../build/gumstix/_compiled)
+
+if [ ! -d "$mypath" ]; then
+ echo "$0: error: Cannot find $mypath"
+ exit 1
+fi
+
+if [ -z "$myip" ]; then
+ echo "$0: error: You do not seem to be connected to the GUMSTIX"
+ echo "Your IP address should be 10.1.1.xxx"
+ exit 1
+fi
+
+set -x
+
+if ! grep -q $mypath /var/lib/nfs/etab; then
+ sudo exportfs -o ro,no_subtree_check,no_root_squash 10.1.1.2:$mypath
+fi
+
+mount_point="/tmp/$USER@$myip"
+
+if [ "$1" = "-u" ]; then
+ cmd="umount $mount_point"
+else
+ cmd="mkdir -p $mount_point && mount -o nolock $myip:$mypath $mount_point"
+fi
+
+ssh root@10.1.1.2 $cmd
+if [ "$1" = "-c" ]; then
+ ssh root@10.1.1.2 /home/bin/copy_from $mount_point
+fi
--- /dev/null
+.pc/
+build/bitbake.lock
+build/cache/
+build/conf/sanity_info
+build/downloads/
+build/out-eglibc/
+build/sstate-cache/
--- /dev/null
+all: gumstix toradex
+
+init:
+ cd .. && git submodule update --init openembedded
+ ln -sf ../bitbake openembedded-core/bitbake
+ if test -f meta-angstrom/recipes-core/systemd/systemd_206.bbappend; then \
+ rm -f meta-angstrom/recipes-core/systemd/systemd_206.bbappend; \
+ fi
+ if test -f meta-angstrom/recipes-tweaks/connman/connman_1.17.bbappend; then \
+ mv meta-angstrom/recipes-tweaks/connman/connman_1.17.bbappend \
+ meta-angstrom/recipes-tweaks/connman/connman_1.12.bbappend; \
+ fi
+ if test -f meta-toradex-community/recipes-bsp/u-boot/u-boot-fw-utils_2013.07.bbappend; then \
+ mv meta-toradex-community/recipes-bsp/u-boot/u-boot-fw-utils_2013.07.bbappend \
+ meta-toradex-community/recipes-bsp/u-boot/u-boot-fw-utils_2013.01.01.bbappend; \
+ fi
+ if quilt unapplied; then quilt push -a; fi
+
+gumstix: init
+ bash -c ". openembedded-core/oe-init-build-env && MACHINE=overo bitbake console-image"
+
+toradex: init
+ bash -c ". openembedded-core/oe-init-build-env && MACHINE=colibri-t20 bitbake console-image"
--- /dev/null
+Subproject commit bbb4fa427739912ff3b87379bf629066f6662458
--- /dev/null
+# LAYER_CONF_VERSION is increased each time build/conf/bblayers.conf
+# changes incompatibly
+LCONF_VERSION = "5"
+
+BBPATH = "${TOPDIR}"
+BBFILES ?= ""
+
+# These layers hold recipe metadata not found in OE-core, but lack any machine or distro content
+BASELAYERS ?= " \
+ ${TOPDIR}/../meta-linaro/meta-linaro-toolchain \
+ ${TOPDIR}/../meta-openembedded/toolchain-layer \
+ ${TOPDIR}/../meta-openembedded/meta-oe \
+ ${TOPDIR}/../meta-openembedded/meta-efl \
+ ${TOPDIR}/../meta-openembedded/meta-gpe \
+ ${TOPDIR}/../meta-openembedded/meta-gnome \
+ ${TOPDIR}/../meta-openembedded/meta-xfce \
+ ${TOPDIR}/../meta-openembedded/meta-initramfs \
+ ${TOPDIR}/../meta-openembedded/meta-systemd \
+ ${TOPDIR}/../meta-openembedded/meta-networking \
+ ${TOPDIR}/../meta-openembedded/meta-multimedia \
+"
+
+# These layers hold machine specific content, aka Board Support Packages
+BSPLAYERS ?= " \
+ ${TOPDIR}/../meta-toradex-community \
+ ${TOPDIR}/../meta-gumstix-community \
+ ${TOPDIR}/../meta-ti \
+"
+
+BBLAYERS ?= " \
+ ${TOPDIR}/../meta-angstrom \
+ ${BSPLAYERS} \
+ ${BASELAYERS}\
+ ${TOPDIR}/../openembedded-core/meta \
+"
--- /dev/null
+#
+# This file is your local configuration file and is where all local user settings
+# are placed. The comments in this file give some guide to the options a new user
+# to the system might want to change but pretty much any configuration option can
+# be set in this file. More adventurous users can look at local.conf.extended
+# which contains other examples of configuration which can be placed in this file
+# but new users likely won't need any of them initially.
+#
+# Lines starting with the '#' character are commented out and in some cases the
+# default values are provided as comments to show people example syntax. Enabling
+# the option is a question of removing the # character and making any change to the
+# variable as required.
+
+#
+# Parallelism Options
+#
+# These two options control how much parallelism BitBake should use. The first
+# option determines how many tasks bitbake should run in parallel:
+#
+BB_NUMBER_THREADS = "8"
+#
+# The second option controls how many processes make should run in parallel when
+# running compile tasks:
+#
+PARALLEL_MAKE = "-j 4"
+#
+# For a quad-core machine, BB_NUMBER_THREADS = "4", PARALLEL_MAKE = "-j 4" would
+# be appropriate for example.
+
+#
+# Machine Selection
+#
+# You need to select a specific machine to target the build with. There are a selection
+# of emulated machines available which can boot and run in the QEMU emulator:
+#
+#MACHINE ?= "qemuarm"
+#MACHINE ?= "qemumips"
+#MACHINE ?= "qemuppc"
+#MACHINE ?= "qemux86"
+#MACHINE ?= "qemux86-64"
+#
+# This sets the default machine to be qemux86 if no other machine is selected:
+
+#
+# Where to place downloads
+#
+# During a first build the system will download many different source code tarballs
+# from various upstream projects. This can take a while, particularly if your network
+# connection is slow. These are all stored in DL_DIR. When wiping and rebuilding you
+# can preserve this directory to speed up this part of subsequent builds. This directory
+# is safe to share between multiple builds on the same machine too.
+#
+# The default is a downloads directory under TOPDIR which is the build directory.
+#
+DL_DIR ?= "${TOPDIR}/downloads"
+
+#
+# Where to place shared-state files
+#
+# BitBake has the capability to accelerate builds based on previously built output.
+# This is done using "shared state" files which can be thought of as cache objects
+# and this option determines where those files are placed.
+#
+# You can wipe out TMPDIR leaving this directory intact and the build would regenerate
+# from these files if no changes were made to the configuration. If changes were made
+# to the configuration, only shared state files where the state was still valid would
+# be used (done using checksums).
+#
+# The default is a sstate-cache directory under TOPDIR.
+#
+#SSTATE_DIR ?= "${TOPDIR}/sstate-cache"
+
+#
+# Where to place the build output
+#
+# This option specifies where the bulk of the building work should be done and
+# where BitBake should place its temporary files and output. Keep in mind that
+# this includes the extraction and compilation of many applications and the toolchain
+# which can use Gigabytes of hard disk space.
+#
+# The default is a tmp directory under TOPDIR.
+#
+#TMPDIR = "${TOPDIR}/tmp"
+TMPDIR = "${TOPDIR}/out"
+#
+# Package Management configuration
+#
+# This variable lists which packaging formats to enable. Multiple package backends
+# can be enabled at once and the first item listed in the variable will be used
+# to generate the root filesystems.
+# Options are:
+# - 'package_deb' for debian style deb files
+# - 'package_ipk' for ipk files are used by opkg (a debian style embedded package manager)
+# - 'package_rpm' for rpm style packages
+# E.g.: PACKAGE_CLASSES ?= "package_rpm package_deb package_ipk"
+# We default to ipk:
+PACKAGE_CLASSES ?= "package_ipk"
+
+#
+# SDK/ADT target architecture
+#
+# This variable specified the architecture to build SDK/ADT items for and means
+# you can build the SDK packages for architectures other than the machine you are
+# running the build on (i.e. building i686 packages on an x86_64 host._
+# Supported values are i686 and x86_64
+# SDKMACHINE ?= "i686"
+
+#
+# Extra image configuration defaults
+#
+# The EXTRA_IMAGE_FEATURES variable allows extra packages to be added to the generated
+# images. Some of these options are added to certain image types automatically. The
+# variable can contain the following options:
+# "dbg-pkgs" - add -dbg packages for all installed packages
+# (adds symbol information for debugging/profiling)
+# "dev-pkgs" - add -dev packages for all installed packages
+# (useful if you want to develop against libs in the image)
+# "tools-sdk" - add development tools (gcc, make, pkgconfig etc.)
+# "tools-debug" - add debugging tools (gdb, strace)
+# "tools-profile" - add profiling tools (oprofile, exmap, lttng valgrind (x86 only))
+# "tools-testapps" - add useful testing tools (ts_print, aplay, arecord etc.)
+# "debug-tweaks" - make an image suitable for development
+# e.g. ssh root access has a blank password
+# There are other application targets that can be used here too, see
+# meta/classes/image.bbclass and meta/classes/core-image.bbclass for more details.
+# We default to enabling the debugging tweaks.
+EXTRA_IMAGE_FEATURES = "debug-tweaks"
+
+#
+# Additional image features
+#
+# The following is a list of additional classes to use when building images which
+# enable extra features. Some available options which can be included in this variable
+# are:
+# - 'buildstats' collect build statistics
+# - 'image-mklibs' to reduce shared library files size for an image
+# - 'image-prelink' in order to prelink the filesystem image
+# - 'image-swab' to perform host system intrusion detection
+# NOTE: if listing mklibs & prelink both, then make sure mklibs is before prelink
+# NOTE: mklibs also needs to be explicitly enabled for a given image, see local.conf.extended
+USER_CLASSES ?= "buildstats image-mklibs image-prelink"
+
+#
+# Runtime testing of images
+#
+# The build system can test booting virtual machine images under qemu (an emulator)
+# after any root filesystems are created and run tests against those images. To
+# enable this uncomment this line
+#IMAGETEST = "qemu"
+#
+# This variable controls which tests are run against virtual images if enabled
+# above. The following would enable bat, boot the test case under the sanity suite
+# and perform toolchain tests
+#TEST_SCEN = "sanity bat sanity:boot toolchain"
+#
+# Because of the QEMU booting slowness issue (see bug #646 and #618), the
+# autobuilder may suffer a timeout issue when running sanity tests. We introduce
+# the variable TEST_SERIALIZE here to reduce the time taken by the sanity tests.
+# It is set to 1 by default, which will boot the image and run cases in the same
+# image without rebooting or killing the machine instance. If it is set to 0, the
+# image will be copied and tested for each case, which will take longer but be
+# more precise.
+#TEST_SERIALIZE = "1"
+
+#
+# Interactive shell configuration
+#
+# Under certain circumstances the system may need input from you and to do this it
+# can launch an interactive shell. It needs to do this since the build is
+# multithreaded and needs to be able to handle the case where more than one parallel
+# process may require the user's attention. The default is iterate over the available
+# terminal types to find one that works.
+#
+# Examples of the occasions this may happen are when resolving patches which cannot
+# be applied, to use the devshell or the kernel menuconfig
+#
+# Supported values are auto, gnome, xfce, rxvt, screen, konsole (KDE 3.x only), none
+# Note: currently, Konsole support only works for KDE 3.x due to the way
+# newer Konsole versions behave
+#OE_TERMINAL = "auto"
+# By default disable interactive patch resolution (tasks will just fail instead):
+PATCHRESOLVE = "noop"
+
+#
+# Shared-state files from other locations
+#
+# As mentioned above, shared state files are prebuilt cache data objects which can
+# used to accelerate build time. This variable can be used to configure the system
+# to search other mirror locations for these objects before it builds the data itself.
+#
+# This can be a filesystem directory, or a remote url such as http or ftp. These
+# would contain the sstate-cache results from previous builds (possibly from other
+# machines). This variable works like fetcher MIRRORS/PREMIRRORS and points to the
+# cache locations to check for the shared objects.
+#SSTATE_MIRRORS ?= "\
+#file://.* http://someserver.tld/share/sstate/ \n \
+#file://.* file:///some/local/dir/sstate/"
+
+# CONF_VERSION is increased each time build/conf/ changes incompatibly and is used to
+# track the version of this file when it was generated. This can safely be ignored if
+# this doesn't mean anything to you.
+CONF_VERSION = "1"
+
+# Delete the the source/object/binary files once a package is built to preserve disk space
+INHERIT += "rm_work"
+
+# Use this distro
+DISTRO = "angstrom-v2013.06"
+
+# What image type(s) are to be built?
+IMAGE_FSTYPES += "tar.bz2"
+
+# Don't generate the mirror tarball for SCM repos, the snapshot is enough
+# BB_GENERATE_MIRROR_TARBALLS = "0"
+
+PREFERRED_PROVIDER_psplash-support = "psplash-angstrom"
+PREFERRED_PROVIDER_eglibc = "eglibc"
--- /dev/null
+Subproject commit 99039d846566f63247ca237c040922b97d033c60
--- /dev/null
+Subproject commit 963180a674dcd83ad028d9ae2e741ae8ff972257
--- /dev/null
+Subproject commit c9f78d6eff14ada3f2bc960e81883b27a10afecc
--- /dev/null
+Subproject commit 44754206632dd5b0725aeb43e99e4ff9e0245dca
--- /dev/null
+Subproject commit a22618208949997cf4b6082c83ad49ab2e570d9c
--- /dev/null
+Subproject commit a70c67a024cefebbaab3563572fabadf33448d06
--- /dev/null
+Subproject commit 3549f5f203363302256848bb33c05c4fd4871948
--- /dev/null
+Index: openembedded/meta-toradex-community/recipes-graphics/mesa/mesa-grate_git.bb
+===================================================================
+--- openembedded.orig/meta-toradex-community/recipes-graphics/mesa/mesa-grate_git.bb 2013-11-19 21:52:14.539440657 +0100
++++ openembedded/meta-toradex-community/recipes-graphics/mesa/mesa-grate_git.bb 2013-11-19 22:03:22.059468331 +0100
+@@ -2,6 +2,7 @@
+
+ SUMMARY = "tegra fork of mesa: A free implementation of the OpenGL API"
+
++LICENSE = "Proprietary"
+ LIC_FILES_CHKSUM = "file://docs/license.html;md5=f69a4626e9efc40fa0d3cc3b02c9eacf"
+
+ SRCREV = "1ac1285aa42a3b54e98fa8a56377343e6ae3ad40"
--- /dev/null
+Index: openembedded/meta-ti/recipes-bsp/powervr-drivers/omap3-sgx-modules_4.05.00.03.bb
+===================================================================
+--- openembedded.orig/meta-ti/recipes-bsp/powervr-drivers/omap3-sgx-modules_4.05.00.03.bb 2013-11-19 21:46:46.311427049 +0100
++++ openembedded/meta-ti/recipes-bsp/powervr-drivers/omap3-sgx-modules_4.05.00.03.bb 2013-11-19 21:59:39.423459102 +0100
+@@ -10,6 +10,7 @@
+
+ inherit module
+
++MACHINE_KERNEL_PR ?= "r0"
+ MACHINE_KERNEL_PR_append = "b"
+ PR = "${MACHINE_KERNEL_PR}"
+
+Index: openembedded/meta-ti/recipes-kernel/linux/linux-mainline_3.2.bb
+===================================================================
+--- openembedded.orig/meta-ti/recipes-kernel/linux/linux-mainline_3.2.bb 2013-11-19 21:46:46.331427050 +0100
++++ openembedded/meta-ti/recipes-kernel/linux/linux-mainline_3.2.bb 2013-11-19 22:00:00.547459976 +0100
+@@ -9,6 +9,7 @@
+ SRCREV_pn-${PN} = "1f873aca1c7aa7a574b276c040d304d16f1dbfa4"
+
+ # The main PR is now using MACHINE_KERNEL_PR, for omap3 see conf/machine/include/omap3.inc
++MACHINE_KERNEL_PR ?= "r0"
+ MACHINE_KERNEL_PR_append = "b"
+ PR = "${MACHINE_KERNEL_PR}"
+
--- /dev/null
+meta-ti-machine-kernel-pr.patch
+mesa-grate-license.patch
+toradex-kernel-git.patch
--- /dev/null
+Index: openembedded/meta-toradex-community/recipes-kernel/linux/linux-toradex_git.bb
+===================================================================
+--- openembedded.orig/meta-toradex-community/recipes-kernel/linux/linux-toradex_git.bb 2013-11-19 22:52:45.221743748 +0100
++++ openembedded/meta-toradex-community/recipes-kernel/linux/linux-toradex_git.bb 2013-11-19 22:57:42.625235839 +0100
+@@ -14,7 +14,7 @@
+
+ PV = "${LINUX_VERSION}+gitr${SRCREV}"
+ S = "${WORKDIR}/git"
+-SRC_URI = "git://git.toradex.com/linux-toradex.git;protocol=git;branch=colibri"
++SRC_URI = "git://git.toradex.com/linux-toradex.git;protocol=http;branch=colibri"
+ # Patch to set parallel RGB (aka LVDS-1) to full-hd
+ #SRC_URI += "file://full-hd.patch "
+
--- /dev/null
+Subproject commit ab9cd5a7be637f7b793987971a706b1d11c27ded
--- /dev/null
+Subproject commit 0828c69ad805406568f6eef2bffbbd54b04dea41
# the \include command).
EXAMPLE_PATH = pathplan/test/ \
- fsm/example
+ fsm/example \
+ .
# If the value of the EXAMPLE_PATH tag contains directories, you can use the
# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
# commands irrespective of the value of the RECURSIVE tag.
# Possible values are YES and NO. If left blank NO is used.
-EXAMPLE_RECURSIVE = NO
+EXAMPLE_RECURSIVE = YES
# The IMAGE_PATH tag can be used to specify one or more files or
# directories that contain image that are included in the documentation (see
--- /dev/null
+all doc:
+ doxygen
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+#bin_PROGRAMS = barcode
+
+barcode_SOURCES = barcode.c
+
+include_HEADERS = barcode.h
+
+lib_LOADLIBES = pthread roboorte robottype orte sercom
--- /dev/null
+#include <stdio.h>
+#include <unistd.h>
+#include <sercom.h>
+#include <string.h>
+#include <pthread.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <termios.h>
+#include "barcode.h"
+
+
+static struct sercom_data sercom;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+
+typedef enum {
+ AIM_OFF,
+ AIM_ON,
+ BEEP,
+ CMD_ACK,
+ CMD_NAK,
+ LED_OFF,
+ LED_ON,
+ PARAM_DEFAULTS,
+ PARAM_REQUEST,
+ PARAM_SEND,
+ REQUEST_REVISION,
+ SCAN_DISABLE,
+ SCAN_ENABLE,
+ SLEEP,
+ START_DECODE,
+ STOP_DECODE,
+ WAKEUP,
+ CUSTOM_DEFAULTS
+} commandes;
+
+
+unsigned char getOpcode(commandes command)
+{
+ switch (command)
+ {
+ case AIM_OFF:
+ return 0xC4;
+ break;
+ case AIM_ON:
+ return 0xC5;
+ break;
+ case BEEP:
+ return 0xE6;
+ break;
+ case CMD_ACK:
+ return 0xD0;
+ break;
+ case CMD_NAK:
+ return 0xD1;
+ break;
+ case LED_OFF:
+ return 0xE8;
+ break;
+ case LED_ON:
+ return 0xE7;
+ break;
+ case PARAM_DEFAULTS:
+ return 0xC8;
+ break;
+ case PARAM_REQUEST:
+ return 0xC7;
+ break;
+ case PARAM_SEND:
+ return 0xC6;
+ break;
+ case REQUEST_REVISION:
+ return 0xA3;
+ break;
+ case SCAN_DISABLE:
+ return 0xEA;
+ break;
+ case SCAN_ENABLE:
+ return 0xE9;
+ break;
+ case SLEEP:
+ return 0xEB;
+ break;
+ case START_DECODE:
+ return 0xE4;
+ break;
+ case STOP_DECODE:
+ return 0xE5;
+ break;
+ case WAKEUP:
+ return 0x00;
+ break;
+ case CUSTOM_DEFAULTS:
+ return 0x12;
+ break;
+ }
+}
+
+int send_command(commandes command, unsigned char *data, int dataLength)
+{
+ unsigned char length;
+ int i; //pomocne cyklitko
+
+ if (data==NULL) {
+ length=7;
+ }
+ else {
+ length=dataLength+6;
+ printf("\ndelka paketu je %d\n", length);
+ }
+
+ unsigned char message[length];
+
+ //delka zpravy
+ message[0]=length-2;
+ //Opcode
+ message[1]=getOpcode(command);
+ //odesilatel
+ message[2]=0x04;
+ //status
+ message[3]=0x00; //to se musi najit, co jaky bit dela
+
+ if (data!=NULL) {
+ //nahraju zpravu
+ for (i=0; i<dataLength; i++)
+ message[4+i]=data[i];
+ }
+ else {
+ //necham prazdna data
+ message[4]=0x00;
+ }
+
+ //spocte checksum
+ count_checksum(message, length);
+
+
+
+ printf("\ncelkovy paket je:\n");
+ for (i=0; i<length; i++)
+ printf("0x%0.2X ", message[i]);
+ printf("\n");
+
+ barcode_write(message, length);
+
+
+ return 0;
+}
+
+void count_checksum(unsigned char *message, int length)
+{
+ //to jako nevim jak spocitat
+ message[length-1]=0xAA;
+ message[length-2]=0xBB;
+}
+
+
+int barcode_sercom_init(char * tty, void(*sighandler)(int))
+{
+ int ret;
+ strcpy((char *)&sercom.devname, tty);
+ sercom.baudrate = 9600;
+ sercom.parity = SERCOM_PARNONE;
+ sercom.databits = 8;
+ sercom.stopbits = 1;
+ sercom.mode = 0;
+ sercom.sighandler = NULL;
+ ret = sercom_open(&sercom);
+ if (ret < 0) {
+ return -1;
+ }
+ else {
+#define EBTEST
+#ifdef EBTEST
+ /* this part is used for testing purposess with ebboard,
+ remove this when conected to real scanner */
+ /* toogle DTR and RTS signal to drive the ebboard from reset */
+ int status;
+
+ ioctl(sercom.fd, TIOCMGET, &status); /* get the serial port status */
+
+ status &= ~TIOCM_DTR;
+ status &= ~TIOCM_RTS;
+
+ ioctl(sercom.fd, TIOCMSET, &status); /* set the serial port status */
+#endif
+ printf("Open serial connection to barcode scanner!");
+ return 0;
+ }
+}
+
+
+int barcode_read(unsigned char *buff, int size)
+{
+ int ret;
+ pthread_mutex_lock(&mutex);
+ ret = read(sercom.fd, buff, size);
+ pthread_mutex_unlock(&mutex);
+
+ if (ret != size) {
+ printf("READ FAILED!\n");
+ return -1;
+ }
+ return ret;
+}
+
+int barcode_write(unsigned char *buff, int size)
+{
+ int ret;
+ int i,j;
+ pthread_mutex_lock(&mutex);
+ printf("sending command (%2d bytes): ", size);
+
+
+ ret = write(sercom.fd, buff, size);
+ pthread_mutex_unlock(&mutex);
+ if (ret != size) {
+ printf("uoled: WRITE FAILED!\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+int main(void)
+{
+ char rcmd[4];
+
+ barcode_sercom_init("/dev/ttyUSB0", NULL);
+
+
+ send_command(AIM_OFF, NULL, 0);
+
+
+ //cekam na odpoved, coz je delka prijimane zpravy
+ barcode_read(rcmd,1);
+
+ printf("\ndelka nadchazejici zpravy 0x%0.2X\n", rcmd[0]);
+
+
+
+ return 0;
+
+ while (1) {
+ barcode_read(rcmd, 4);
+ putchar(rcmd[0]);
+ putchar(rcmd[1]);
+ putchar(rcmd[2]);
+ putchar('\n');
+
+ //printf("%s", rcmd);
+ }
+
+ return 0;
+}
# -*- makefile -*-
-SUBDIRS = rozkuk ijgjpeg barcam
+#SUBDIRS = ijgjpeg barcam
#include <opencv/highgui.h>
// gnuplot fifo plot
-#include <c2gnuplot.h>
+#include "c2gnuplot.h"
extern "C" {
#include <roboorte_robottype.h>
--- /dev/null
+/*
+ * c2gnuplot.h
+ *
+ * Copyright 2007 Zoltan Kuscsik <kuscsik@gmail.com>
+ *
+ * This file is GPLv2 as found in COPYING.
+ */
+
+
+/*
+ @fille c2gnuplo.h
+ @author Z. Kuscsik
+ @date jan 2007
+ @version $Revision: 0.2 $
+ @brief C interface to gnuplot.
+
+ gnuplot is a freely available, command-driven graphical display tool for
+ Unix. It compiles and works quite well on a number of Unix flavours as
+ well as other operating systems. The following module enables sending
+ display requests to gnuplot through simple C calls.
+ This code is partially based on the gnuplot_i.h library:
+ http://ndevilla.free.fr/gnuplot/
+
+--------------------------------------------------------------------------*/
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/stat.h>
+#include <string>
+#include <stdarg.h>
+#include <map>
+//#include "operations.h"
+
+using namespace std;
+
+#define GNPL_COMM_SIZE 2000
+#define MAX_COMM_NAME_LENGTH 100
+#define MAX_PATH_NAME_LENGTH 1000
+#define tmp_name "/tmp/tmpdataXXXXXXX"
+unsigned int FIRST_INIT = 1;
+
+char command_list[][MAX_COMM_NAME_LENGTH] = {"gnuplot","ppmto4ym","mpeg2enc"};
+
+#define NUMBER_OF_EXTERNAL_COMMANDS (sizeof command_list)/MAX_COMM_NAME_LENGTH //number of external commands
+
+map<char,char> path_list;
+
+
+
+
+class gnuplot_window{
+ public:
+ gnuplot_window();
+ void splot_data(); //plot 3d data file
+ void splot_data(char * cmd, ...);
+ void plot_data(); //plot 2d data file
+ void plot_data(char * cmd, ...);
+ void flush(); //flush gnuplot data
+ void data(char * cmd, ...); //write data to gnuplot session
+ void command(char * cmd, ...); //set gnuplot variables and draw functions
+ void mpeg_out(char * cmd, ...);
+ char* fit_range;
+ char* fit_function;
+ void fit(char * cmd, ...);
+ float get_variable(char * cmd, ...);
+ private:
+ FILE *pipe;
+ FILE *input_param;
+ FILE *input;
+ char gnuplot_fifo[(sizeof tmp_name)];
+ char gnuplot_output_fifo[(sizeof tmp_name)];
+ };
+
+
+
+char * get_program_path(char * pname)
+{
+ int i, j, lg;
+ char * path;
+ static char buf[MAX_PATH_NAME_LENGTH];
+
+ /* Trivial case: try in CWD */
+ sprintf(buf, "./%s", pname) ;
+ if (access(buf, X_OK)==0) {
+ sprintf(buf, ".");
+ return buf ;
+ }
+ /* Try out in all paths given in the PATH variable */
+ buf[0] = 0;
+ path = getenv("PATH") ;
+ if (path!=NULL) {
+ for (i=0; path[i]; ) {
+ for (j=i ; (path[j]) && (path[j]!=':') ; j++);
+ lg = j - i;
+ strncpy(buf, path + i, lg);
+ if (lg == 0) buf[lg++] = '.';
+ buf[lg++] = '/';
+ strcpy(buf + lg, pname);
+ if (access(buf, X_OK) == 0) {
+ /* Found it! */
+ break ;
+ }
+ buf[0] = 0;
+ i = j;
+ if (path[i] == ':') i++ ;
+ }
+ } else {
+ fprintf(stderr, "PATH variable not set\n");
+ }
+ /* If the buffer is still empty, the command was not found */
+ if (buf[0] == 0) return NULL ;
+ /* Otherwise truncate the command name to yield path only */
+ lg = strlen(buf) - 1 ;
+ while (buf[lg]!='/') {
+ buf[lg]=0 ;
+ lg -- ;
+ }
+ buf[lg] = 0;
+ return buf ;
+}
+
+
+
+void gnuplot_window::mpeg_out(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+ strcat(local_cmd, "\n");
+ command("set term pbm color ");
+ command("set output '| ppmtoy4m -S 420mpeg2 | mpeg2enc -o %s",local_cmd);
+ fit_range="[*:*]";
+ fit_function="";
+
+}
+
+void gnuplot_window::data(char * cmd, ...)
+{
+
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+
+ if(FIRST_INIT)
+ {
+ FIRST_INIT=0;
+ }
+
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ strcat(local_cmd, "\n");
+ fputs(local_cmd, input);
+ fflush(input);
+}
+
+
+void gnuplot_window::command(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ strcat(local_cmd, "\n");
+
+fprintf(pipe,local_cmd);
+fflush(pipe);
+}
+
+void gnuplot_window::plot_data()
+{
+fprintf(pipe,"plot '%s'\n",gnuplot_fifo);
+fflush(pipe);
+input = fopen(gnuplot_fifo,"w");
+}
+
+void gnuplot_window::splot_data()
+{
+fprintf(pipe,"splot '%s'\n",gnuplot_fifo);
+fflush(pipe);
+input = fopen(gnuplot_fifo,"w");
+}
+
+void gnuplot_window::splot_data(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ strcat(local_cmd, "\n");
+
+
+fprintf(pipe,"splot '%s' %s\n",gnuplot_fifo,local_cmd);
+fflush(pipe);
+input = fopen(gnuplot_fifo,"w");
+}
+void gnuplot_window::plot_data(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ strcat(local_cmd, "\n");
+
+
+fprintf(pipe,"plot '%s' %s\n",gnuplot_fifo,local_cmd);
+fflush(pipe);
+input = fopen(gnuplot_fifo,"w");
+}
+
+/*
+ * Fit function
+ *
+ */
+
+void gnuplot_window::fit(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ strcat(local_cmd, "\n");
+
+ if(fit_function=="")
+ fprintf(stderr,"Function for datafit is not defined. Please define the fit_function parameter\n");
+
+ fprintf(pipe,"fit [%s] %s '%s' %s\n",fit_range, fit_function,gnuplot_fifo,local_cmd);
+ fflush(pipe);
+ input = fopen(gnuplot_fifo,"w");
+
+
+
+
+}
+
+void gnuplot_window::flush()
+{
+fclose(input);
+fflush(pipe);
+}
+
+
+gnuplot_window::gnuplot_window()
+{
+ int fd;
+ pipe=popen("/usr/bin/gnuplot 2>/dev/null","w");
+ setvbuf(pipe, NULL, _IONBF, 0 );
+
+ strcpy(gnuplot_fifo,tmp_name);
+ strcpy(gnuplot_output_fifo,tmp_name);
+ if((fd=mkstemp(gnuplot_fifo) ) < 0) { fprintf(stderr,"Creating temporary filename failed\n"); exit(0);}
+ close(fd); remove(gnuplot_fifo);
+
+ if (mkfifo(gnuplot_fifo, S_IRUSR | S_IWUSR ) != 0) {fprintf(stderr,"Make fifo file %s failed\n",gnuplot_fifo); exit(0);}
+
+
+ if((fd=mkstemp(gnuplot_output_fifo) ) < 0) { fprintf(stderr,"Creating temporary filename failed\n"); exit(0);}
+ close(fd); remove(gnuplot_output_fifo);
+
+ if (mkfifo(gnuplot_output_fifo, S_IRUSR | S_IWUSR ) != 0) {fprintf(stderr,"Make fifo file %s failed\n",gnuplot_output_fifo); exit(0);}
+
+ fflush(pipe);
+ command("set fit logfile '/dev/null'");
+
+}
+
+float gnuplot_window::get_variable(char * cmd, ...)
+{
+ va_list ap ;
+ char local_cmd[GNPL_COMM_SIZE];
+ va_start(ap, cmd);
+ vsprintf(local_cmd, cmd, ap);
+ va_end(ap);
+
+ float param;
+ command("set print '%s'",gnuplot_output_fifo);
+
+ command("print %s",local_cmd);
+
+ usleep(10000); /* There must be a little sleep because of asynchronous relation to the gnuplot pipe*/
+
+ input_param=fopen(gnuplot_output_fifo,"r");
+
+ fscanf(input_param,"%f",¶m);
+
+ fclose(input_param);
+
+ return param;
+}
+
return 0;
}
-int set_pwr_alert(struct robottype_orte_data *orte_data)
-{
- unsigned char data=0;
- data = orte_data->pwr_alert.value;
-
- can_send(CAN_PWR_ALERT, 1, &data);
-
- return 0;
-}
-
int send_can_msg(struct robottype_orte_data *orte_data)
{
return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
return 0;
}
-int set_vidle_cmd(uint16_t req_pos, char speed)
+int set_jaws_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data[2];
+ unsigned char data[3];
+
+ data[0] = orte_data->jaws_cmd.req_pos.left >> 8;
+ data[1] = orte_data->jaws_cmd.req_pos.left & 0xff;
+ data[2] = orte_data->jaws_cmd.speed.left;
+ can_send(CAN_JAW_LEFT_CMD, 3, data);
- data[0] = req_pos >> 8;
- data[1] = req_pos & 0xff;
- data[2] = speed;
- can_send(CAN_VIDLE_CMD, 3, data);
+ data[0] = orte_data->jaws_cmd.req_pos.right >> 8;
+ data[1] = orte_data->jaws_cmd.req_pos.right & 0xff;
+ data[2] = orte_data->jaws_cmd.speed.right;
+ can_send(CAN_JAW_RIGHT_CMD, 3, data);
return 0;
}
-/**
- * Sends #CAN_HOKUYO_PITCH message.
- *
- * - data = orte_data->pusher.pos
- */
-int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
+int set_lift_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data = orte_data->hokuyo_pitch.angle;
+ unsigned char data[4];
- can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
- return 0;
-}
+ data[0] = orte_data->lift_cmd.req_pos >> 8;
+ data[1] = orte_data->lift_cmd.req_pos & 0xff;
+ data[2] = orte_data->lift_cmd.speed;
+ data[3] = orte_data->lift_cmd.homing;
+ can_send(CAN_LIFT_CMD, 4, data);
+ return 0;
+}
int can_send(canid_t id, unsigned char length, unsigned char *data)
{
static int status_cnt = 0;
switch(frame.can_id) {
- /* voltage measurements from power board */
-
/* robot commands (start, ..) */
- case CAN_VIDLE_STATUS:
- orte->vidle_status.act_pos = (frame.data[0] << 8) | frame.data[1];
- orte->vidle_status.response = (frame.data[2] << 8) | frame.data[3];
- orte->vidle_status.flags = frame.data[4];
- ORTEPublicationSend(orte->publication_vidle_status);
+ case CAN_JAW_LEFT_STATUS:
+ orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
+ orte->jaws_status.response.left = (frame.data[2] << 8) | frame.data[3];
+ orte->jaws_status.flags.left = frame.data[4];
+ ORTEPublicationSend(orte->publication_jaws_status);
break;
+ case CAN_JAW_RIGHT_STATUS:
+ orte->jaws_status.act_pos.right = (frame.data[0] << 8) | frame.data[1];
+ orte->jaws_status.response.right = (frame.data[2] << 8) | frame.data[3];
+ orte->jaws_status.flags.right = frame.data[4];
+ ORTEPublicationSend(orte->publication_jaws_status);
+ break;
+ case CAN_LIFT_STATUS:
+ orte->lift_status.act_pos = (frame.data[0] << 8) | frame.data[1];
+ orte->lift_status.response = (frame.data[2] << 8) | frame.data[3];
+ orte->lift_status.flags = frame.data[4];
+ ORTEPublicationSend(orte->publication_lift_status);
+ break;
case CAN_ROBOT_CMD:
orte->robot_cmd.start_condition = frame.data[0];
ORTEPublicationSend(orte->publication_robot_cmd);
break;
case CAN_ROBOT_SWITCHES:
- orte->robot_switches.bumper_pressed = !!(frame.data[0] & CAN_SWITCH_BUMPER);
- orte->robot_switches.bumper_left = !!(frame.data[0] & CAN_SWITCH_LEFT);
- orte->robot_switches.bumper_right = !!(frame.data[0] & CAN_SWITCH_RIGHT);
orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
+ orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY ? 1 : 0);
ORTEPublicationSend(orte->publication_robot_switches);
break;
+ case CAN_ROBOT_BUMPERS:
+ orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
+ orte->robot_bumpers.bumper_left_across = (frame.data[0] & CAN_BUMPER_LEFT_ACROSS) ? 0 : 1;
+ orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
+ orte->robot_bumpers.bumper_right_across = (frame.data[0] & CAN_BUMPER_RIGHT_ACROSS) ? 0 : 1;
+ orte->robot_bumpers.bumper_rear_left = (frame.data[0] & CAN_BUMPER_REAR_LEFT) ? 0 : 1;
+ orte->robot_bumpers.bumper_rear_right = (frame.data[0] & CAN_BUMPER_REAR_RIGHT) ? 0 : 1;
+ ORTEPublicationSend(orte->publication_robot_bumpers);
+ break;
/* positioning by odometry */
case CAN_ODO_DATA:
- orte->odo_data.left =
+ orte->odo_data.right =
((frame.data[0]<<24)|
(frame.data[1]<<16)|
(frame.data[2]<<8));
- orte->odo_data.right =
+ orte->odo_data.left =
((frame.data[3]<<24)|
(frame.data[4]<<16)|
(frame.data[5]<<8));
/* positioning by odometry */
case CAN_MOTION_ODOMETRY_SIMPLE:
- orte->motion_irc.right =
+ orte->motion_irc.left =
((frame.data[0]<<24)|
(frame.data[1]<<16)|
(frame.data[2]<<8));
- orte->motion_irc.left =
+ orte->motion_irc.right =
((frame.data[3]<<24)|
(frame.data[4]<<16)|
(frame.data[5]<<8));
/* motion status */
case CAN_MOTION_STATUS:
- orte->motion_status.err_left =
+ orte->motion_status.err_left =
(frame.data[0]<<8)|(frame.data[1]);
- orte->motion_status.err_right =
+ orte->motion_status.err_right =
(frame.data[2]<<8)|(frame.data[3]);
if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
ORTEPublicationSend(orte->publication_motion_status);
status_cnt = 0;
}
break;
+ /* voltage measurements from power board */
case CAN_PWR_ADC1:
double volt33, voltBAT;
voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
ORTEPublicationSend(orte->publication_pwr_voltage);
break;
+
+ case CAN_PWR_ALERT:
+ orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
+ orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
+ orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
+ orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
+ orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
+ orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
+ orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
+
+ ORTEPublicationSend(orte->publication_pwr_alert);
+ break;
+
default:
//FIXME: add logging here (Filip)
// printf("received CAN msg with unknown id: %x\n",frame.can_id);
break;
}
}
-void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
-
- struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
- switch (info->status) {
- case NEW_DATA:
- set_pwr_alert(orte_data);
- break;
- case DEADLINE:
- //printf("ORTE deadline occurred - PWR_CTRL receive\n");
- break;
- }
-}
void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
switch (info->status) {
case NEW_DATA:
/* reversing motion direction, as it is different, than last year */
- orte_data->motion_speed.left *= -1;
- orte_data->motion_speed.right *=-1;
+ orte_data->motion_speed.left *= 1;
+ orte_data->motion_speed.right *= 1;
set_motor_speed(orte_data);
/*printf("motor cmd received\n");*/
break;
}
}
-void rcv_vidle_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_cmd_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_cmd_type *vidle_cmd = (struct vidle_cmd_type *)vinstance;
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- set_vidle_cmd(vidle_cmd->req_pos,vidle_cmd->speed);
+ set_jaws_cmd(orte_data);
break;
case DEADLINE:
break;
}
}
-void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
- case NEW_DATA:
- set_hokuyo_pitch(orte_data);
- break;
- case DEADLINE:
-// printf("ORTE deadline occurred - hokuyo pitch receive\n");
- break;
- }
+ case NEW_DATA:
+ set_lift_cmd(orte_data);
+ break;
+ case DEADLINE:
+ break;
+}
}
+struct robottype_orte_data orte;
int main(int argc, char *argv[])
{
int ret;
int size;
- struct robottype_orte_data orte;
struct can_frame frame;
if (cand_init() < 0) {
printf("cand: init OK\n");
}
- orte.strength = 1;
-
/* orte initialization */
if(robottype_roboorte_init(&orte)) {
printf("Roboorte initialization failed! Exiting...\n");
robottype_publisher_motion_irc_create(&orte, NULL, NULL);
robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
robottype_publisher_robot_switches_create(&orte, NULL, NULL);
- robottype_publisher_vidle_status_create(&orte, NULL, NULL);
+ robottype_publisher_robot_bumpers_create(&orte, NULL, NULL);
+ robottype_publisher_jaws_status_create(&orte, NULL, NULL);
+ robottype_publisher_lift_status_create(&orte, NULL, NULL);
printf("Publishers OK\n");
/* creating subscribers */
robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
- robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
- robottype_subscriber_vidle_cmd_create(&orte, rcv_vidle_cmd_cb, &orte);
+ robottype_subscriber_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
+ robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);
robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
-
+
printf("subscribers OK\n");
--- /dev/null
+~/git_ebot/build/host/_compiled/include
* @file can_ids.h
* @author Michal Sojka <sojkam1@fel.cvut.cz>
* @date Wed Feb 25 14:28:26 2009
- *
+ *
* @brief IDs of CAN bus messages
*
*/
#define CAN_PWR_ALERT to_boa(0x05) /**< alert power status */
-#define CAN_CORR_TRIG to_boa(0x008) /* ULoPoS: correlation started */
-#define CAN_CORR_DIST to_boa(0x009) /* ULoPoS: measured distances */
-
#define CAN_ROBOT_CMD to_boa(0x10) /**< robot command (start, ..) */
#define CAN_ODO_RESET to_boa(0x14) /* ODO->BOA */
#define CAN_MOTION_ODOMETRY_SIMPLE to_boa(0x22) /* MOT->BOA */
#define CAN_MOTION_STATUS to_boa(0x23) /* MOT->BOA */
-
-
#define CAN_ROBOT_SWITCHES to_boa(0x30)
+#define CAN_ROBOT_BUMPERS to_boa(0x31)
// ids of can-peripherials
-#define CAN_CHELAE to_per(0x32) /**< BOA->PER @copydetails set_chelae() front view 1st B left, 2nd B right */
-#define CAN_ADC_1 to_boa(0x33) /* PER->BOA */
-#define CAN_ADC_2 to_boa(0x34) /* PER->BOA */
-#define CAN_IR to_boa(0x35) /* PER->BOA */
-#define CAN_LED to_per(0x36) /* BOA->PER */
-#define CAN_ADC_3 to_boa(0x37) /* PER->BOA */
-
-#define CAN_BELTS to_per(0x38) /**< BOA->PER @copydetails set_belts()*/
-
-
#define CAN_PWR to_per(0x40) /* BOA->PER */
/* spodni 3 bity: zapnout. dalsi 3 b zapnout */
#define CAN_PWR_ADC2 to_boa(0x42) /* PER->BOA */
/* napeti na jednotlivych vetvich, 4B hodnoty */
-//#define CAN_BRUSHES_STATUS to_boa(0x44) // FIXME: (F.J.) rename me, ...
-
-#define CAN_PUCK to_boa(0x43)
-
-
-#define CAN_CMU to_boa(0x46) /* PER->BOA */
-#define CAN_HOKUYO_PITCH to_per(0x47) /* BOA->PER */
-
-//#define CAN_ERROR to_boa(0x48) // FIXME: (F.J.) rename me, ...
+#define CAN_JAW_RIGHT_STATUS to_boa(0x50)
+#define CAN_JAW_RIGHT_CMD to_per(0x51)
-#define CAN_VIDLE_STATUS to_boa(0x48)
-#define CAN_VIDLE_CMD to_per(0x49)
+#define CAN_JAW_LEFT_STATUS to_boa(0x52)
+#define CAN_JAW_LEFT_CMD to_per(0x53)
+#define CAN_LIFT_STATUS to_boa(0x54)
+#define CAN_LIFT_CMD to_per(0x55)
// #undef to_boa
// #undef to_mot
// #undef to_per
+#ifndef CANMSGDEFH
+#define CANMSGDEFH
-/* Flags sent in CAN_VIDLE_STATUS message */
-#define CAN_VIDLE_INITIALIZING 0x01
-#define CAN_VIDLE_TIMEOUT 0x02
-#define CAN_VIDLE_OUT_OF_BOUNDS 0x04
+//flags sent in CAN_LIFT_STATUS message
+#define CAN_LIFT_INITIALIZING 1
+#define CAN_LIFT_TIMEOUT 2
+#define CAN_LIFT_OUT_OF_BOUNDS 4
+#define CAN_LIFT_SWITCH_UP 8
+#define CAN_LIFT_SWITCH_DOWN 16
+#define CAN_LIFT_SWITCH_HOME 32
+#define CAN_LIFT_HOMED 64
+#define CAN_LIFT_START 128
-#define CAN_SWITCH_BUMPER 0x01
-#define CAN_SWITCH_COLOR 0x02
-#define CAN_SWITCH_LEFT 0x04
-#define CAN_SWITCH_RIGHT 0x08
+//flags sent in CAN_JAW_RIGHT_STATUS, CAN_JAW_LEFT_STATUS message
+#define CAN_JAW_INITIALIZING 0x01
+#define CAN_JAW_TIMEOUT 0x02
+#define CAN_JAW_OUT_OF_BOUNDS 0x04
+
+//flags sent in CAN_SWITCHES
+#define CAN_SWITCH_COLOR 2
+#define CAN_SWITCH_STRATEGY 4
+
+//flags sent in CAN_ROBOT_BUMPERS
+#define CAN_BUMPER_REAR_LEFT 1
+#define CAN_BUMPER_REAR_RIGHT 2
+#define CAN_BUMPER_LEFT 4
+#define CAN_BUMPER_RIGHT 8
+#define CAN_BUMPER_LEFT_ACROSS 16
+#define CAN_BUMPER_RIGHT_ACROSS 32
+
+/* power */
+#define CAN_PWR_ALERT_33 0x01
+#define CAN_PWR_ALERT_50 0x02
+#define CAN_PWR_ALERT_80 0x04
+#define CAN_PWR_BATT_FULL 0x08
+#define CAN_PWR_BATT_MEAN 0x10
+#define CAN_PWR_BATT_LOW 0x20
+#define CAN_PWR_BATT_CRITICAL 0x40
+
+#endif
*/
/** @ingroup pp */
/**@{*/
-#define MAP_WIDTH 30 /**< @brief Field width*/
-#define MAP_HEIGHT 21 /**< @brief Field height*/
+#include <map.h>
+// #define MAP_WIDTH 100 /**< @brief Field width*/
+// #define MAP_HEIGHT 100 /**< @brief Field height*/
-#define WALL_COST 1000 /**< @brief Cost fo jumping a wall. */
+#define WALL_COST 1000 /**< @brief Cost fo jumping a wall. */
-#define MAP_WALL 0xFF /**< @brief Wall cell. Black in robomon.*/
-#define MAP_PATH 0xFE /**< @brief Path cell. Darkred in robomon.*/
-#define MAP_START 0xFD /**< @brief Start cell. Red in robomon.*/
+#define MAP_WALL 0xFF /**< @brief Wall cell. Black in robomon.*/
+#define MAP_PATH 0xFE /**< @brief Path cell. Darkred in robomon.*/
+#define MAP_START 0xFD /**< @brief Start cell. Red in robomon.*/
#define MAP_GOAL 0xFC /**< @brief Goal cell. Green in robomon.*/
#define MAP_NEW_OBSTACLE 0xFB /**< @brief Fouded obstacle cell. Blue in robomon.*/
-#define MAP_FREE 0x00 /**< @brief Free cell. The robot can move on. White in robomon.*/
+#define MAP_FREE 0x00 /**< @brief Free cell. The robot can move on. White in robomon.*/
-#define SHM_MAP_KEY 555 /**< @brief Key use to share the memory SHM*/
+#define SHM_MAP_KEY 555 /**< @brief Key use to share the memory SHM*/
// some macros to acces to map
#define GETMAPPOS(i,j) (*(map+i+j*MAP_WIDTH))
// Some macros useful to convert from Graph coordonates to robot field
-#define GRAPHX2FIELD(x) ((x+0.5)/10.0) /**< @brief Convert from cell coordonates X to real coordonates. Use better cell2real_X(). */
-#define GRAPHY2FIELD(y) ((abs(y-MAP_HEIGHT)-0.5)/10.0) /**< @brief Convert from cell coordonates Y to real coordonates. Use better cell2real_Y(). */
+// #define GRAPHX2FIELD(x) ((x+0.5)/10.0) /**< @brief Convert from cell coordonates X to real coordonates. Use better cell2real_X(). */
+// #define GRAPHY2FIELD(y) ((abs(y-MAP_HEIGHT)-0.5)/10.0) /**< @brief Convert from cell coordonates Y to real coordonates. Use better cell2real_Y(). */
typedef unsigned char map_t;
/**@}*/
\ No newline at end of file
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
--- /dev/null
+QT_PROJECTS = display-qt.pro
+
+default_CONFIG += CONFIG_QT4_DIR=x
+QTDIR = $(CONFIG_QT4_DIR)
+
--- /dev/null
+#-------------------------------------------------
+#
+# Project created by QtCreator 2011-03-18T20:05:37
+#
+#-------------------------------------------------
+
+QT += core gui
+
+TARGET = display-qt
+TEMPLATE = app
+
+
+SOURCES += main.cpp\
+ displayqt.cpp \
+ ortesignals.cpp \
+ display_orte.cpp
+
+HEADERS += displayqt.h \
+ ortesignals.h \
+ promene.h \
+ display_orte.h
+
+FORMS += displayqt.ui \
+
+LIBS += -lm -lpthread -lrobodim -lroboorte -lrobottype -lorte -lrt
--- /dev/null
+#include "display_orte.h"
+#include "ortesignals.h"
+//#include <can_msg_def.h>
+
+#include <time.h>
+
+
+/**
+ * Subtract the `struct timespec' values X and Y,
+ * storing the result in RESULT (result = x - y).
+ * Return 1 if the difference is negative, otherwise 0.
+ */
+int timespec_subtract (struct timespec *result,
+ struct timespec *x,
+ struct timespec *y)
+{
+ /* Perform the carry for the later subtraction by updating Y. */
+ if (x->tv_nsec < y->tv_nsec) {
+ int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+ y->tv_nsec -= 1000000000 * num_sec;
+ y->tv_sec += num_sec;
+ }
+ if (x->tv_nsec - y->tv_nsec > 1000000000) {
+ int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
+ y->tv_nsec += 1000000000 * num_sec;
+ y->tv_sec -= num_sec;
+ }
+
+ /* Compute the time remaining to wait.
+ `tv_nsec' is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_nsec = x->tv_nsec - y->tv_nsec;
+
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
+{
+ struct timespec now, diff;
+ unsigned long long elapsed;
+
+ if (t->tv_sec == 0 && t->tv_nsec == 0)
+ return true; /* Always elapsed after program start */
+
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ timespec_subtract(&diff, &now, t);
+ elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
+ return elapsed > miliseconds;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+void rcv_match_time_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ inst->time_con();
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(TIM, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ inst->pwr_con();
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(PWR, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(ODO, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->start_condition)
+ status = STATUS_WARNING; /* Start plug must be plugged before competition start */
+ else
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 500)) {
+ //uoled_display_status(STA, status);
+ inst->status_con(STA, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static char last_color;
+ static struct timespec last_sent;
+ struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->team_color != last_color ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->color_con(instance->team_color);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_color = instance->team_color;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+}
+
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ struct motion_status_type *m = (struct motion_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->err_left == 0 && m->err_right == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(MOT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ struct lift_status_type *m = (struct lift_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->flags)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(LFT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ struct jaws_status_type *m = (struct jaws_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->flags.left == 0 && m->flags.right == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(JAW, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+
+}
+
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->status_con(HOK, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 100)) {
+ inst->status_con(APP, status);
+ if (status == STATUS_OK)
+ inst->position_con();
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ QString status;
+ static QString last_status;
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ status=fsm_state->state_name;
+ break;
+ case DEADLINE:
+ status="?";
+ break;
+ }
+ if (status!=last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->fsm_con(FSM_MAIN, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status=status;
+}
+
+void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ QString status;
+ static QString last_status;
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ status=fsm_state->state_name;
+ break;
+ case DEADLINE:
+ status="?";
+ break;
+ }
+ if (status!=last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->fsm_con(FSM_ACT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status=status;
+}
+
+void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
+{
+ OrteSignals *inst=(OrteSignals *)recvCallBackParam;
+ QString status;
+ static QString last_status;
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ status=fsm_state->state_name;
+ break;
+ case DEADLINE:
+ status="?";
+ break;
+ }
+ if (status!=last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ inst->fsm_con(FSM_MOVE, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status=status;
+}
--- /dev/null
+#ifndef DISPLAY_ORTE_H
+#define DISPLAY_ORTE_H
+
+#include <orte.h>
+#include <roboorte_robottype.h>
+#include <robottype.h>
+
+
+bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds);
+int timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y);
+
+
+void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+void rcv_match_time_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+
+#endif
--- /dev/null
+#include "displayqt.h"
+#include "ui_displayqt.h"
+#include <QtGui>
+#include <QObject>
+#include <QTimer>
+#include <QString>
+#include <robomath.h>
+
+//backround colors for labels
+#define GREEN "background-color:rgb(27, 255, 11)"
+#define RED "background-color:red"
+#define YELLOW "background-color:yellow"
+
+DisplayQT::DisplayQT(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::DisplayQT)
+{
+ ui->setupUi(this);
+
+ /* create the window without the title bar */
+#ifndef __i386__
+ Qt::WindowFlags flags = this->windowFlags();
+ flags |= Qt::FramelessWindowHint;
+ this->setWindowFlags(flags);
+
+ //cursor will be hidden
+ setCursor(QCursor(Qt::BlankCursor));
+#endif
+
+ //na zacatku nazname polohu
+ this->pos.positionIsActual=false;
+
+ /*ASCII tocitko
+ pomoci timeru je kazdych 500 ms
+ vyvolan signal pro pohnuti tocitka
+ */
+
+ QTimer *timer = new QTimer(this);
+ timer->start(500);
+ connect(timer, SIGNAL(timeout()), this, SLOT(alive()));
+ ///***///
+
+ //premalovavani kompasu
+ connect(this, SIGNAL(repaintCompass()), this, SLOT(update()));
+}
+
+//sipka od uhlu natoceni
+void DisplayQT::paintEvent(QPaintEvent *)
+{
+ //umisteni stredu kompasu
+ int const x=410;
+ int const y=95;
+ int const dimension=80;
+
+ static const QPoint arrow[3] = {
+ QPoint(4, 4),
+ QPoint(-4, 4),
+ QPoint(0, -((dimension/2)-15))
+ };
+
+
+ //barvy pro malovani
+ QColor prvniColor(127, 0, 127);
+ QColor druhaColor(0, 127, 127, 191);
+ QColor pozadi(240, 240, 240);
+
+ QPainter painter(this);
+
+ //background
+ painter.fillRect(x-dimension/2, y-dimension/2, dimension, dimension, pozadi);
+
+ painter.setRenderHint(QPainter::Antialiasing);
+ painter.translate(x,y);
+
+ painter.setPen(Qt::NoPen);
+ painter.setBrush(prvniColor);
+ painter.save();
+
+ //namalovani sipky p.t.k. je aktualni pozice
+ if(pos.positionIsActual){
+ painter.rotate(90-pos.phi);
+ painter.drawConvexPolygon(arrow, 3);
+ }
+ painter.restore();
+
+ //namaluju 4 cary po 90 stupnich
+ painter.setPen(prvniColor);
+ for(int i=0; i<4; i++){
+ painter.drawLine((dimension/2)-10, 0, (dimension/2)-5, 0);
+ painter.rotate(90.0);
+ }
+
+ //namaluju mensi cary mezi ty predchozi
+ painter.setPen(druhaColor);
+
+ for(int j=0; j<12; j++){
+ if(j%3)
+ painter.drawLine((dimension/2)-12, 0, (dimension/2)-7, 0);
+ painter.rotate(30.0);
+ }
+}
+
+DisplayQT::~DisplayQT()
+{
+ delete ui;
+}
+
+void DisplayQT::alive(void)
+{
+ static char aliveState=0;
+ if(++aliveState==4)
+ aliveState=0;
+
+ switch(aliveState){
+ case 0: ui->ziju->setText("|");break;
+ case 1: ui->ziju->setText("/");break;
+ case 2: ui->ziju->setText("-");break;
+ case 3: ui->ziju->setText("\\");break;
+ }
+}
+
+void DisplayQT::display_time(double time)
+{
+ ui->matchTime->display(time);
+}
+
+// 0-fialova
+// 1-cervena
+void DisplayQT::setTeamColor(char color)
+{
+ if(color==0){
+ color=0;
+ ui->our_color->setStyleSheet("background-color: violet");
+ }else if(color==1){
+ color=1;
+ ui->our_color->setStyleSheet("background-color: red");
+ }
+}
+
+void DisplayQT::setPosition(double x, double y, double phi)
+{
+ this->pos.x=x;
+ this->pos.y=y;
+ this->pos.phi=phi;
+
+ //prevod z radianu na stupne a uprava
+ pos.phi = RAD2DEG(pos.phi);
+ pos.phi = fmod(pos.phi, 360);
+ if ( pos.phi < 0 )
+ pos.phi += 360;
+
+ ui->position_x->setText("x: "+QString::number(pos.x, 10, 3)+" m");
+ ui->position_y->setText("y: "+QString::number(pos.y, 10, 3)+" m");
+ ui->position_phi->setText("phi: "+QString::number(pos.phi, 10, 1)+" deg");
+
+ pos.positionIsActual=true;
+ emit repaintCompass();
+}
+
+void DisplayQT::display_status(UDE_component_t c, UDE_hw_status_t s)
+{
+ switch(c){
+ case 0:break;
+ case MOT:
+ if(s==STATUS_OK)
+ ui->comp_MOT->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_MOT->setStyleSheet(RED);
+ else
+ ui->comp_MOT->setStyleSheet(YELLOW);
+ break;
+ case ODO:
+ if(s==STATUS_OK)
+ ui->comp_ODO->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_ODO->setStyleSheet(RED);
+ else
+ ui->comp_ODO->setStyleSheet(YELLOW);
+ break;
+ case JAW:
+ if(s==STATUS_OK)
+ ui->comp_JAW->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_JAW->setStyleSheet(RED);
+ else
+ ui->comp_JAW->setStyleSheet(YELLOW);
+ break;
+ case PWR:
+ if(s==STATUS_OK)
+ ui->comp_PWR->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED){
+ ui->comp_PWR->setStyleSheet(RED);
+
+ ui->voltage_33->setText("v.33 = ?");
+ ui->voltage_50->setText("v.50 = ?");
+ ui->voltage_80->setText("v.80 = ?");
+ ui->voltage_BAT->setText("v.BAT = ?");
+
+ ui->voltage_33->setStyleSheet(YELLOW);
+ ui->voltage_50->setStyleSheet(YELLOW);
+ ui->voltage_80->setStyleSheet(YELLOW);
+ ui->voltage_BAT->setStyleSheet(YELLOW);
+ }
+ else
+ ui->comp_PWR->setStyleSheet(YELLOW);
+ break;
+ case HOK:
+ if(s==STATUS_OK)
+ ui->comp_HOK->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_HOK->setStyleSheet(RED);
+ else
+ ui->comp_HOK->setStyleSheet(YELLOW);
+ break;
+ case APP:
+ if(s==STATUS_OK)
+ ui->comp_APP->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED){
+ ui->comp_APP->setStyleSheet(RED);
+
+ pos.positionIsActual=false;
+ ui->position_x->setText("x: ?");
+ ui->position_y->setText("y: ?");
+ ui->position_phi->setText("phi: ?");
+ emit repaintCompass();
+ }
+ else
+ ui->comp_APP->setStyleSheet(YELLOW);
+ break;
+ case LFT:
+ if(s==STATUS_OK)
+ ui->comp_LFT->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_LFT->setStyleSheet(RED);
+ else
+ ui->comp_LFT->setStyleSheet(YELLOW);
+ break;
+ case STA:
+ if(s==STATUS_OK)
+ ui->comp_STA->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->comp_STA->setStyleSheet(RED);
+ else
+ ui->comp_STA->setStyleSheet(YELLOW);
+ break;
+ case TIM:
+ if(s==STATUS_OK)
+ ui->matchTime->setStyleSheet(GREEN);
+ else if(s==STATUS_FAILED)
+ ui->matchTime->setStyleSheet(RED);
+ else
+ ui->matchTime->setStyleSheet(YELLOW);
+ break;
+ }
+}
+
+void DisplayQT::display_fsm(UDE_fsm_t fsm, QString state){
+
+ switch(fsm){
+ case FSM_MAIN:
+ ui->fsm_main->setText(state);
+ break;
+ case FSM_MOVE:
+ ui->fsm_move->setText(state);
+ break;
+ case FSM_ACT:
+ ui->fsm_act->setText(state);
+ break;
+ }
+}
+
+void DisplayQT::display_voltage(double voltage33, double voltage50, double voltage80, double voltageBAT){
+ ui->voltage_33->setText("v.33 = "+QString::number(voltage33, 10, 2)+" V");
+ ui->voltage_50->setText("v.50 = "+QString::number(voltage50, 10, 2)+" V");
+ ui->voltage_80->setText("v.80 = "+QString::number(voltage80, 10, 2)+" V");
+ ui->voltage_BAT->setText("v.BAT = "+QString::number(voltageBAT, 10, 2)+" V");
+
+
+ if( voltageBAT < WARNING_VOLTAGEBAT && voltageBAT > TRESHOLDS_VOLTAGEBAT )
+ ui->voltage_BAT->setStyleSheet(YELLOW);
+ else if( voltageBAT < TRESHOLDS_VOLTAGEBAT )
+ ui->voltage_BAT->setStyleSheet(RED);
+ else
+ ui->voltage_BAT->setStyleSheet(GREEN);
+
+ if( voltage33 < TRESHOLDS_VOLTAGE33 )
+ ui->voltage_33->setStyleSheet(YELLOW);
+ else
+ ui->voltage_33->setStyleSheet(GREEN);
+
+ if( voltage50 < TRESHOLDS_VOLTAGE50 )
+ ui->voltage_50->setStyleSheet(YELLOW);
+ else
+ ui->voltage_50->setStyleSheet(GREEN);
+
+ if( voltage80 < TRESHOLDS_VOLTAGE80 )
+ ui->voltage_80->setStyleSheet(YELLOW);
+ else
+ ui->voltage_80->setStyleSheet(GREEN);
+}
--- /dev/null
+#ifndef DISPLAYQT_H
+#define DISPLAYQT_H
+
+#include <QWidget>
+#include "promene.h"
+
+namespace Ui {
+ class DisplayQT;
+}
+
+class DisplayQT : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit DisplayQT(QWidget *parent = 0);
+ ~DisplayQT();
+
+private:
+ Ui::DisplayQT *ui;
+
+
+protected:
+ void paintEvent(QPaintEvent *event);
+
+
+private:
+ char teamColor;
+
+ struct position
+ {
+ double x;
+ double y;
+ double phi;
+ bool positionIsActual;
+ } pos;
+
+ double match_time;
+
+signals:
+ void repaintCompass(void);
+
+public slots:
+ void alive(void);
+ void setTeamColor(char color);
+ void setPosition(double x, double y, double phi);
+ ///
+ void display_time(double time);
+ void display_status(UDE_component_t c, UDE_hw_status_t s);
+ void display_fsm(UDE_fsm_t fsm, QString state);
+ void display_voltage(double voltage33, double voltage50, double voltage80, double voltageBAT);
+};
+
+#endif
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DisplayQT</class>
+ <widget class="QWidget" name="DisplayQT">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>480</width>
+ <height>272</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>ROBO display</string>
+ </property>
+ <property name="autoFillBackground">
+ <bool>false</bool>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(0, 0, 0);</string>
+ </property>
+ <widget class="QWidget" name="horizontalLayoutWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>-1</y>
+ <width>484</width>
+ <height>51</height>
+ </rect>
+ </property>
+ <layout class="QHBoxLayout" name="STATUS_layout">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="comp_MOT">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>MOT</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_ODO">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>ODO</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_JAW">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>JAW</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_PWR">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>PWR</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_HOK">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>HOK</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_APP">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>APP</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_LFT">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>LFT</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="comp_STA">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>STA</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayoutWidget">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>70</y>
+ <width>41</width>
+ <height>61</height>
+ </rect>
+ </property>
+ <layout class="QVBoxLayout" name="TEAM_ALIVE_layout">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="our_color">
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="ziju">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(27, 255, 11);</string>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QLabel" name="team_color_label">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>50</y>
+ <width>81</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">color: rgb(255, 255, 255);</string>
+ </property>
+ <property name="text">
+ <string>Team color</string>
+ </property>
+ </widget>
+ <widget class="QWidget" name="horizontalLayoutWidget_2">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>140</y>
+ <width>321</width>
+ <height>67</height>
+ </rect>
+ </property>
+ <layout class="QHBoxLayout" name="FSM_layout">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <layout class="QVBoxLayout" name="FSM_I">
+ <property name="spacing">
+ <number>3</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="main_label">
+ <property name="maximumSize">
+ <size>
+ <width>100</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>FSM MAIN</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="move_label">
+ <property name="maximumSize">
+ <size>
+ <width>100</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>FSM MOVE</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="act_label">
+ <property name="maximumSize">
+ <size>
+ <width>100</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>FSM ACT</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QVBoxLayout" name="FSM_II">
+ <property name="spacing">
+ <number>3</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="fsm_main">
+ <property name="minimumSize">
+ <size>
+ <width>215</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>---</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="fsm_move">
+ <property name="minimumSize">
+ <size>
+ <width>215</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>---</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="fsm_act">
+ <property name="minimumSize">
+ <size>
+ <width>215</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>---</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="horizontalLayoutWidget_3">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>250</y>
+ <width>481</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <layout class="QHBoxLayout" name="VOLTAGE_layout">
+ <property name="spacing">
+ <number>10</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="voltage_33">
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>volt. 33</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="voltage_50">
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>volt. 50</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="voltage_80">
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>volt. 80</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="voltage_BAT">
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 0, 0);</string>
+ </property>
+ <property name="text">
+ <string>volt. BAT</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayoutWidget_2">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>140</y>
+ <width>121</width>
+ <height>94</height>
+ </rect>
+ </property>
+ <layout class="QVBoxLayout" name="POSITION_layout">
+ <property name="spacing">
+ <number>3</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="position_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>POSITION</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="position_x">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">color: rgb(23, 88, 208);
+background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>?</string>
+ </property>
+ <property name="margin">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="position_y">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">color: rgb(173, 87, 29);
+background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>?</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="position_phi">
+ <property name="font">
+ <font>
+ <pointsize>12</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">color: rgb(80, 202, 15);
+background-color: rgb(255, 255, 255);
+padding-left:5px;</string>
+ </property>
+ <property name="text">
+ <string>?</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QLCDNumber" name="matchTime">
+ <property name="geometry">
+ <rect>
+ <x>110</x>
+ <y>60</y>
+ <width>221</width>
+ <height>71</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <family>Gentium</family>
+ <pointsize>9</pointsize>
+ <weight>50</weight>
+ <italic>false</italic>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">font: 9pt "Gentium";
+background-color: rgb(255, 255, 0)</string>
+ </property>
+ <property name="smallDecimalPoint">
+ <bool>true</bool>
+ </property>
+ <property name="numDigits">
+ <number>5</number>
+ </property>
+ <property name="digitCount">
+ <number>5</number>
+ </property>
+ <property name="mode">
+ <enum>QLCDNumber::Dec</enum>
+ </property>
+ <property name="segmentStyle">
+ <enum>QLCDNumber::Filled</enum>
+ </property>
+ <property name="value" stdset="0">
+ <double>90.000000000000000</double>
+ </property>
+ </widget>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>
--- /dev/null
+#include <QtGui>
+#include <QObject>
+#include "displayqt.h"
+#include "ortesignals.h"
+
+
+int main(int argc, char *argv[])
+{
+ QApplication a(argc, argv);
+
+ DisplayQT disp;
+ disp.show();
+
+ OrteSignals ortesig;
+
+
+ qRegisterMetaType<UDE_fsm_t>("UDE_fsm_t");
+ qRegisterMetaType<UDE_fsm_t>("UDE_component_t");
+ qRegisterMetaType<UDE_fsm_t>("UDE_hw_status_t");
+
+
+ QObject::connect(&ortesig, SIGNAL(fsm_sig(UDE_fsm_t, QString)),
+ &disp, SLOT(display_fsm(UDE_fsm_t, QString)));
+
+ QObject::connect(&ortesig, SIGNAL(status_sig(UDE_component_t, UDE_hw_status_t)),
+ &disp, SLOT(display_status(UDE_component_t, UDE_hw_status_t)));
+
+ QObject::connect(&ortesig, SIGNAL(position_sig(double, double, double)),
+ &disp, SLOT(setPosition(double, double, double)));
+
+ QObject::connect(&ortesig, SIGNAL(pwr_sig(double, double, double, double)),
+ &disp, SLOT(display_voltage(double, double, double, double)));
+
+ QObject::connect(&ortesig, SIGNAL(color_sig(char)),
+ &disp, SLOT(setTeamColor(char)));
+
+ QObject::connect(&ortesig, SIGNAL(time_sig(double)),
+ &disp, SLOT(display_time(double)));
+
+
+ return a.exec();
+}
--- /dev/null
+#include "ortesignals.h"
+
+
+OrteSignals::OrteSignals()
+{
+ createOrte();
+}
+
+
+void OrteSignals::createOrte()
+{
+ memset(&orte, 0, sizeof(orte));
+ robottype_roboorte_init(&orte); //kontrola uspechu ?
+
+ //subscribers
+ robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, this);
+ robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, this);
+ robottype_subscriber_lift_status_create(&orte, rcv_lift_status_cb, this);
+ robottype_subscriber_jaws_status_create(&orte, rcv_jaws_status_cb, this);
+ robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, this);
+ robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, this);
+ robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, this);
+ robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte); //nebude uzito
+ robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, this);
+ robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, this);
+ robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, this);
+ robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, this);
+ robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, this);
+ robottype_subscriber_match_time_create(&orte, rcv_match_time_cb, this);
+}
+
+void OrteSignals::fsm_con(UDE_fsm_t fsm, QString state)
+{
+ emit fsm_sig(fsm, state);
+}
+
+void OrteSignals::status_con(UDE_component_t c, UDE_hw_status_t s)
+{
+ emit status_sig(c, s);
+}
+
+void OrteSignals::position_con(void)
+{
+ emit position_sig(orte.est_pos_best.x, orte.est_pos_best.y, orte.est_pos_best.phi);
+}
+
+void OrteSignals::pwr_con(void)
+{
+ emit pwr_sig(orte.pwr_voltage.voltage33, orte.pwr_voltage.voltage50,
+ orte.pwr_voltage.voltage80, orte.pwr_voltage.voltageBAT);
+}
+
+void OrteSignals::color_con(char color)
+{
+ emit color_sig(color);
+}
+
+void OrteSignals::time_con(void)
+{
+ emit time_sig(orte.match_time.time);
+}
--- /dev/null
+#ifndef ORTESIGNALS_H
+#define ORTESIGNALS_H
+
+
+#include <QObject>
+#include <orte.h>
+#include <robottype.h>
+#include <roboorte_robottype.h>
+#include "display_orte.h"
+#include "promene.h"
+
+
+class OrteSignals : public QObject
+{
+
+Q_OBJECT
+
+public:
+ OrteSignals();
+
+ struct robottype_orte_data orte;
+
+ void createOrte();
+
+ //metody pro emitovani signalu
+ void fsm_con(UDE_fsm_t fsm, QString state);
+ void status_con(UDE_component_t c, UDE_hw_status_t s);
+ void position_con(void);
+ void pwr_con(void);
+ void color_con(char color);
+ void time_con(void);
+
+signals:
+ void fsm_sig(UDE_fsm_t fsm, QString state);
+ void status_sig(UDE_component_t c, UDE_hw_status_t s);
+ void position_sig(double x, double y, double phi);
+ void pwr_sig(double voltage33, double voltage50, double voltage80, double voltageBAT);
+ void color_sig(char color);
+ void time_sig(double time);
+};
+
+#endif
--- /dev/null
+#ifndef PROMENE_H
+#define PROMENE_H
+
+/**
+ * 0. (neni pouzito)
+ * 1. MOT - ridici jednotka motoru
+ * 2. ODO - odometrie
+ * 3. JAW - jaws
+ * 4. PWR - napajeci zdroje
+ * 5. HOK - Hokuyo
+ * 6. APP - ridici aplikace
+ * 7. LFT - lift
+ * 8. STA - startovaci tlacitko
+ */
+typedef enum {
+ UKN = 0,
+ MOT = 1,
+ ODO = 2,
+ JAW = 3,
+ PWR = 4,
+ HOK = 5,
+ APP = 6,
+ LFT = 7,
+ STA = 8,
+ TIM = 9
+} UDE_component_t;
+
+/**
+ * 00 = HW_STATUS_FAILED
+ * 01 = HW_STATUS_OK
+ * 10 = HW_STATUS_WARNING
+ */
+typedef enum {
+ STATUS_FAILED = 0,
+ STATUS_OK = 1,
+ STATUS_WARNING = 2
+} UDE_hw_status_t;
+
+/** ID_DISPLAY_FSM + ...
+ * 0 = MAIN FSM STATE
+ * 1 = MOVE FSM STATE
+ * 2 = ACT FSM STATE
+ */
+typedef enum {
+ FSM_MAIN = 0,
+ FSM_MOVE = 1,
+ FSM_ACT = 2
+} UDE_fsm_t;
+
+#define TRESHOLDS_VOLTAGE33 3.2
+#define TRESHOLDS_VOLTAGE50 4.9
+#define TRESHOLDS_VOLTAGE80 7.9
+#define TRESHOLDS_VOLTAGEBAT 12.5
+#define WARNING_VOLTAGEBAT 13.0
+
+#endif
# -*- makefile -*-
-bin_PROGRAMS += displayd
+#bin_PROGRAMS += displayd
displayd_SOURCES = displayd.c uoled.c
lib_LOADLIBES = roboorte robottype orte pthread sercom rt m
}
-
+struct robottype_orte_data orte;
int main(int argc, char *argv[])
{
int ret;
//int size;
- struct robottype_orte_data orte;
char * tty;
signal(SIGINT, sig_handler);
//#endif
}
- orte.strength = 1;
-
/* orte initialization */
if(robottype_roboorte_init(&orte)) {
fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
- \ref leds
- \ref canmsg
- \ref carousel
+- \ref shapedet
To regenerate this documentation, run @c doxygen in @c src
directory. The result can be found in @c src/doc/html/index.html.
\page compilation How to compile it
+\section Prerequisites
+
+To compile the software, you need the following programs and libraries.
+- python
+- pkg-config
+- libidl
+- libpopt
+- OpenCV
+- libfft
+- Qt version 4.x
+- libusb
+
+On a Debian-based system, it is sufficient to run
+\verbatim
+apt-get install build-essential python pkg-config libidl-dev libpopt-dev libcv-dev libhighgui-dev libfftw3-dev qt4-dev-tools libusb-dev
+\endverbatim
+
+\section Preparation
+
Before first compilation, you should execute
<tt>build/prepare_infrastructure script</tt>.
\verbatim
--- /dev/null
+//-------------------------------------------------\r
+Created by: Jiri Kubias - CVUT Prague\r
+ \r
+\r
+\r
+ servo.c use ISR vector 1 for Timer 1
\ No newline at end of file
--- /dev/null
+# Doxyfile 1.5.4
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING = UTF-8
+PROJECT_NAME = eb_ebb
+PROJECT_NUMBER = 1
+OUTPUT_DIRECTORY = doc
+CREATE_SUBDIRS = NO
+OUTPUT_LANGUAGE = English
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = YES
+ABBREVIATE_BRIEF = "The $name class " \
+ "The $name widget " \
+ "The $name file " \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+ALWAYS_DETAILED_SEC = NO
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = NO
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = NO
+QT_AUTOBRIEF = NO
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP = NO
+INHERIT_DOCS = YES
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 8
+ALIASES =
+OPTIMIZE_OUTPUT_FOR_C = YES
+OPTIMIZE_OUTPUT_JAVA = NO
+BUILTIN_STL_SUPPORT = NO
+CPP_CLI_SUPPORT = NO
+SIP_SUPPORT = NO
+DISTRIBUTE_GROUP_DOC = NO
+SUBGROUPING = YES
+TYPEDEF_HIDES_STRUCT = NO
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = YES
+EXTRACT_PRIVATE = YES
+EXTRACT_STATIC = YES
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+EXTRACT_ANON_NSPACES = NO
+HIDE_UNDOC_MEMBERS = NO
+HIDE_UNDOC_CLASSES = NO
+HIDE_FRIEND_COMPOUNDS = NO
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+CASE_SENSE_NAMES = YES
+HIDE_SCOPE_NAMES = NO
+SHOW_INCLUDE_FILES = YES
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = YES
+SORT_BRIEF_DOCS = NO
+SORT_BY_SCOPE_NAME = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = YES
+GENERATE_BUGLIST = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+MAX_INITIALIZER_LINES = 30
+SHOW_USED_FILES = YES
+SHOW_DIRECTORIES = NO
+FILE_VERSION_FILTER =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = NO
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_FORMAT = "$file:$line: $text "
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT = .
+INPUT_ENCODING = UTF-8
+FILE_PATTERNS = *.c \
+ *.cc \
+ *.cxx \
+ *.cpp \
+ *.c++ \
+ *.d \
+ *.java \
+ *.ii \
+ *.ixx \
+ *.ipp \
+ *.i++ \
+ *.inl \
+ *.h \
+ *.hh \
+ *.hxx \
+ *.hpp \
+ *.h++ \
+ *.idl \
+ *.odl \
+ *.cs \
+ *.php \
+ *.php3 \
+ *.inc \
+ *.m \
+ *.mm \
+ *.dox \
+ *.py \
+ *.C \
+ *.CC \
+ *.C++ \
+ *.II \
+ *.I++ \
+ *.H \
+ *.HH \
+ *.H++ \
+ *.CS \
+ *.PHP \
+ *.PHP3 \
+ *.M \
+ *.MM \
+ *.PY \
+ *.C \
+ *.H \
+ *.tlh \
+ *.diff \
+ *.patch \
+ *.moc \
+ *.xpm \
+ *.dox
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXCLUDE_PATTERNS =
+EXCLUDE_SYMBOLS =
+EXAMPLE_PATH =
+EXAMPLE_PATTERNS = *
+EXAMPLE_RECURSIVE = NO
+IMAGE_PATH =
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = NO
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = NO
+REFERENCES_RELATION = NO
+REFERENCES_LINK_SOURCE = YES
+USE_HTAGS = NO
+VERBATIM_HEADERS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = NO
+COLS_IN_ALPHA_INDEX = 5
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = YES
+HTML_OUTPUT = html
+HTML_FILE_EXTENSION = .html
+HTML_HEADER =
+HTML_FOOTER =
+HTML_STYLESHEET =
+HTML_ALIGN_MEMBERS = YES
+GENERATE_HTMLHELP = NO
+HTML_DYNAMIC_SECTIONS = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+BINARY_TOC = NO
+TOC_EXPAND = NO
+DISABLE_INDEX = NO
+ENUM_VALUES_PER_LINE = 4
+GENERATE_TREEVIEW = NO
+TREEVIEW_WIDTH = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME = latex
+MAKEINDEX_CMD_NAME = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4wide
+EXTRA_PACKAGES =
+LATEX_HEADER =
+PDF_HYPERLINKS = NO
+USE_PDFLATEX = NO
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = NO
+XML_OUTPUT = xml
+XML_SCHEMA =
+XML_DTD =
+XML_PROGRAMLISTING = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = NO
+EXPAND_ONLY_PREDEF = NO
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+PREDEFINED =
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES =
+GENERATE_TAGFILE = eb_ebb.tag
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+PERL_PATH = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+MSCGEN_PATH =
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = NO
+CLASS_GRAPH = YES
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = YES
+INCLUDED_BY_GRAPH = YES
+CALL_GRAPH = NO
+CALLER_GRAPH = NO
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = YES
+DOT_IMAGE_FORMAT = png
+DOT_PATH =
+DOTFILE_DIRS =
+DOT_GRAPH_MAX_NODES = 50
+MAX_DOT_GRAPH_DEPTH = 1000
+DOT_TRANSPARENT = NO
+DOT_MULTI_TARGETS = NO
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine
+#---------------------------------------------------------------------------
+SEARCHENGINE = NO
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or parent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+bin_PROGRAMS = eb_ebb
+
+eb_ebb_SOURCES = main.c
+eb_ebb_LIBS = can ebb
+
+
+lib_LIBRARIES = ebb
+ebb_SOURCES = powswitch.c uart.c servo.c engine.c adc.c adc_filtr.c
+
+include_HEADERS = engine.h powswitch.h servo.h uart.h adc.h adc_filtr.h expansion.h
+
+link_VARIANTS = flash
--- /dev/null
+#include <lpc21xx.h> // LPC21XX Peripheral Registers
+#include <types.h>
+#include "adc.h"
+#include <stdlib.h>
+
+
+#define ADCCH0 22 ///< ADC0 value for PINSEL
+#define ADCCH1 24 ///< ADC1 value for PINSEL
+#define ADCCH2 26 ///< ADC2 value for PINSEL
+#define ADCCH3 28 ///< ADC3 value for PINSEL
+
+/**
+ * ADC ISR routine. This routine reads selected ADC value and
+ * multiplies it by #ADC_MUL and adds #ADC_OFFSET to calculate the
+ * volage (3.25mV/div). After this reading the next ADC channel is
+ * set up for measuring.
+ */
+void adc_isr(void) __attribute__ ((interrupt));
+void adc_isr(void)
+{
+ unsigned char chan =0;
+ unsigned int val =0;
+
+
+ chan = (char) ((ADDR>>24) & 0x07);
+ val = (((((ADDR >> 6) & 0x3FF) * ADC_MUL + ADC_OFFSET) + adc_val[chan]) >> 1) ;
+
+
+
+ adc_val[chan] = val;
+
+ ADCR &= ~(ADC_CR_START_OFF_m);
+
+
+ switch(chan)
+ {
+ case 0:
+ ADCR = ((ADC_CR_ADC1_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (20*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 1:
+ ADCR = ((ADC_CR_ADC2_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (20*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 2:
+ ADCR = ((ADC_CR_ADC3_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (20*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 3:
+ ADCR = ((ADC_CR_ADC0_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (20*ADC_CR_CLK_DIV_1_m));
+ break;
+ }
+
+ VICVectAddr = 0;
+
+}
+
+/**
+ * Inicializes ADC service for all ADC (4) channels and installs ISR routine to VIC.
+ * Automaticly starts the conversion of first channel on given conversion frequency.
+ */
+void init_adc(unsigned rx_isr_vect)
+{
+
+ PINSEL1 |= ((PINSEL_1 << ADCCH0) | (PINSEL_1 << ADCCH1) | (PINSEL_1 << ADCCH2) | (PINSEL_1 << ADCCH3));
+
+ ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x32;
+ ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned) adc_isr;
+ VICIntEnable = 0x40000;
+
+ ADCR = ((ADC_CR_ADC0_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+}
--- /dev/null
+#ifndef ADC_H
+#define ADC_H
+
+
+#define ADC_MUL 1 ///< This value multiplies mesured value from ADC
+#define ADC_OFFSET 0 ///< This valueis added to mesured value from ADC
+
+#define ADC_VPB_DIV 255 ///< VPB divisor for ADC FIXME
+
+
+
+
+volatile unsigned int adc_val[4]; ///< measured ADC values
+
+
+
+
+
+/** inicializes ADC lines and starts converion (use ISR)
+ * @param rx_isr_vect ISR vector
+ */
+void init_adc(unsigned rx_isr_vect);
+
+#endif
--- /dev/null
+#include <lpc21xx.h> // LPC21XX Peripheral Registers
+#include <types.h>
+#include "adc_filtr.h"
+#include <stdlib.h>
+#include <string.h>
+#include <deb_led.h>
+
+
+
+#define ADCCH0 22 ///< ADC0 value for PINSEL
+#define ADCCH1 24 ///< ADC1 value for PINSEL
+#define ADCCH2 26 ///< ADC2 value for PINSEL
+#define ADCCH3 28 ///< ADC3 value for PINSEL
+
+
+/**
+ * Comparsion function for quicksort
+ *
+ * @param *a first number for comparion
+ * @param *b second number for comparion
+ *
+ * @return 1 if b is higher than a
+ */
+int compare( const void *a, const void *b)
+{
+ return (*((unsigned int*)a) < *(((unsigned int*)b)));
+}
+
+
+/**
+ * Median filter for ADC. If new data is available the median is recalculated.
+ * This function may be called from main function, it should take long time
+ * to calculate the median for all ADC value.
+ *
+ * @param chan bit oriented (0~3) value, defines which ADC channels have new data
+ */
+void adc_filter(unsigned char chan)
+{
+ if(adc_update_adc & 1)
+ {
+ memcpy(&adc_filtr_0_m, adc_filtr_0, sizeof(uint16_t) * ADC_FILTR_SIZE );
+ qsort(adc_filtr_0_m , ADC_FILTR_SIZE , sizeof(uint16_t), compare);
+ adc_val[0] = adc_filtr_0_m[(ADC_FILTR_SIZE/2 + 1)];
+ adc_update_adc &= ~ 1;
+
+ }
+ if(adc_update_adc & 2)
+ {
+ memcpy(&adc_filtr_1_m, adc_filtr_1, sizeof(uint16_t) * ADC_FILTR_SIZE );
+ qsort(adc_filtr_1_m , ADC_FILTR_SIZE , sizeof(uint16_t), compare);
+ adc_val[1] = adc_filtr_1_m[(ADC_FILTR_SIZE/2 + 1)];
+ adc_update_adc &= ~ 2;
+ }
+ if(adc_update_adc & 4)
+ {
+ memcpy(&adc_filtr_2_m, adc_filtr_2, sizeof(uint16_t) * ADC_FILTR_SIZE );
+ qsort(adc_filtr_2_m , ADC_FILTR_SIZE , sizeof(uint16_t), compare);
+ adc_val[2] = adc_filtr_2_m[(ADC_FILTR_SIZE/2 + 1)];
+ adc_update_adc &= ~ 4;
+ }
+ if(adc_update_adc & 8)
+ {
+ memcpy(&adc_filtr_3_m, adc_filtr_3, sizeof(uint16_t) * ADC_FILTR_SIZE );
+ qsort(adc_filtr_3_m , ADC_FILTR_SIZE , sizeof(uint16_t), compare);
+ adc_val[3] = adc_filtr_3_m[(ADC_FILTR_SIZE/2 + 1)];
+ adc_update_adc &= ~ 8;
+ }
+}
+
+
+
+
+/**
+ * ADC ISR routine. This routine reads selected ADC value and
+ * multiplies it by #ADC_MUL and adds #ADC_OFFSET to calculate the
+ * volage (3.25mV/div). After this reading the next ADC channel is
+ * set up for measuring.
+ */
+void adc_isr_filtr(void) __attribute__ ((interrupt));
+void adc_isr_filtr(void)
+{
+ unsigned char chan =0;
+ unsigned int val =0;
+
+
+ chan = (char) ((ADDR>>24) & 0x07);
+ val = (((((ADDR >> 6) & 0x3FF) * ADC_MUL + ADC_OFFSET) + adc_val[chan]) >> 1) ;
+
+
+ if(chan == 0)
+ {
+ adc_filtr_0[adc_filtr_p] = val;
+ adc_update_adc |= 1;
+ }
+ else if(chan == 1)
+ {
+ adc_filtr_1[adc_filtr_p] = val;
+ adc_update_adc |= 2;
+ }
+ else if(chan == 2)
+ {
+ adc_filtr_2[adc_filtr_p] = val;
+ adc_update_adc |= 4;
+ }
+ else if(chan == 3)
+ {
+ adc_filtr_3[adc_filtr_p] = val;
+ adc_update_adc |= 8;
+
+ if(adc_filtr_p == (ADC_FILTR_SIZE -1 )) adc_filtr_p = 0;
+ else ++adc_filtr_p;
+ }
+
+
+ ADCR &= ~(ADC_CR_START_OFF_m);
+
+
+ switch(chan)
+ {
+ case 0:
+ ADCR = ((ADC_CR_ADC1_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 1:
+ ADCR = ((ADC_CR_ADC2_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 2:
+ ADCR = ((ADC_CR_ADC3_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+ break;
+
+ case 3:
+ ADCR = ((ADC_CR_ADC0_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+ break;
+ }
+
+ VICVectAddr = 0;
+
+}
+
+/**
+ * Inicializes ADC service for all ADC (4) channels and installs ISR routine to VIC.
+ * Automaticly starts the conversion of first channel on given conversion frequency.
+ */
+void init_adc_filter(unsigned rx_isr_vect)
+{
+
+ PINSEL1 |= ((PINSEL_1 << ADCCH0) | (PINSEL_1 << ADCCH1) | (PINSEL_1 << ADCCH2) | (PINSEL_1 << ADCCH3));
+
+ int x;
+
+ for (x = 0; x < 21; ++x)
+ {
+ adc_filtr_0[x] = 0;
+ adc_filtr_1[x] = 0;
+ adc_filtr_2[x] = 0;
+ adc_filtr_3[x] = 0;
+ }
+
+ adc_filtr_p = 0;
+ adc_update_adc = 0;
+
+ ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x32;
+ ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned) adc_isr_filtr;
+ VICIntEnable = 0x40000;
+
+ ADCR = ((ADC_CR_ADC0_m) | (ADC_CR_CLKS_11_m) | (ADC_CR_PDN_ON_m) | (ADC_CR_START_NOW_m) | (ADC_VPB_DIV*ADC_CR_CLK_DIV_1_m));
+}
--- /dev/null
+#ifndef ADC_FILTR_H
+#define ADC_FILTR_H
+
+#include <system_def.h>
+
+
+#define ADC_MUL 1 ///< This value multiplies mesured value from ADC
+#define ADC_OFFSET 0 ///< This valueis added to mesured value from ADC
+
+#define ACD_HZ 3000 ///< Sampling frequency
+
+#define ADC_VPB_DIV 0x80 ///< VPB divisor
+
+
+
+#if ADC_VPB_DIV & 0xffffff00
+ #error Wrong ACD_HZ
+#endif
+
+
+#define ADC_FILTR_SIZE 201 ///< Size of the median filter for each channel, be aware sinal size is size * 8
+
+volatile unsigned int adc_val[4]; ///< Final ADC value
+volatile unsigned int adc_update_adc; ///< bite oriented, updated when new ADC channel is measured
+
+
+
+uint16_t adc_filtr_0_m[ADC_FILTR_SIZE]; ///<
+uint16_t adc_filtr_1_m[ADC_FILTR_SIZE];
+uint16_t adc_filtr_2_m[ADC_FILTR_SIZE];
+uint16_t adc_filtr_3_m[ADC_FILTR_SIZE];
+
+
+
+volatile uint16_t adc_filtr_0[ADC_FILTR_SIZE];
+volatile uint16_t adc_filtr_1[ADC_FILTR_SIZE];
+volatile uint16_t adc_filtr_2[ADC_FILTR_SIZE];
+volatile uint16_t adc_filtr_3[ADC_FILTR_SIZE];
+volatile uint16_t adc_filtr_p ;
+
+void adc_filter(unsigned char chan);
+
+
+
+void init_adc_filter(unsigned rx_isr_vect);
+
+#endif
--- /dev/null
+
+
+
+
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include "engine.h"
+
+#define ENGINE_PIN_A 7 ///< PWM output for engine A, pin P0.7
+#define ENGINE_PIN_B 8 ///< PWM output for engine B, pin P0.8
+
+#define ENGINE_ENA 28 ///< Enable output for engine A, pin P1.28
+#define ENGINE_ENB 30 ///< Enable output for engine B, pin P1.30
+
+#define ENGINE_IN2A 27 ///< Direction output for engine A , pin P1.27
+#define ENGINE_IN2B 29 ///< Direction output for engine A , pin P1.29
+
+#define PWM_FREQ 20000 ///< PWM frequency
+#define PWM_RES 100 ///< PWM resolution
+
+#define PWM_STEP ((CPU_APB_HZ / PWM_FREQ)/PWM_RES + 1) ///< Calculates smallest step PWM for resolution 0~100%
+
+
+
+unsigned int pwm_A = 0; ///< Actual value of PWM A
+unsigned int pwm_B = 0; ///< Actual value of PWM B
+
+
+/**
+ * Enables or disables the ouput on engine A. Causes smooth stop, without break.
+ *
+ * @param status status parameters are defined in engine.h
+ *
+ */
+void engine_A_en(unsigned status)
+{
+ if (status == ENGINE_EN_ON)
+ IO1SET |= (1<<ENGINE_ENA);
+ else
+ IO1CLR |= (1<<ENGINE_ENA);
+}
+
+/**
+ * Enables or disables the ouput on engine B. Causes smooth stop, without break.
+ *
+ * @param status status parameters are defined in engine.h
+ *
+ */
+void engine_B_en(unsigned status)
+{
+ if (status == ENGINE_EN_ON)
+ IO1SET |= (1<<ENGINE_ENB);
+ else
+ IO1CLR |= (1<<ENGINE_ENB);
+}
+
+/**
+ * Changes motor A direction
+ *
+ * @param dir direction parameters are defined in engine.h
+ *
+ */
+void engine_A_dir(unsigned dir)
+{
+ if (dir == ENGINE_DIR_BW)
+ IO1SET |= (1<<ENGINE_IN2A);
+ else
+ IO1CLR |= (1<<ENGINE_IN2A);
+
+ engine_A_pwm(pwm_A);
+
+}
+
+/**
+ * Changes motor B direction
+ *
+ * @param dir direction parameters are defined in engine.h
+ *
+ */
+void engine_B_dir(unsigned dir)
+{
+ if (dir == ENGINE_DIR_BW)
+ IO1SET |= (1<<ENGINE_IN2B);
+ else
+ IO1CLR |= (1<<ENGINE_IN2B);
+
+ engine_B_pwm(pwm_B);
+}
+
+/**
+ * Sets motor A PWM value
+ *
+ * @param pwm Unsigned int, 0~100%
+ *
+ * @note 0 causes fast stop
+ */
+void engine_A_pwm(unsigned pwm)
+{
+ if (pwm>100) pwm = 100;
+
+ pwm_A = pwm;
+ if (IO1PIN & (1<<ENGINE_IN2A)) PWMMR2 = PWM_STEP * (100 - pwm);
+ else PWMMR2 = PWM_STEP * (pwm);
+ PWMLER |= PWMLER_LA2_m;
+
+}
+
+/**
+ * Sets motor B PWM value
+ *
+ * @param pwm Unsigned int, 0~100%
+ *
+ * @note 0 causes fast stop
+ */
+void engine_B_pwm(unsigned pwm)
+{
+ if (pwm>100) pwm = 100;
+ pwm_B = pwm;
+ if (IO1PIN & (1<<ENGINE_IN2B)) PWMMR4 = PWM_STEP * (100 - pwm);
+ else PWMMR4 = PWM_STEP * (pwm);
+ PWMLER |= PWMLER_LA4_m;
+
+}
+
+
+/**
+ * Inicializes Engine A - enables PWM outputs and sets IOs. Uses PWM channel - PWMMR2.
+ * If is somthing other set on PWM channels, it will affect the main PWM frequency
+ * If the engine B is set it will afect its output for one cycle
+ *
+ *
+ */
+void init_engine_A()
+{
+ PINSEL0 &= ~((PINSEL_3 << 14) );
+ PINSEL0 |= (PINSEL_2 << 14) ;
+
+ IO0DIR |= (1<<ENGINE_PIN_A);
+ IO1DIR |= (1<<ENGINE_ENA) | (1<<ENGINE_IN2A);
+ IO1SET |= (1<<ENGINE_ENA) | (1<<ENGINE_IN2A);
+
+ PWMPR = 0;
+ PWMMR0 = CPU_APB_HZ / 20000;
+
+ PWMMR2 =0;
+
+ PWMPCR |= PWMPCR_PWMENA2_m;
+ PWMLER |= PWMLER_LA0_m | PWMLER_LA2_m;
+ PWMMCR |= PWMMCR_PWMMR0R_m;
+ PWMTCR |= PWMTCR_CE_m | PWMTCR_EN_m;
+}
+
+/**
+ * Inicializes Engine B - enables PWM outputs and sets IOs. Uses PWM channel - PWMMR2.
+ * If is somthing other set on PWM channels, it will affect the main PWM frequency
+ * If the engine A is set it will afect its output for one cycle
+ *
+ *
+ */
+void init_engine_B()
+{
+ PINSEL0 &= ~((PINSEL_3 << 16));
+ PINSEL0 |= (PINSEL_2 << 16);
+
+ IO0DIR |= (1<<ENGINE_PIN_B);
+ IO1DIR |= (1<<ENGINE_ENB) | (1<<ENGINE_IN2B);
+
+ IO1SET |= (1<<ENGINE_ENB) | (1<<ENGINE_IN2B);
+
+
+ PWMPR = 0;
+ PWMMR0 = CPU_APB_HZ / 20000;
+
+ PWMMR4 = 0;
+
+ PWMPCR |= PWMPCR_PWMENA4_m;
+ PWMLER |= PWMLER_LA0_m | PWMLER_LA4_m;
+ PWMMCR |= PWMMCR_PWMMR0R_m;
+ PWMTCR |= PWMTCR_CE_m | PWMTCR_EN_m;
+}
\ No newline at end of file
--- /dev/null
+/**
+ * @file engine.h
+ *
+ * @brief Engines control
+ * This file provides simply how-to use eb_ebb library.
+ * From main function is called init_perip function
+ * where is initialized servos, engines, power switch,
+ * CAN,ADC and serial port. After this initialization is shown
+ * how to control each devices. This sample also include simply
+ * sample of sending and receiving CAN message.
+ *
+ * Due to small number of PWM channels are only one PWM assigned
+ * to one motor. For this case there is two ways how to stop the
+ * engine. If you use engine_x_pwm(0); the motors will be stopped
+ * by shorting its coils. This will stop as fast as possible, but
+ * it also generates huge electromagnetic noise. If you want to
+ * stop smoothly use engine_x_en(ENGINE_EN_OFF); this will disconnect
+ * the power from the engines.
+ *
+ */
+
+
+
+// Author: Jirka Kubias <jirka@Jirka-NB>, (C) 2008
+//
+// Copyright: See COPYING file that comes with this distribution
+//
+//
+
+
+
+
+
+#ifndef ENGINE_H
+#define ENGINE_H
+
+
+#define ENGINE_EN_ON 1 ///< Enables Engine
+#define ENGINE_EN_OFF 0 ///< Disables Engine
+
+
+#define ENGINE_DIR_BW 1 ///< Backward direction
+#define ENGINE_DIR_FW 0 ///< Forward direction
+
+
+void init_engine_A();
+void init_engine_B();
+
+void engine_A_en(unsigned status);
+void engine_B_en(unsigned status);
+
+void engine_A_dir(unsigned dir);
+void engine_B_dir(unsigned dir);
+
+void engine_A_pwm(unsigned pwm); // pwm is in percent, range 0~100
+void engine_B_pwm(unsigned pwm); // pwm is in percent, range 0~100
+
+
+
+#endif
--- /dev/null
+#ifndef EXPANSIONH
+#define EXPANSIONH
+
+/**
+ Defines for EbBoard Expansion port and GPIO port names.
+ This table describes relation between expansion/GPIO port number on EbBoard
+ and its coresponding port number on the processor.
+*/
+
+#define EXPPORT_1 2 /* P0_2 / SCL */
+#define EXPPORT_2 3 /* P0_3 / SDA */
+#define EXPPORT_3 8 /* P0_8 / TXD1 */
+#define EXPPORT_4 9 /* P0_9 / RXD1 */
+#define EXPPORT_5 18 /* P0_18 / MISO1 */
+#define EXPPORT_6 19 /* P0_19 / MOSI1 */
+#define EXPPORT_7 15 /* P0_15 / EINT2*/
+#define EXPPORT_8 17 /* P0_17 / SCK1*/
+
+#define GPIOPORT_1 16 /* P1_16 / GPIO1 */
+#define GPIOPORT_2 17 /* P1_17 / GPIO2 */
+#define GPIOPORT_3 18 /* P0_18 / GPIO3 */
+#define GPIOPORT_4 19 /* P0_19 / GPIO4 */
+#define GPIOPORT_5 20 /* P0_20 / GPIO5 */
+#define GPIOPORT_6 21 /* P0_21 / GPIO6 */
+#define GPIOPORT_7 22 /* P0_22 / GPIO7 */
+#define GPIOPORT_8 23 /* P0_23 / GPIO8 */
+
+#endif
\ No newline at end of file
--- /dev/null
+// $Id$
+// $Name$
+// $ProjectName$
+
+/**
+ * \mainpage EB-ebb code sample
+ *
+ * \section Introduction
+ * This codes are designed for use with ebBoard developed in DCE CVUT.
+ * There are two purpose of this program:
+ *
+ * 1) library named ebb, it contains function for handling engines,
+ * servos, ADC (median or avery filtered) and power switch.
+ *
+ * 2) in main.c file is simply HOW-TO use this libraries
+ *
+ * Short description of ebb library:
+ *
+ * ADC \96 This library use 4 ADC channels which parameters I defined
+ * in adc.h or adc_filtr.h. There is two implementation of ADC output
+ * filter. First is simply averaging ((last value + actual value)/2),
+ * for this filter you have to include adc.h. The Second filter is
+ * median filter in adc_filtr.h . The size of this filter is defined
+ * in adc_filtr.h.
+ *
+ * PowerSwitch \96 ebboard have one 6A power switch which can be used for
+ * switching small loads (capacity or resistant recommended). In powswitch.h is defined initialization, on and off function.
+ *
+ * Servos \96 ebboard have three servo connectors. In servo.h is defined
+ * functions for setting them. The servo range is divided into 256 (0~255)
+ * steps.
+ *
+ * Engines \96 this part can controls up two DC motors (5A each) or in
+ * special cases one motor (up to 10A). In engines.h is defined several
+ * controls function. Read carefully description in this section. Is
+ * important to understanding the way how to stop engines.
+ *
+ */
+
+/**
+ * @file main.c
+ *
+* @brief Demo application demonstrating use of eb_ebb library.
+ * This file provides simply how-to use eb_ebb library.
+ * From main function is called init_perip function
+ * where is initialized servos, engines, power switch,
+ * CAN,ADC and serial port. After this initialization is shown
+ * how to control each devices. This sample also include simply
+ * sample of sending and receiving CAN message.
+ *
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <errno.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "uart.h"
+#include "servo.h"
+#include "powswitch.h"
+#include "engine.h"
+//#include "adc.h" // header ADC with averaging filter
+#include "adc_filtr.h" // header ADC with mean filter
+
+
+
+
+#define CAN_SPEED 1000000 ///< CAN speed
+
+#define CAN_SERVO 0x32 ///< CAN ID for servo message
+
+/**
+ * Variables ISR values
+ */
+/*@{*/
+#define CAN_ISR 0
+#define ENG_ISR 1
+#define ADC_ISR 3
+#define SERVO_ISR 5
+/*@}*/
+
+#define SPEED 30 ///< Motor speed
+
+
+/**
+ * Dummy wait (busy wait)
+ * This function shoul be removed in future
+ */
+void dummy_wait(void)
+{
+ int i = 1000000;
+
+ while(--i);
+}
+
+/**
+ * This function is called when we recieve CAN message.
+ * Its called from CAN ISR
+ *
+ * @param *msg structure of can message (can_msg_t)
+ *
+ */
+void can_rx(can_msg_t *msg) {
+ can_msg_t rx_msg;
+
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));
+
+ switch (rx_msg.id) { // demo sample parsing recieved message by ID
+ case CAN_SERVO:
+
+ set_servo(0, rx_msg.data[0]);
+ set_servo(1, rx_msg.data[1]);
+ set_servo(2, rx_msg.data[2]);
+ break;
+
+ default:
+ break;
+ }
+}
+
+
+/**
+ * Inicializes pepherials used in ebb library - sample use
+ *
+ */
+void init_perip(void)
+{
+ init_servo(SERVO_ISR);
+ init_pow_switch();
+
+ init_engine_A();
+ init_engine_B();
+
+ init_uart0((int)9600 ,UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+ //init_adc(ADC_ISR); // inicialization ADC with averaging filter
+ init_adc_filter(ADC_ISR); // inicialization ADC with mean filter
+ can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
+}
+
+
+/**
+ * Main function contains a small sample how to use ebb library.
+ *
+ */
+int main (void) {
+
+ init_perip(); // sys init MCU
+
+ set_servo(0, 0x80); // set servo 1 to center position
+ set_servo(1, 0x80); // set servo 2 to center position
+ set_servo(2, 0x80); // set servo 3 to center position
+
+ engine_A_dir(ENGINE_DIR_FW); // set engine A to direction forward
+ engine_B_dir(ENGINE_DIR_BW); // set engine B to direction backward
+ engine_A_en(ENGINE_EN_ON); // enables engine A
+ engine_B_en(ENGINE_EN_ON); // enables engine B
+ engine_A_pwm(SPEED); // sets speed on Engine A
+ engine_B_pwm(SPEED); // sets speed on Engine B
+
+
+ can_msg_t msg; // This part is sending CAN messge
+ msg.id = 0xAA; // CAN message ID
+ msg.flags = 0;
+ msg.dlc = 1; // number of transmited data bytes
+ msg.data[0] = 0x55; // data byte
+ while(can_tx_msg(&msg)); // sending function
+
+
+ while(1) // this will blink for ever
+ {
+ __deb_led_on(LEDG);
+ dummy_wait();
+ __deb_led_off(LEDG);
+ dummy_wait();
+ }
+}
+
+
+
--- /dev/null
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include "powswitch.h"
+
+
+
+
+
+#define ESW (1<<16) ///< Pin define
+
+
+
+void init_pow_switch(void)
+{
+ IO0DIR |= ESW; // enable output
+ IO0CLR |= ESW; // sets to LOW level
+
+
+ PINSEL1 &= ~(PINSEL_3 << 0); // set as GPIO
+}
+
+void pow_switch_on(void)
+{
+ IO0SET |= ESW; // sets to LOW level
+}
+
+
+void pow_switch_off(void)
+{
+ IO0CLR |= ESW; // sets to LOW level
+}
--- /dev/null
+#ifndef POWSWITCH_H
+#define POWSWITCH_H
+
+/** Initializes power switch
+ */
+void init_pow_switch(void);
+
+
+/** Power switch ON
+ */
+void pow_switch_on(void);
+
+/** Power switch OFF
+ */
+void pow_switch_off(void);
+
+#endif
--- /dev/null
+
+
+#include <lpc21xx.h> // LPC21XX Peripheral Registers
+#include <deb_led.h>
+#include <system_def.h>
+#include "servo.h"
+
+
+#define SERVO2 (1<<10)
+#define SERVO0 (1<<12)
+#define SERVO1 (1<<13)
+
+
+
+#define TIM_EMR_NOTHING 0
+#define TIM_EMR_CLEAR 1
+#define TIM_EMR_SET 2
+#define TIM_EMR_TOGLE 3
+
+#define TIM_EMR_PIN_ON 1
+#define TIM_EMR_PIN_OFF 0
+
+#define TIME20MS ((CPU_APB_HZ) / 50)
+#define SERVOTICK (((CPU_APB_HZ / 50) / 20) / 256)
+
+
+unsigned char servo[3];
+
+
+
+// ---------------- SERVO PART -------------------------------
+
+
+void tc1 (void) __attribute__ ((interrupt));
+
+void tc1 (void) {
+
+ time_ms +=20;
+
+ T1EMR |= (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3);
+
+ T1MR0 = (servo[0] + 256) * SERVOTICK;
+ T1MR1 = (servo[1] + 256) * SERVOTICK;
+ T1MR3 = (servo[2] + 256) * SERVOTICK;
+
+ if (T1IR != 4)
+ {
+ __deb_led_on(LEDR);
+ }
+
+ T1IR = 4; // Vynulovani priznaku preruseni
+ VICVectAddr = 0; // Potvrzeni o obsluze preruseni
+}
+
+void set_servo(char serv, char position)
+{
+ if (serv == 0) servo[0] = position;
+ if (serv == 1) servo[1] = position;
+ if (serv == 2) servo[2] = position;
+}
+
+/* Setup the Timer Counter 1 Interrupt */
+void init_servo (unsigned rx_isr_vect)
+{
+
+ IO0DIR |= (SERVO0 | SERVO1 | SERVO2); // enables servo output
+ IO0SET |= (SERVO0 | SERVO1 | SERVO2); // sets to High level
+
+ PINSEL0 &= ~((PINSEL_3 << 24) | (PINSEL_3 << 26));
+ PINSEL0 |= (PINSEL_2 << 24) | (PINSEL_2 << 26);
+ PINSEL1 &= ~(PINSEL_3 << 8);
+ PINSEL1 |= (PINSEL_1 << 8);
+
+
+ servo[0] = 0;
+ servo[1] = 127;
+ servo[2] = 0xFF;
+
+ T1PR = 0;
+ T1MR2 = TIME20MS;
+ T1MR0 = (servo[0] + 256) * SERVOTICK;
+ T1MR1 = (servo[1] + 256) * SERVOTICK;
+ T1MR3 = (servo[2] + 256)* SERVOTICK;
+ T1MCR = (3<<6); // interrupt on MR1
+
+ T1EMR = (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3) \
+ | (TIM_EMR_CLEAR << 4) | (TIM_EMR_CLEAR << 6) | (TIM_EMR_CLEAR << 10);
+
+
+ T1TCR = 1; // Starts Timer 1
+
+
+ ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned long)tc1; // Nastaveni adresy vektotu preruseni
+ ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x20 | 0x5; // vyber casovece pro preruseni
+ VICIntEnable = (1<<5); // Povoli obsluhu preruseni
+}
+
+
+// ---------------- powSitch PART -------------------------------
+
--- /dev/null
+/**
+ * @file servo.h
+ *
+ * @brief FIXME
+ * This file provides simply how-to use eb_ebb library.
+ * From main function is called init_perip function
+ * where is initialized servos, engines, power switch,
+ * CAN,ADC and serial port. After this initialization is shown
+ * how to control each devices. This sample also include simply
+ * sample of sending and receiving CAN message.
+ *
+ */
+
+
+
+#ifndef SERVO_H
+#define SERVO_H
+
+
+
+/** gobal time
+ * @note incremented twenty 20ms, overrun every 1194hours
+ */
+volatile unsigned int time_ms;
+
+/** Initialize servos
+ * @note All three servos - should be fixed FIXME
+ */
+void init_servo(unsigned rx_isr_vect);
+
+
+/** Sets serv position
+ * @return 0
+ * @note VPB = APB - name conflict FIXME
+ * @param servo define servo
+ * @param position new position for servo
+ */
+void set_servo(char servo, char position);
+
+#endif
--- /dev/null
+\r
+ #include <lpc21xx.h> \r
+ #include "uart.h"\r
+ #include <system_def.h>\r
+\r
+\r
+\r
+\r
+ #define UART_DLAB 0x80\r
+\r
+\r
+void init_uart0(int baudrate, char bits, char stopbit, char parit_en, char parit_mode ) \r
+{ \r
+\r
+ int pom , vala, valb;\r
+\r
+ PINSEL0 |= ( PINSEL_1<< 0) | ( PINSEL_1 << 2); \r
+\r
+ U0LCR =UART_DLAB | bits | stopbit | parit_en | parit_mode; // nastaven� datov�ho slova na 8 bit� a jeden stop bit bez parity, nastaven� DLAB na log "1"\r
+ \r
+ \r
+ pom = (CPU_APB_HZ)/(16 * baudrate);\r
+ \r
+ vala = (CPU_APB_HZ)/(16 * pom);\r
+ valb = (CPU_APB_HZ)/(16 * (pom + 1));\r
+\r
+ vala = baudrate - vala;\r
+ valb = baudrate - valb;\r
+\r
+ if (vala < 0) vala *= -1;\r
+ if (valb < 0) valb *= -1;\r
+ \r
+ if (vala > valb) pom += 1;\r
+\r
+ U0DLL = (char) (pom & 0xFF);\r
+ U0DLM = (char) ((pom >> 8) & 0xFF); // nastaven� p�edd�li�ky na 57600Bd\r
+ \r
+ U0LCR &= ~UART_DLAB; // vynulov�n� DLAB \r
+\r
+ }\r
+\r
+\r
+unsigned char uart1GetCh(void) // Nuceny prijem z uart1\r
+{\r
+ while (!(U1LSR & 1)); // cekani na prichozi byte\r
+ return (unsigned char)U1RBR; // navraceni prijateho byte\r
+}\r
+\r
+unsigned char uart0GetCh(void) // Nuceny prijem z uart1\r
+{\r
+ while (!(U0LSR & 1)); // cekani na prichozi byte\r
+ return (unsigned char)U0RBR; // navraceni prijateho byte\r
+}\r
+\r
+void uart1SendCh(char ch) // vyslani znaku na uart1\r
+{\r
+ while (!(U1LSR & 0x20)); // ceka na odeslani predchozich dat\r
+ U1THR=ch; // odeslani Byte\r
+ \r
+}\r
+\r
+void uart0SendCh(char ch) // vyslani znaku na uart0\r
+{\r
+ while (!(U0LSR & 0x20)); // ceka na odeslani predchozich dat\r
+ U0THR=ch; // odeslani Byte\r
+}\r
+\r
+\r
+void init_uart1(int baudrate, char bits, char stopbit, char parit_en, char parit_mode ) \r
+{ \r
+\r
+ int pom , vala, valb;\r
+\r
+ PINSEL0 |= ( PINSEL_1<< 16) | ( PINSEL_1 << 18); \r
+\r
+ U1LCR =UART_DLAB | bits | stopbit | parit_en | parit_mode; // nastaven� datov�ho slova na 8 bit� a jeden stop bit bez parity, nastaven� DLAB na log "1"\r
+ \r
+ \r
+ pom = (CPU_APB_HZ)/(16 * baudrate);\r
+ \r
+ vala = (CPU_APB_HZ)/(16 * pom);\r
+ valb = (CPU_APB_HZ)/(16 * (pom + 1));\r
+\r
+ vala = baudrate - vala;\r
+ valb = baudrate - valb;\r
+\r
+ if (vala < 0) vala *= -1;\r
+ if (valb < 0) valb *= -1;\r
+ \r
+ if (vala > valb) pom += 1;\r
+\r
+ U1DLL = (char) (pom & 0xFF);\r
+ U1DLM = (char) ((pom >> 8) & 0xFF); // nastaven� p�edd�li�ky na 57600Bd\r
+ \r
+ U1LCR &= ~UART_DLAB; // vynulov�n� DLAB \r
+\r
+ }\r
+\r
+\r
+\r
--- /dev/null
+#ifndef UART_H\r
+#define UART_H\r
+\r
+#define UART_BITS_5 0x0\r
+#define UART_BITS_6 0x1\r
+#define UART_BITS_7 0x2\r
+#define UART_BITS_8 0x3\r
+\r
+#define UART_STOP_BIT_1 (0x0 << 2)\r
+#define UART_STOP_BIT_2 (0x1 << 2)\r
+\r
+#define UART_PARIT_ENA (0x1 << 3)\r
+#define UART_PARIT_OFF (0x0 << 3)\r
+#define UART_PARIT_ODD (0x0 << 4)\r
+#define UART_PARIT_EVEN (0x1 << 4)\r
+#define UART_PARIT_FORCE_1 (0x2 << 4)\r
+#define UART_PARIT_FORCE_0 (0x3 << 4)\r
+\r
+\r
+\r
+\r
+void init_uart1(int baudrate, char bits, char stopbit, char parit_en, char parit_mode );\r
+void init_uart0(int baudrate, char bits, char stopbit, char parit_en, char parit_mode );\r
+unsigned char uart1GetCh(void);\r
+unsigned char uart0GetCh(void);\r
+void uart1SendCh(char ch);\r
+void uart0SendCh(char ch);\r
+\r
+#endif\r
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+bin_PROGRAMS = eb_jaws
+
+eb_jaws_SOURCES = main.c fsm.c fsm_jaws.c uar.c
+eb_jaws_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+#endif
--- /dev/null
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+ fsm->current_state = initial_state;
+ fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+ fsm->last_state = fsm->current_state; // set actual state
+ fsm->current_state(fsm, EVENT_DO); // change parameter
+
+ if(fsm->last_state != fsm->current_state){ // if state was changed
+ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
+ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+ EVENT_ENTRY,
+ EVENT_DO,
+ EVENT_EXIT
+};
+
+// enum motor {
+// A="A",
+// B="B"
+// };
+
+// enum adc{
+// ADC0="0",
+// ADC1="1"
+// };
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+typedef void (*engine_fcn)(unsigned value);//pointer to function returning void and two input parametr
+
+
+
+struct fsm {
+ state_fcn current_state; // current state
+ state_fcn last_state; // last state
+ int32_t act_pos; // actual position
+ int32_t req_pos; // requested position
+ int32_t req_spd;
+ int32_t req_target;
+ volatile int32_t can_req_spd;
+ volatile uint32_t can_req_position; // next requested position
+ int32_t start_pos;
+ uint32_t can_response; // when the move is done, the value here equals to the req_pos
+ uint8_t flags; //< CAN flags bits (defined in can_msg_def.h)
+ uint32_t time_start; /* For timeout detection */
+ bool trigger_can_send;
+ int32_t max_pos;
+ int32_t min_pos;
+ engine_fcn engine_en;
+ engine_fcn engine_dir;
+ engine_fcn engine_pwm;
+ uint32_t can_id;
+// enum motor motor;
+// enum adc adc;
+};
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+
+
+void fsm_jaw_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->can_req_position = fsm->act_pos;
+ fsm->flags |= CAN_JAW_INITIALIZING;
+ break;
+ case EVENT_DO:
+ /* TODO: Homing */
+ fsm->flags &= ~CAN_JAW_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void stop(struct fsm *fsm)
+{
+ fsm->engine_pwm(0);
+ fsm->engine_en(ENGINE_EN_OFF);
+}
+
+
+#define DEAD_ZONE 10
+static bool do_control(struct fsm *fsm, int P)
+{
+ bool finished;
+ int e = fsm->req_pos - fsm->act_pos;
+ int action = (P*e) / 1; // ORIGINAL: int action = P*e;
+
+ fsm->engine_dir(action > 0);
+//#define BANG_BANG
+#ifdef BANG_BANG
+ action = action > 0 ? action : -action;
+ action = action > 40 ? 100 : 0;
+#else
+ action = action > 0 ? action : -action;
+#endif
+ fsm->engine_pwm(action);
+ fsm->engine_en(ENGINE_EN_ON);
+
+
+ if (fsm->req_target > fsm->start_pos)
+ finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
+ else
+ finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
+
+ return finished;
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ static int last_can_req_pos = 0;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop(fsm);
+ break;
+ case EVENT_DO:
+ do_control(fsm, 2);
+ if (fsm->can_req_position != last_can_req_pos &&
+ fsm->can_req_position != fsm->req_target) {
+ last_can_req_pos = fsm->can_req_position;
+ fsm->req_target = fsm->can_req_position;
+ fsm->req_spd = fsm->can_req_spd;
+ fsm->current_state = move;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
+static void move(struct fsm *fsm, enum event event)
+{
+ static int counter;
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ counter = 0;
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->start_pos = fsm->act_pos;
+ if(fsm->req_spd == 0)
+ fsm->req_pos = fsm->req_target;
+ else
+ fsm->req_pos = fsm->start_pos;
+ break;
+ case EVENT_DO:
+ if (fsm->can_req_position != fsm->req_target) {
+ fsm->flags |= CAN_JAW_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+ if(fsm->req_spd != 0 && counter++ >= 10)
+ {
+ counter = 0;
+ if(fsm->req_target > fsm->start_pos) {
+ fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+ } else {
+ fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+ }
+ }
+ if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
+ fsm->flags |= CAN_JAW_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+ finished = do_control(fsm, fsm->req_spd ? 2 : 2);
+ if (finished && fsm->req_pos == fsm->req_target) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop(fsm);
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+#include <servo.h>
+#include <expansion.h>
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define JAW_LEFT 0
+#define JAW_RIGHT 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+#define BUMPER_LEFT EXPPORT_5
+#define BUMPER_RIGHT EXPPORT_4
+
+#define BUMPER_LEFT_ACROSS EXPPORT_6
+#define BUMPER_RIGHT_ACROSS EXPPORT_8
+
+#define BUMPER_REAR_LEFT EXPPORT_7
+#define BUMPER_REAR_RIGHT EXPPORT_1
+
+struct fsm fsm_jaw_right;
+struct fsm fsm_jaw_left;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void init_motors(void){
+
+ init_engine_A(); // initialization of PWM unit
+ engine_A_en(ENGINE_EN_ON); //enable motor A
+ engine_A_dir(ENGINE_DIR_FW); //set direction
+ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+ init_engine_B(); // initialization of PWM unit
+ engine_B_en(ENGINE_EN_ON); //enable motor A
+ engine_B_dir(ENGINE_DIR_FW); //set direction
+ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+/* vhn_init(); */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ //JAW_LEFT_OPEN ff 0xff
+ //JAW_RIGHT_OPEN 0x00
+
+ //JAW_LEFT_CLOSE 0x80
+ //JAW_RIGHT_CLOSE 0x80
+
+ //JAW_LEFT_CATCH 0xB8
+ //JAW_RIGHT_CATCH 0x38
+
+ case CAN_JAW_LEFT_CMD:
+
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+
+ if (req >= 0x50 && req <= 0xD0) {
+ set_servo(JAW_LEFT, (char)req);
+ }
+
+// if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
+// fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
+// fsm_jaw_left.can_req_position = req;
+// fsm_jaw_left.can_req_spd = spd;
+// } else
+// fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
+ break;
+
+ case CAN_JAW_RIGHT_CMD:
+
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+
+ if (req >= 0x60 && req <= 0xD0) {
+ set_servo(JAW_RIGHT, (char)req);
+ }
+
+// if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
+// fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
+// fsm_jaw_right.can_req_position = req;
+// fsm_jaw_right.can_req_spd = spd;
+// } else
+// fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
+ break;
+
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ //init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+ init_adc(ADC_ISR);
+
+ init_servo(7); //7 is interrupt priority
+ set_servo(JAW_LEFT, 0xB0);
+ set_servo(JAW_RIGHT,0x70);
+}
+
+/*********************************************************/
+void can_send_status(struct fsm *fsm){
+ can_msg_t tx_msg;
+ tx_msg.id = fsm->can_id;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm->act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm->act_pos & 0xFF;
+ tx_msg.data[2] = (fsm->can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm->can_response & 0xFF;
+ tx_msg.data[4] = fsm->flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_jaw_init(struct fsm *fsm, enum event event);
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+ /* static int up;
+ if (up == 0)
+ fsm_vidle.can_req_position = 0x380;
+ if (up == 6)
+ fsm_vidle.can_req_position = 0x1e0;
+ up = (up+1)%12;
+ */
+ deb_led_change(LEDG);
+
+ send_rs_str("LEFT_JAW: ");
+ send_rs_int(fsm_jaw_left.act_pos);
+ send_rs_str("\tLEFT_FLAGS: ");
+ send_rs_int(fsm_jaw_left.flags);
+
+ send_rs_str("\tRIGHT_JAW: ");
+ send_rs_int(fsm_jaw_right.act_pos);
+ send_rs_str("\tRIGHT_FLAGS: ");
+ send_rs_int(fsm_jaw_right.flags);
+
+ send_rs_str("\n");
+
+ }
+}
+
+void handle_bumper()
+{
+ static uint32_t bumper_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= bumper_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ bumper_time = timer_msec;
+
+
+
+ if (IO0PIN & (1<<BUMPER_REAR_LEFT)){
+ sw &= ~CAN_BUMPER_REAR_LEFT;
+
+ send_rs_str("rear_left\n");
+
+// deb_led_on(LEDY);
+ }
+ else{
+ sw |= CAN_BUMPER_REAR_LEFT;
+
+// deb_led_off(LEDY);
+ }
+
+ if (IO0PIN & (1<<BUMPER_REAR_RIGHT)){
+ sw &= ~CAN_BUMPER_REAR_RIGHT;
+
+ send_rs_str("rear_right\n");
+
+// deb_led_on(LEDY);
+ }
+ else{
+ sw |= CAN_BUMPER_REAR_RIGHT;
+
+// deb_led_off(LEDY);
+ }
+
+ if (IO0PIN & (1<<BUMPER_LEFT)){
+
+ sw &= ~CAN_BUMPER_LEFT;
+
+ send_rs_str("left\n");
+
+// deb_led_on(LEDB);
+ }
+ else{
+
+ sw |= CAN_BUMPER_LEFT;
+
+// deb_led_off(LEDB);
+ }
+ if (IO0PIN & (1<<BUMPER_RIGHT)){
+ sw &= ~CAN_BUMPER_RIGHT;
+ send_rs_str("right\n");
+// deb_led_on(LEDG);
+ }
+ else{
+ sw |= CAN_BUMPER_RIGHT;
+
+// deb_led_off(LEDG);
+ }
+ if (IO0PIN & (1<<BUMPER_LEFT_ACROSS)){
+ sw &= ~CAN_BUMPER_LEFT_ACROSS;
+ send_rs_str("left_across\n");
+// deb_led_on(LEDR);
+ }
+ else{
+ sw |= CAN_BUMPER_LEFT_ACROSS;
+
+// deb_led_off(LEDR);
+ }
+ if (IO0PIN & (1<<BUMPER_RIGHT_ACROSS)){
+ sw &= ~CAN_BUMPER_RIGHT_ACROSS;
+ send_rs_str("right_across\n");
+// deb_led_on(LEDY);
+ }
+ else{
+ sw |= CAN_BUMPER_RIGHT_ACROSS;
+
+// deb_led_off(LEDY);
+ }
+
+ if (sw & (CAN_BUMPER_REAR_LEFT | CAN_BUMPER_REAR_RIGHT | CAN_BUMPER_LEFT | CAN_BUMPER_RIGHT | CAN_BUMPER_LEFT_ACROSS | CAN_BUMPER_RIGHT_ACROSS))
+ deb_led_on(LEDR);
+ else
+ deb_led_off(LEDR);
+
+// send_rs_int(IO1PIN);
+// send_rs_int(sw);
+// send_rs_str("\n");
+//
+
+ tx_msg.id = CAN_ROBOT_BUMPERS;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time_left = timer_usec;
+ uint32_t status_time_right = timer_usec;
+
+
+ init_periphery();
+
+ SET_PIN(PINSEL1, 1, PINSEL_0);
+ SET_PIN(PINSEL1, 2, PINSEL_0);
+ SET_PIN(PINSEL1, 3, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_REAR_LEFT, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_REAR_RIGHT, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
+
+// IO0DIR &= ~((1<<BUMPER_REAR) | (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
+
+ send_rs_str("Jaws started\n");
+
+ //close jaw, max_pos, open jaw min_pos
+ //letf jaw potenciometer on ADC 0, header J32 on board
+ fsm_jaw_left.max_pos=0x320; //max 0x324
+ fsm_jaw_left.min_pos=0xB0; //min 0xC3
+ fsm_jaw_left.act_pos = adc_val[0];
+
+ //close jaw, max_pos, open jaw min_pos
+ //letf jaw potenciometer on ADC 1, header J33 on board
+ fsm_jaw_right.max_pos=0x380; //max 0x382
+ fsm_jaw_right.min_pos=0x010; //min 0xF5
+ fsm_jaw_right.act_pos = adc_val[1];
+
+ //left jaw engine is engine A, conector MOTA on board
+ fsm_jaw_left.engine_en = &engine_A_en;
+ fsm_jaw_left.engine_dir = &engine_A_dir;
+ fsm_jaw_left.engine_pwm = &engine_A_pwm;
+
+ fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
+
+ //right jaw engine is engine B, conector MOTB on board
+ fsm_jaw_right.engine_en = &engine_B_en;
+ fsm_jaw_right.engine_dir = &engine_B_dir;
+ fsm_jaw_right.engine_pwm = &engine_B_pwm;
+
+ fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
+
+ init_fsm(&fsm_jaw_right, &fsm_jaw_init);
+ init_fsm(&fsm_jaw_left, &fsm_jaw_init);
+
+/* test_vhn(); */
+/* return; */
+
+ while(1){
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+
+ //fsm_jaw_left.act_pos = adc_val[0];
+ //run_fsm(&fsm_jaw_left);
+
+ //fsm_jaw_right.act_pos = adc_val[1];
+ //run_fsm(&fsm_jaw_right);
+
+ }
+
+ if (timer_msec >= status_time_left + 100 /*|| //repeat sending message every 100 ms
+ fsm_jaw_left.trigger_can_send*/) { //or when something important happen
+ //fsm_jaw_left.trigger_can_send = false;
+ status_time_left = timer_msec; //save new time, when message was sent
+ can_send_status(&fsm_jaw_left);
+
+ }
+
+ if (timer_msec >= status_time_right + 100 /*|| //repeat sending message every 100 ms
+ fsm_jaw_right.trigger_can_send*/) { //or when something important happen
+ //fsm_jaw_right.trigger_can_send = false;
+ status_time_right = timer_msec; //save new time, when message was sent
+ can_send_status(&fsm_jaw_right);
+
+ }
+
+ handle_bumper();
+
+ blink_led();
+ }
+}
+
+/** @} */
--- /dev/null
+#include <uart.h>
+
+
+
+
+/**
+ * Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+ uart0SendCh(val);
+}
+
+/**
+ * Read one char from uart.
+ */
+char uart_get_char(void)
+{
+ return uart0GetCh();
+}
+
+/**
+ * Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+ init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ * Send string to serial output in ASCII code. .
+ * @param data[] string to print
+ */
+void send_rs_str(const char data[])
+{
+
+ int i = 0;
+ int j = 0;
+
+ for (j = 0; j < 255; j++)
+ {
+ if(data[j] == 0) break;
+ }
+
+ for (i= 0 ; i < j; i++)
+ {
+ uart_send_char(data[i]);
+ }
+}
+
+/**
+ * Send int value to serial output in ASCII code. Removes unused zeros.
+ * @param val value to print
+ */
+void send_rs_int(int val)
+{
+ char dat[8];
+ int i;
+ int pom = 0;
+
+ for(i = 0; i < 8; i++)
+ {
+ dat[i] = (val & 0xF) + 0x30;
+ if(dat[i] > '9')
+ dat[i] += 7;
+ val >>= 4;
+ }
+
+ for(i = 0; i < 8; i++)
+ {
+ if((pom == 0) & (dat[7-i] == '0'))
+ {
+ if((i == 6) | (i == 7))
+ uart_send_char('0');
+ continue;
+ }
+ pom = 1;
+ uart_send_char(dat[7-i]);
+ }
+
+}
--- /dev/null
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+bin_PROGRAMS = eb_lift
+
+eb_lift_SOURCES = main.c fsm.c fsm_lift.c uar.c
+eb_lift_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+#endif
--- /dev/null
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+ fsm->current_state = initial_state;
+ fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+ fsm->last_state = fsm->current_state; // set actual state
+ fsm->current_state(fsm, EVENT_DO); // change parameter
+
+ if(fsm->last_state != fsm->current_state){ // if state was changed
+ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
+ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+ EVENT_ENTRY,
+ EVENT_DO,
+ EVENT_EXIT
+};
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+
+struct fsm {
+ state_fcn current_state; // current state
+ state_fcn last_state; // last state
+ int32_t act_pos; // actual position
+ int32_t req_pos; // requested position
+ int32_t req_spd;
+ int32_t req_target;
+ volatile int32_t can_req_spd;
+ volatile uint32_t can_req_position; // next requested position
+ int32_t start_pos;
+ uint32_t can_response; // when the move is done, the value here equals to the req_pos
+ uint8_t flags; //< CAN flags bits (defined in can_msg_def.h)
+ uint32_t time_start; /* For timeout detection */
+ bool trigger_can_send;
+ uint8_t can_req_homing;
+};
+
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+static void homing(struct fsm *fsm, enum event event);
+static void stop(void);
+
+
+static void homing(struct fsm *fsm, enum event event)
+{
+ static uint32_t time_start = 0;
+
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ time_start = timer_msec;
+ break;
+ case EVENT_DO:
+ //homing with 8s timeout
+ engine_A_en(ENGINE_EN_ON);
+ engine_A_dir(ENGINE_DIR_FW);
+ engine_A_pwm(50);
+
+ if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
+ engine_A_pwm(0);
+ fsm->act_pos = 0;
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ fsm->can_req_position = 0;
+ // fsm->can_req_position = 0x54E2;
+ } else if (timer_msec >= (time_start + 4000)) {
+ stop();
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->flags &= ~CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->flags |= CAN_LIFT_INITIALIZING;
+ break;
+ case EVENT_DO:
+ fsm->flags &= ~CAN_LIFT_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void stop()
+{
+ engine_A_pwm(0);
+ engine_A_en(ENGINE_EN_OFF);
+}
+
+
+#define DEAD_ZONE 10
+static bool do_control(struct fsm *fsm, int P)
+{
+ bool finished;
+ int e = fsm->req_pos - fsm->act_pos;
+ int action = (P*e) / 5; // ORIGINAL: int action = P*e;
+
+ engine_A_dir(action > 0);
+ action = action > 100 ? 100 : action;
+
+//#define BANG_BANG
+#ifdef BANG_BANG
+ action = action > 0 ? action : -action;
+ action = action > 40 ? 100 : 0;
+#else
+ action = action > 0 ? action : -action;
+#endif
+
+ engine_A_pwm(action);
+ engine_A_en(ENGINE_EN_ON);
+
+ if (fsm->req_target > fsm->start_pos)
+ finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
+ else
+ finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
+
+ return finished;
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ static int last_can_req_pos = 0;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ //homing if push home button or homing can msg. received
+ if ((fsm->flags & CAN_LIFT_SWITCH_HOME) || (fsm->can_req_homing)) {
+ fsm->can_req_homing = 0;
+ fsm->current_state = homing;
+ break;
+ }
+ /* if lift is not homed, do not allow movement and regulation */
+ if (fsm->flags & CAN_LIFT_HOMED) {
+ do_control(fsm, 2);
+
+ if (fsm->can_req_position != last_can_req_pos &&
+ fsm->can_req_position != fsm->req_target) {
+ last_can_req_pos = fsm->can_req_position;
+ fsm->req_target = fsm->can_req_position;
+ fsm->req_spd = fsm->can_req_spd;
+ fsm->current_state = move;
+ }
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
+static void move(struct fsm *fsm, enum event event)
+{
+ static int counter;
+ static bool lift_stopped_on_end = false;
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ counter = 0;
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->start_pos = fsm->act_pos;
+
+ /* check if movement starts on end switch */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) || (fsm->flags & CAN_LIFT_SWITCH_UP))
+ lift_stopped_on_end = true;
+ else
+ lift_stopped_on_end = false;
+
+ if(fsm->req_spd == 0)
+ fsm->req_pos = fsm->req_target;
+ else
+ fsm->req_pos = fsm->start_pos;
+ break;
+ case EVENT_DO:
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_UP) && !lift_stopped_on_end){
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && !lift_stopped_on_end) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->act_pos = 0;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+
+ if (fsm->can_req_position != fsm->req_target) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+
+ if(fsm->req_spd != 0 && counter++ >= 10)
+ {
+ counter = 0;
+ if(fsm->req_target > fsm->start_pos) {
+ fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+ } else {
+ fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+ }
+ }
+
+ if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 2000 : 4000)) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+
+ finished = do_control(fsm, fsm->req_spd ? 2 : 2);
+
+ if (finished && fsm->req_pos == fsm->req_target) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop();
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+
+struct fsm fsm_lift;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#define END_SWITCH_UP_PIN 9 //pin 4, exp. port on board
+#define END_SWITCH_DOWN_PIN 8 //pin 3, exp. port on board
+#define SWITCH_HOME_PIN 19 //pin 6, exp. port on board
+#define START_PIN 15 //pin 7, exp. port on board
+#define COLOR_PIN 18 //pin 5, exp. port on board
+#define SWITCH_STRATEGY_PIN 17 //pin 8, exp. port on board
+
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
+#define LIFT_IRC_VAL_MAX 0x19C
+#define LIFT_IRC_VAL_MIN 0x0
+
+#define IRC_A_PIN 2 //pin 1, exp. port on board
+#define IRC_B_PIN 3 //pin 2, exp. port on board
+
+#define IRC_A_MASK 0x04 //(1<<IRC_A)
+#define IRC_B_MASK 0x08 //(1<<IRC_B)
+#define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
+
+void lift_switches_handler(void);
+
+void lift_switches_handler()
+{
+ if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<SWITCH_HOME_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
+ deb_led_on(LEDR);
+ }
+// if (IO0PIN & (1<<IRC_A_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+// if (IO0PIN & (1<<IRC_B_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+}
+
+//source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
+int32_t irc_read_tick(){
+
+ static uint16_t cnt_up = 0;
+ static uint16_t cnt_down = 0;
+ static uint16_t last_irc = 0;
+ static int32_t position = 0;
+ uint16_t irc, temp;
+
+ irc = IO0PIN & IRC_AB_MASK;
+ if ((irc & IRC_B_MASK) != 0){
+ irc ^= IRC_A_MASK;
+ }
+
+ temp = (irc - last_irc) & IRC_AB_MASK;
+
+ last_irc = irc;
+
+ if (temp == IRC_A_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_down >= 100) {
+ cnt_down = 0;
+ cnt_up = 0;
+ deb_led_change(LEDB);
+ return --position;
+ }
+ } else if (temp == IRC_AB_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_up >= 100) {
+ cnt_up = 0;
+ cnt_down = 0;
+ deb_led_change(LEDB);
+ return ++position;
+ }
+ }
+ return position;
+}
+
+void init_motors(void){
+
+ init_engine_A(); // initialization of PWM unit
+ engine_A_en(ENGINE_EN_ON); //enable motor A
+ engine_A_dir(ENGINE_DIR_FW); //set direction
+ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+
+
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+
+ fsm_lift.flags |= CAN_LIFT_START;
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+
+// send_rs_str("start\n");
+
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ case CAN_LIFT_CMD:
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+ fsm_lift.can_req_homing=rx_msg.data[3];
+ // range 0 - A9C5
+ if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
+ fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;
+ fsm_lift.can_req_position = req;// save new req position of lift
+ fsm_lift.can_req_spd = spd;// save new req spd of lift
+ } else
+ fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS;
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+// init_adc(ADC_ISR);
+}
+/*********************************************************/
+void can_send_status(void)
+{
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_LIFT_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_lift.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_lift.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_lift.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_lift.can_response & 0xFF;
+ tx_msg.data[4] = fsm_lift.flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event);
+
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+// static int up;
+// if (up == 0)
+// fsm_lift.can_req_position = 0x380;
+// if (up == 6)
+// fsm_lift.can_req_position = 0x1e0;
+// up = (up+1)%12;
+
+ deb_led_change(LEDG);
+
+ send_rs_str("ACT_POS\t");
+ send_rs_int(fsm_lift.act_pos);
+ send_rs_str("\t");
+ send_rs_str("CAN_FLAGS\t");
+ send_rs_int(fsm_lift.flags);
+ send_rs_str("\n");
+ }
+}
+
+void robot_switches_handler()
+{
+ static uint32_t color_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= color_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ color_time = timer_msec;
+
+ if (IO0PIN & (1<<COLOR_PIN))
+ sw |= CAN_SWITCH_COLOR;
+ else
+ sw &= ~CAN_SWITCH_COLOR;
+
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
+ sw &= ~CAN_SWITCH_STRATEGY;
+ else
+ sw |= CAN_SWITCH_STRATEGY;
+
+ if (sw & CAN_SWITCH_COLOR){
+ deb_led_off(LEDY);
+
+ send_rs_str("color\n");}
+ else
+ deb_led_on(LEDY);
+
+ if (sw & CAN_SWITCH_STRATEGY){
+ deb_led_off(LEDY);
+
+ send_rs_str("strategy\n");
+ }
+ else
+ deb_led_on(LEDY);
+
+// send_rs_int(IO1PIN);
+// send_rs_int(sw);
+// send_rs_str("\n");
+//
+ /* TODO: Put color to the second bit */
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+
+ //lift motor is motor A, MOTA connctor on board
+ init_periphery();
+
+ SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
+ SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0); //init of color pin
+ SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN - 16), PINSEL_0); //init of strategy pin
+ SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0); //init of home pin
+
+ IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN) | (1 << SWITCH_STRATEGY_PIN));
+ IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
+ IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
+ send_rs_str("Lift started\n");
+
+ fsm_lift.act_pos = 0;
+ init_fsm(&fsm_lift, &fsm_lift_init);
+
+/* return; */
+
+ while(1){
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+
+// fsm_lift.act_pos = adc_val[0];
+
+ run_fsm(&fsm_lift);
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_lift.trigger_can_send) { //or when something important happen
+ fsm_lift.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ can_send_status();
+ }
+
+ fsm_lift.act_pos = irc_read_tick();
+
+ start_button();
+ robot_switches_handler();
+ lift_switches_handler();
+ blink_led();
+ }
+}
+
+/** @} */
--- /dev/null
+#include <uart.h>
+
+
+
+
+/**
+ * Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+ uart0SendCh(val);
+}
+
+/**
+ * Read one char from uart.
+ */
+char uart_get_char(void)
+{
+ return uart0GetCh();
+}
+
+/**
+ * Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+ init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ * Send string to serial output in ASCII code. .
+ * @param data[] string to print
+ */
+void send_rs_str(const char data[])
+{
+
+ int i = 0;
+ int j = 0;
+
+ for (j = 0; j < 255; j++)
+ {
+ if(data[j] == 0) break;
+ }
+
+ for (i= 0 ; i < j; i++)
+ {
+ uart_send_char(data[i]);
+ }
+}
+
+/**
+ * Send int value to serial output in ASCII code. Removes unused zeros.
+ * @param val value to print
+ */
+void send_rs_int(int val)
+{
+ char dat[8];
+ int i;
+ int pom = 0;
+
+ for(i = 0; i < 8; i++)
+ {
+ dat[i] = (val & 0xF) + 0x30;
+ if(dat[i] > '9')
+ dat[i] += 7;
+ val >>= 4;
+ }
+
+ for(i = 0; i < 8; i++)
+ {
+ if((pom == 0) & (dat[7-i] == '0'))
+ {
+ if((i == 6) | (i == 7))
+ uart_send_char('0');
+ continue;
+ }
+ pom = 1;
+ uart_send_char(dat[7-i]);
+ }
+
+}
--- /dev/null
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+bin_PROGRAMS = eb_lift_11
+
+eb_lift_11_SOURCES = main.c fsm.c fsm_lift.c uar.c
+eb_lift_11_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+#endif
--- /dev/null
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+ fsm->current_state = initial_state;
+ fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+ fsm->last_state = fsm->current_state; // set actual state
+ fsm->current_state(fsm, EVENT_DO); // change parameter
+
+ if(fsm->last_state != fsm->current_state){ // if state was changed
+ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
+ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+ EVENT_ENTRY,
+ EVENT_DO,
+ EVENT_EXIT
+};
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+
+struct fsm {
+ state_fcn current_state; // current state
+ state_fcn last_state; // last state
+ int32_t act_pos; // actual position
+ int32_t req_pos; // requested position
+ int32_t req_spd;
+ int32_t req_target;
+ volatile int32_t can_req_spd;
+ volatile uint32_t can_req_position; // next requested position
+ int32_t start_pos;
+ uint32_t can_response; // when the move is done, the value here equals to the req_pos
+ uint8_t flags; //< CAN flags bits (defined in can_msg_def.h)
+ uint32_t time_start; /* For timeout detection */
+ bool trigger_can_send;
+ uint8_t can_req_homing;
+};
+
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+static void homing(struct fsm *fsm, enum event event);
+static void stop(void);
+
+
+static void homing(struct fsm *fsm, enum event event)
+{
+ static uint32_t time_start = 0;
+
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ time_start = timer_msec;
+ break;
+ case EVENT_DO:
+ //homing with 8s timeout
+ engine_A_en(ENGINE_EN_ON);
+ engine_A_dir(ENGINE_DIR_FW);
+ engine_A_pwm(50);
+
+ if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
+ engine_A_pwm(0);
+ fsm->act_pos = 0;
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ fsm->can_req_position = 0;
+ // fsm->can_req_position = 0x54E2;
+ } else if (timer_msec >= (time_start + 4000)) {
+ stop();
+ fsm->current_state = wait_for_cmd;
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->flags &= ~CAN_LIFT_HOMED;
+ fsm->trigger_can_send = 1;
+ fsm->can_req_homing = 0;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->flags |= CAN_LIFT_INITIALIZING;
+ break;
+ case EVENT_DO:
+ fsm->flags &= ~CAN_LIFT_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void stop()
+{
+ engine_A_pwm(0);
+ engine_A_en(ENGINE_EN_OFF);
+}
+
+
+#define DEAD_ZONE 10
+static bool do_control(struct fsm *fsm, int P)
+{
+ bool finished;
+ int e = fsm->req_pos - fsm->act_pos;
+ int action = (P*e) / 5; // ORIGINAL: int action = P*e;
+
+ engine_A_dir(action > 0);
+ action = action > 100 ? 100 : action;
+
+//#define BANG_BANG
+#ifdef BANG_BANG
+ action = action > 0 ? action : -action;
+ action = action > 40 ? 100 : 0;
+#else
+ action = action > 0 ? action : -action;
+#endif
+
+ engine_A_pwm(action);
+ engine_A_en(ENGINE_EN_ON);
+
+ if (fsm->req_target > fsm->start_pos)
+ finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
+ else
+ finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
+
+ return finished;
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ static int last_can_req_pos = 0;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ //homing if push home button or homing can msg. received
+ if ((fsm->flags & CAN_LIFT_SWITCH_HOME) || (fsm->can_req_homing)) {
+ fsm->can_req_homing = 0;
+ fsm->current_state = homing;
+ break;
+ }
+ /* if lift is not homed, do not allow movement and regulation */
+ if (fsm->flags & CAN_LIFT_HOMED) {
+ do_control(fsm, 2);
+
+ if (fsm->can_req_position != last_can_req_pos &&
+ fsm->can_req_position != fsm->req_target) {
+ last_can_req_pos = fsm->can_req_position;
+ fsm->req_target = fsm->can_req_position;
+ fsm->req_spd = fsm->can_req_spd;
+ fsm->current_state = move;
+ }
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
+static void move(struct fsm *fsm, enum event event)
+{
+ static int counter;
+ static bool lift_stopped_on_end = false;
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ counter = 0;
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->start_pos = fsm->act_pos;
+
+ /* check if movement starts on end switch */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) || (fsm->flags & CAN_LIFT_SWITCH_UP))
+ lift_stopped_on_end = true;
+ else
+ lift_stopped_on_end = false;
+
+ if(fsm->req_spd == 0)
+ fsm->req_pos = fsm->req_target;
+ else
+ fsm->req_pos = fsm->start_pos;
+ break;
+ case EVENT_DO:
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_UP) && !lift_stopped_on_end){
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+ /* if movement starts on end switch, ignore this, else stop movement on act position */
+ if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && !lift_stopped_on_end) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->act_pos = 0;
+ fsm->req_pos = fsm->act_pos;
+ lift_stopped_on_end = true;
+ }
+
+ if (fsm->can_req_position != fsm->req_target) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+
+ if(fsm->req_spd != 0 && counter++ >= 10)
+ {
+ counter = 0;
+ if(fsm->req_target > fsm->start_pos) {
+ fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+ } else {
+ fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+ }
+ }
+
+ if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 2000 : 4000)) {
+ fsm->flags |= CAN_LIFT_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+
+ finished = do_control(fsm, fsm->req_spd ? 2 : 2);
+
+ if (finished && fsm->req_pos == fsm->req_target) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop();
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+#include <expansion.h>
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+
+struct fsm fsm_lift;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#define END_SWITCH_UP_PIN EXPPORT_4 //pin 4, exp. port on board
+#define END_SWITCH_DOWN_PIN EXPPORT_3 //pin 3, exp. port on board
+#define SWITCH_HOME_PIN EXPPORT_6 //pin 6, exp. port on board
+#define START_PIN EXPPORT_7 //pin 7, exp. port on board
+#define COLOR_PIN EXPPORT_5 //pin 5, exp. port on board
+#define SWITCH_STRATEGY_PIN EXPPORT_8 //pin 8, exp. port on board
+
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
+#define LIFT_IRC_VAL_MAX 0x19C
+#define LIFT_IRC_VAL_MIN 0x0
+
+#define IRC_A_PIN 2 //pin 1, exp. port on board
+#define IRC_B_PIN 3 //pin 2, exp. port on board
+
+#define IRC_A_MASK 0x04 //(1<<IRC_A)
+#define IRC_B_MASK 0x08 //(1<<IRC_B)
+#define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
+
+void lift_switches_handler(void);
+
+void lift_switches_handler()
+{
+ if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
+ deb_led_on(LEDR);
+ }
+
+ if (IO0PIN & (1<<SWITCH_HOME_PIN)){
+ fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
+ deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
+ deb_led_on(LEDR);
+ }
+// if (IO0PIN & (1<<IRC_A_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+// if (IO0PIN & (1<<IRC_B_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+}
+
+//source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
+int32_t irc_read_tick(){
+
+ static uint16_t cnt_up = 0;
+ static uint16_t cnt_down = 0;
+ static uint16_t last_irc = 0;
+ static int32_t position = 0;
+ uint16_t irc, temp;
+
+ irc = IO0PIN & IRC_AB_MASK;
+ if ((irc & IRC_B_MASK) != 0){
+ irc ^= IRC_A_MASK;
+ }
+
+ temp = (irc - last_irc) & IRC_AB_MASK;
+
+ last_irc = irc;
+
+ if (temp == IRC_A_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_down >= 100) {
+ cnt_down = 0;
+ cnt_up = 0;
+ deb_led_change(LEDB);
+ return --position;
+ }
+ } else if (temp == IRC_AB_MASK){
+ /* count 100times slower - we do not need 250 ticks per milimeter*/
+ if (++cnt_up >= 100) {
+ cnt_up = 0;
+ cnt_down = 0;
+ deb_led_change(LEDB);
+ return ++position;
+ }
+ }
+ return position;
+}
+
+void init_motors(void){
+
+ init_engine_A(); // initialization of PWM unit
+ engine_A_en(ENGINE_EN_ON); //enable motor A
+ engine_A_dir(ENGINE_DIR_FW); //set direction
+ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+
+
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+
+ fsm_lift.flags |= CAN_LIFT_START;
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+
+// send_rs_str("start\n");
+
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ case CAN_LIFT_CMD:
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+ fsm_lift.can_req_homing=rx_msg.data[3];
+ // range 0 - A9C5
+ if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
+ fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;
+ fsm_lift.can_req_position = req;// save new req position of lift
+ fsm_lift.can_req_spd = spd;// save new req spd of lift
+ } else
+ fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS;
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+// init_adc(ADC_ISR);
+}
+/*********************************************************/
+void can_send_status(void)
+{
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_LIFT_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_lift.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_lift.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_lift.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_lift.can_response & 0xFF;
+ tx_msg.data[4] = fsm_lift.flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event);
+
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+// static int up;
+// if (up == 0)
+// fsm_lift.can_req_position = 0x380;
+// if (up == 6)
+// fsm_lift.can_req_position = 0x1e0;
+// up = (up+1)%12;
+
+ deb_led_change(LEDG);
+
+ send_rs_str("ACT_POS\t");
+ send_rs_int(fsm_lift.act_pos);
+ send_rs_str("\t");
+ send_rs_str("CAN_FLAGS\t");
+ send_rs_int(fsm_lift.flags);
+ send_rs_str("\n");
+ }
+}
+
+void robot_switches_handler()
+{
+ static uint32_t color_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= color_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ color_time = timer_msec;
+
+ if (IO0PIN & (1<<COLOR_PIN))
+ sw |= CAN_SWITCH_COLOR;
+ else
+ sw &= ~CAN_SWITCH_COLOR;
+
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
+ sw &= ~CAN_SWITCH_STRATEGY;
+ else
+ sw |= CAN_SWITCH_STRATEGY;
+
+ if (sw & CAN_SWITCH_COLOR){
+ deb_led_off(LEDY);
+
+ send_rs_str("color\n");}
+ else
+ deb_led_on(LEDY);
+
+ if (sw & CAN_SWITCH_STRATEGY){
+ deb_led_off(LEDY);
+
+ send_rs_str("strategy\n");
+ }
+ else
+ deb_led_on(LEDY);
+
+// send_rs_int(IO1PIN);
+// send_rs_int(sw);
+// send_rs_str("\n");
+//
+ /* TODO: Put color to the second bit */
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+
+ //lift motor is motor A, MOTA connctor on board
+ init_periphery();
+
+ SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
+ SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0); //init of color pin
+ SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN - 16), PINSEL_0); //init of strategy pin
+ SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0); //init of home pin
+
+ IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN) | (1 << SWITCH_STRATEGY_PIN));
+ IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
+ IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
+ send_rs_str("Lift started\n");
+
+ fsm_lift.act_pos = 0;
+ init_fsm(&fsm_lift, &fsm_lift_init);
+
+/* return; */
+
+ while(1){
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+
+// fsm_lift.act_pos = adc_val[0];
+
+ run_fsm(&fsm_lift);
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_lift.trigger_can_send) { //or when something important happen
+ fsm_lift.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ can_send_status();
+ }
+
+ fsm_lift.act_pos = irc_read_tick();
+
+ start_button();
+ robot_switches_handler();
+ lift_switches_handler();
+ blink_led();
+ }
+}
+
+/** @} */
--- /dev/null
+#include <uart.h>
+
+
+
+
+/**
+ * Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+ uart0SendCh(val);
+}
+
+/**
+ * Read one char from uart.
+ */
+char uart_get_char(void)
+{
+ return uart0GetCh();
+}
+
+/**
+ * Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+ init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ * Send string to serial output in ASCII code. .
+ * @param data[] string to print
+ */
+void send_rs_str(const char data[])
+{
+
+ int i = 0;
+ int j = 0;
+
+ for (j = 0; j < 255; j++)
+ {
+ if(data[j] == 0) break;
+ }
+
+ for (i= 0 ; i < j; i++)
+ {
+ uart_send_char(data[i]);
+ }
+}
+
+/**
+ * Send int value to serial output in ASCII code. Removes unused zeros.
+ * @param val value to print
+ */
+void send_rs_int(int val)
+{
+ char dat[8];
+ int i;
+ int pom = 0;
+
+ for(i = 0; i < 8; i++)
+ {
+ dat[i] = (val & 0xF) + 0x30;
+ if(dat[i] > '9')
+ dat[i] += 7;
+ val >>= 4;
+ }
+
+ for(i = 0; i < 8; i++)
+ {
+ if((pom == 0) & (dat[7-i] == '0'))
+ {
+ if((i == 6) | (i == 7))
+ uart_send_char('0');
+ continue;
+ }
+ pom = 1;
+ uart_send_char(dat[7-i]);
+ }
+
+}
--- /dev/null
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+
eb_pwr_LIBS = can
-#link_VARIANTS = flash
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+#define CAN_SPEED 1000000
+
+#define CAN_ISR 0
+#define ADC_ISR 1
+#define TIME_ISR 2
+
+#define TIMER_IRQ_PRIORITY 5
+
+#define CAN_REPORT_TIME 500
+#define CAN_ALERT_TIME 200
+#define CAN_TIMEOUT_TIME 20
+
+#define BAT_CNT 10
+#define BAT_STAT_MEAN 120000
+#define BAT_STAT_LOW 110000
+#define BAT_STAT_CRITICAL 105000
+
+#define V33_MIN 30000
+#define V33_MAX 40000
+#define V50_MIN 45000
+#define V50_MAX 55000
+#define V80_MIN 75000
+#define V80_MAX 100000
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10us @100kHz */
+
+#endif
\ No newline at end of file
//
// Description
// -----------
-// This software controls the eurobot powerboard
+// This software controls the eurobot powerboard
// Author : Jiri Kubias DCE CVUT
//
//
#include <system_def.h>
#include <periph/can.h>
#include <can_ids.h>
-
+#include <can_msg_def.h>
#include "pwrstep.h"
#include "uart.h"
+#include "def.h"
-#define CAN_SPEED 1000000
-
-#define CAN_ISR 0
-#define ADC_ISR 1
-#define TIME_ISR 2
-
-
-#define TIME1MS ((CPU_APB_HZ) / 1000)
-
-
-#define CAN_TRY 20
-
-enum {
- ALERT_LOW = 1,
- ALERT_MAIN,
- ALERT_BYE,
- ALERT_33V,
- ALERT_50V,
- ALERT_80V
-} alert_states;
-
-
-
-#define TIME_COUNT 4
-#define LEDG_TIME 500
-#define CAN_REPORT_TIME 500
-#define CAN_ALERT_TIME 200
-#define CAN_TIMEOUT_TIME 50
-enum {
- TIM_CAN_REPORT = 0,
- TIM_CAN_ALERT,
- TIM_LEDG,
- TIM_CAN_TIMEOUT
-}time;
-
-
-
-#define BAT_CNT 10
-#define BAT_STAT_LOW 120000
-#define BAT_STAT_MAIN 110000
-#define BAT_STAT_BYE 105000
+can_msg_t msg;
-#define V33_MIN 30000
-#define V50_MIN 45000
-#define V80_MIN 75000
+struct power power_state;
-#define CAN_TIMEOUT 10
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)())
+{
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+void timer0_irq() __attribute__((interrupt));
+void timer0_irq()
+{
+ static unsigned cnt1khz = 0;
+ /* reset timer irq */
+ T0IR = -1;
-//#define TEST
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
-can_msg_t msg;
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
-volatile uint32_t dev_time[TIME_COUNT];
+void init_timer0(uint32_t prescale, uint32_t period)
+{
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
-unsigned int cnt_12V;
-unsigned int cnt_10V;
+void can_rx(can_msg_t *msg) {
+ can_msg_t rx_msg;
+
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));
+
+ if (rx_msg.id == CAN_PWR) {
+ /* set 3.3V power line */
+ if ((rx_msg.data[0] & (1<<0)) && !(rx_msg.data[0] & (1<<3))) {
+ pwr_33(PWR_ON, &power_state);
+ power_state.can_33v_req = PWR_ON;
+ } else if (rx_msg.data[0] & (1<<3)) {
+ pwr_33(PWR_OFF, &power_state);
+ power_state.can_33v_req = PWR_OFF;
+ }
+ /* set 5.0V power line */
+ if ((rx_msg.data[0] & (1<<1)) && !(rx_msg.data[0] & (1<<4))) {
+ pwr_50(PWR_ON, &power_state);
+ power_state.can_50v_req = PWR_ON;
+ } else if (rx_msg.data[0] & (1<<4)) {
+ pwr_50(PWR_OFF, &power_state);
+ power_state.can_50v_req = PWR_OFF;
+ }
+ /* set 8.0V power line */
+ if ((rx_msg.data[0] & (1<<2)) && !(rx_msg.data[0] & (1<<5))) {
+ pwr_80(PWR_ON, &power_state);
+ power_state.can_80v_req = PWR_ON;
+ } else if(rx_msg.data[0] & (1<<5)) {
+ pwr_80(PWR_OFF, &power_state);
+ power_state.can_80v_req = PWR_OFF;
+ }
+ }
+}
+void init_perip(struct power *pwr) // inicializace periferii mikroprocesoru
+{
+ can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
+ init_uart0((int)9600 ,UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+ init_adc(ADC_ISR);
+ init_pwr(pwr);
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ pwr_80(PWR_ON, pwr);
+ pwr_50(PWR_ON, pwr);
+}
-void led_blik()
+void blink_led()
{
-
- if (dev_time[TIM_LEDG] == 0)
- {
- deb_led_change(LEDG);
- dev_time[TIM_LEDG] = LEDG_TIME;
- }
+ static uint32_t led_time = 0;
+ if(timer_msec >= led_time + 500) {
+ led_time = timer_msec;
+ deb_led_change(LEDG);
+ }
}
-void send_alert(unsigned char type )
+void send_alert(unsigned char alert)
{
-
-
msg.id = CAN_PWR_ALERT;
msg.flags = 0;
msg.dlc = 1;
- msg.data[0] = type;
-
- dev_time[TIM_CAN_TIMEOUT] = CAN_TIMEOUT_TIME;
- while(can_tx_msg(&msg) && (dev_time[TIM_CAN_TIMEOUT] != 0));
-
+ msg.data[0] = alert;
+
+ can_tx_msg(&msg);
}
-void power_alert()
+void power_lines_check(struct power *pwr)
{
- if (dev_time[TIM_CAN_ALERT] == 0)
- {
-
- if (adc_val[0] < BAT_STAT_BYE) // bat < 9,5V
- {
- deb_led_on(LEDR);
- send_alert(ALERT_BYE);
- pwr_50(PWR_OFF);
- //pwr_80(PWR_OFF);
- pwr_33(PWR_OFF);
-
-
- }
- else if (adc_val[0] < BAT_STAT_MAIN) // bat < 12V
- {
- deb_led_on(LEDB);
- ++cnt_10V;
- if (cnt_10V > BAT_CNT)
- {
- send_alert(ALERT_MAIN);
- pwr_50(PWR_OFF);
- //pwr_80(PWR_OFF);
- }
-
-
- }
- else if (adc_val[0] < BAT_STAT_LOW) // bat < 12V
- {
- deb_led_on(LEDY);
- ++cnt_12V;
- if (cnt_12V > BAT_CNT)
- send_alert(ALERT_LOW);
- }
- else
- deb_led_off(LEDY);
-
- if (cnt_10V < BAT_CNT)
- {
- if (adc_val[3] < V80_MIN)
- {
- send_alert(ALERT_80V);
- }
-
- if (adc_val[2] < V50_MIN)
- {
- send_alert(ALERT_50V);
- }
-
- if (adc_val[1] < V33_MIN)
- {
- send_alert(ALERT_33V);
- }
- }
- dev_time[TIM_CAN_ALERT] = CAN_ALERT_TIME;
+ static uint32_t power_time = 0;
+ static unsigned char cnt_33V = 0, cnt_50V = 0, cnt_80V = 0;
+
+ //TODO dodelat kontrolu napajecich vetvi tak aby se merilo cca 10x po sobe a pak vyhodnotila situace
+ if (timer_msec >= power_time + CAN_ALERT_TIME) {
+ if ((V80_CH < V80_MIN || V80_CH > V80_MAX) && (pwr->pwr_80v_state == PWR_ON)) {
+ if (++cnt_80V > 20) {
+ pwr->alert |= CAN_PWR_ALERT_80;
+ send_alert(pwr->alert);
+ //pwr_80(PWR_OFF, pwr);
+ }
+ }
+
+ if ((V50_CH < V50_MIN || V50_CH > V50_MAX) && (pwr->pwr_50v_state == PWR_ON)) {
+ if (++cnt_50V > 20) {
+ pwr->alert |= CAN_PWR_ALERT_50;
+ send_alert(pwr->alert);
+ pwr_50(PWR_OFF, pwr);
+ }
+ }
+
+ if ((V33_CH < V33_MIN || V33_CH > V33_MAX) && (pwr->pwr_33v_state == PWR_ON)) {
+ if (++cnt_33V > 10) {
+ pwr->alert |= CAN_PWR_ALERT_33;
+ send_alert(pwr->alert);
+ pwr_33(PWR_OFF, pwr);
+ }
+ }
+
+ power_time = timer_msec;
}
}
-void send_can()
+void battery_check(struct power *pwr)
+{
+ static uint32_t battery_time = 0;
+ static unsigned char cnt_10V = 0, cnt_11V = 0, cnt_12V = 0;
+
+ //TODO upravit mereni baterie, po zapnuti napajeni cekat cca 5s a merit stav, pokud OK, zapnout dasli vetve ap.
+ if (timer_msec >= battery_time + 200) {
+ /* check battery empty state - Ubat < 10.5 V */
+ /* red LED signalization */
+ if (BATTERY_CH < BAT_STAT_CRITICAL) {
+ if (++cnt_10V > 20) {
+ deb_led_on(LEDR);
+ pwr->alert |= CAN_PWR_BATT_CRITICAL;
+ send_alert(pwr->alert);
+ pwr_50(PWR_OFF, pwr);
+ pwr_33(PWR_OFF, pwr);
+ /// do not switch off 8V pwr line - this will cause power-down of this board!!
+ }
+ }
+ /* check battery low state - Ubat < 11V */
+ /* blue LED signalization*/
+ else if (BATTERY_CH < BAT_STAT_LOW) {
+ if (++cnt_11V > BAT_CNT) {
+ deb_led_on(LEDY);
+ pwr->alert |= CAN_PWR_BATT_LOW;
+ send_alert(pwr->alert);
+ }
+ }
+ /* check battery alert state - Ubat < 12V */
+ /* yellow LED signalization*/
+ else if (BATTERY_CH < BAT_STAT_MEAN) {
+ if (++cnt_12V > BAT_CNT) {
+ deb_led_on(LEDY);
+ pwr->alert |= CAN_PWR_BATT_MEAN;
+ send_alert(pwr->alert);
+ }
+ } else {
+ /* batery OK */
+ }
+ battery_time = timer_msec;
+ }
+}
+
+void send_voltage()
{
- if (dev_time[TIM_CAN_REPORT] == 0)
- {
+ static uint32_t send_time = 0;
+ static uint32_t can_timeout = 0;
+
+ if (timer_msec >= send_time + CAN_REPORT_TIME) {
deb_led_on(LEDB);
msg.id = CAN_PWR_ADC1;
msg.data[5] = (((adc_val[1]) >> 16) & 0xFF);
msg.data[6] = (((adc_val[1]) >> 8) & 0xFF);
msg.data[7] = (((adc_val[1]) >> 0) & 0xFF);
-
-
- dev_time[TIM_CAN_TIMEOUT] = CAN_TIMEOUT_TIME;
- while(can_tx_msg(&msg) && (dev_time[TIM_CAN_TIMEOUT] != 0));
+
+
+ can_timeout = timer_msec;
+ while(can_tx_msg(&msg) && (timer_msec <= can_timeout + CAN_TIMEOUT_TIME));
msg.id = CAN_PWR_ADC2;
msg.flags = 0;
msg.data[5] = (((adc_val[3]) >> 16) & 0xFF);
msg.data[6] = (((adc_val[3]) >> 8) & 0xFF);
msg.data[7] = (((adc_val[3]) >> 0) & 0xFF);
-
- dev_time[TIM_CAN_TIMEOUT] = CAN_TIMEOUT_TIME;
- while(can_tx_msg(&msg) && (dev_time[TIM_CAN_TIMEOUT] != 0));
-
- deb_led_off(LEDB);
- dev_time[TIM_CAN_REPORT] = CAN_REPORT_TIME;
- }
-}
-
-void can_rx(can_msg_t *msg) {
- can_msg_t rx_msg;
-
- memcpy(&rx_msg, msg, sizeof(can_msg_t));
-
-
- if (rx_msg.id == CAN_PWR)
- {
- if(rx_msg.data[0] & (1<<0)) pwr_33(PWR_ON);
- if(rx_msg.data[0] & (1<<1)) pwr_50(PWR_ON);
- if(rx_msg.data[0] & (1<<2)) pwr_80(PWR_ON);
-
- if(rx_msg.data[0] & (1<<3)) pwr_33(PWR_OFF);
- if(rx_msg.data[0] & (1<<4)) pwr_50(PWR_OFF);
- if(rx_msg.data[0] & (1<<5)) pwr_80(PWR_OFF);
- }
-}
-
+ can_timeout = timer_msec;
+ while(can_tx_msg(&msg) && (timer_msec <= can_timeout + CAN_TIMEOUT_TIME));
-
-
-void tc1 (void) __attribute__ ((interrupt));
-void tc1 (void) {
-
- uint32_t i;
-
- for (i = 0; i < TIME_COUNT; i++)
- {
- if(dev_time[i] != 0)
- --dev_time[i];
+ deb_led_off(LEDB);
+ send_time = timer_msec;
}
-
- T1IR = 4; // Vynulovani priznaku preruseni
- VICVectAddr = 0; // Potvrzeni o obsluze preruseni
}
-
-/* Setup the Timer Counter 1 Interrupt */
-void init_time (unsigned rx_isr_vect)
+int main (void)
{
+ init_perip(&power_state); // sys init MCU
- T1PR = 0;
- T1MR2 = TIME1MS;
- T1MCR = (3<<6); // interrupt on MR1
- T1TCR = 1; // Starts Timer 1
-
- ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned long)tc1; // Nastaveni adresy vektotu preruseni
- ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x20 | 0x5; // vyber casovece pro preruseni
- VICIntEnable = (1<<5); // Povoli obsluhu preruseni
-}
-
-
-
-void init_perip(void) // inicializace periferii mikroprocesoru
-{
- int i;
- for (i = 0; i < TIME_COUNT; i++)
- dev_time[i] = 0;
-
- init_pwr();
- can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
- init_adc(ADC_ISR);
- init_time (TIME_ISR);
- init_uart0((int)9600 ,UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
-
-
-#ifdef TEST
- pwr_33(PWR_ON);
- pwr_50(PWR_ON);
- pwr_80(PWR_ON);
-#else
- pwr_33(PWR_OFF);
- pwr_50(PWR_OFF);
- pwr_80(PWR_OFF);
-#endif
-
- pwr_33(PWR_ON);
- pwr_50(PWR_ON);
- pwr_80(PWR_ON);
-}
-
-
-int main (void)
-{
- init_perip(); // sys init MCU
-
- dev_time[TIM_LEDG] = 1000;
- while(dev_time[TIM_LEDG]);
-
-
- while(1)
- {
- led_blik();
- send_can();
- power_alert();
-
- }
-}
-
-
+ //TODO wait one secdond here
+ while (1) {
+ blink_led();
+ send_voltage();
+ battery_check(&power_state);
+ power_lines_check(&power_state);
+ }
+}
\ No newline at end of file
#include <lpc21xx.h> // LPC21XX Peripheral Registers\r
-#include <types.h> \r
+#include <types.h>\r
#include <deb_led.h>\r
#include <system_def.h>\r
#include "pwrstep.h"\r
\r
-\r
-#define PWR33 (1<<22) \r
-#define PWR50 (1<<24)\r
-#define PWR80 (1<<23)\r
-\r
-#define ADC0 (1<<27) \r
-#define ADC1 (1<<28) \r
-#define ADC2 (1<<29) \r
-#define ADC3 (1<<30) \r
-\r
-\r
-\r
-#define ADCCH0 22\r
-#define ADCCH1 24\r
-#define ADCCH2 26\r
-#define ADCCH3 28\r
-\r
-// tohla me byt definovano nekde jinde FIXME\r
-\r
-\r
-#define ADC_PIN_0 0x1\r
-#define ADC_PIN_1 0x2\r
-#define ADC_PIN_2 0x4\r
-#define ADC_PIN_3 0x8\r
-\r
-#define ADC_CR_ADC0 0x1\r
-#define ADC_CR_ADC1 0x2\r
-#define ADC_CR_ADC2 0x4\r
-#define ADC_CR_ADC3 0x8\r
-\r
-#define ADC_CR_CLK_DIV_1 (1<<8) // this nuber should be multipied sampe\r
- // requested divisor 4 ---- clk_div = 4 * ADC_CR_CLK_DIV_1\r
-#define ADC_CR_BURST (1<<16)\r
-#define ADC_CR_CLKS_11 (0<<17)\r
-#define ADC_CR_CLKS_10 (1<<17)\r
-#define ADC_CR_CLKS_9 (2<<17)\r
-#define ADC_CR_CLKS_8 (3<<17)\r
-#define ADC_CR_CLKS_7 (4<<17)\r
-#define ADC_CR_CLKS_6 (5<<17)\r
-#define ADC_CR_CLKS_5 (6<<17)\r
-#define ADC_CR_CLKS_4 (7<<17)\r
-\r
-#define ADC_CR_PDN_ON (1<<21)\r
-\r
-#define ADC_CR_START_OFF (0<<24)\r
-#define ADC_CR_START_NOW (1<<24)\r
-#define ADC_CR_START_P016 (2<<24)\r
-#define ADC_CR_START_P022 (3<<24)\r
-#define ADC_CR_START_MAT01 (4<<24)\r
-#define ADC_CR_START_MAT03 (5<<24)\r
-#define ADC_CR_START_MAT10 (6<<24)\r
-#define ADC_CR_START_MAT11 (7<<24)\r
-\r
-#define ADC_CR_EDGE_RISING (0<<27)\r
-#define ADC_CR_EDGE_FALLING (1<<27)\r
-\r
-\r
-\r
-\r
-\r
- \r
-\r
-void pwr_33(char mode) // switch on/off 3,3V power line\r
+void pwr_33(char mode, struct power *pwr) // switch on/off 3,3V power line\r
{\r
- if (mode != PWR_ON)\r
- {\r
- IO1SET |= PWR33; \r
- }\r
- else\r
- {\r
- IO1CLR |= PWR33; \r
+ if (mode != PWR_ON) {\r
+ IO1SET |= PWR33;\r
+ pwr->pwr_33v_state = PWR_OFF;\r
+ } else {\r
+ IO1CLR |= PWR33;\r
+ pwr->pwr_33v_state = PWR_ON;\r
}\r
}\r
\r
-\r
-void pwr_50(char mode) // switch on/off 5V power line\r
+void pwr_50(char mode, struct power *pwr) // switch on/off 5V power line\r
{\r
- if (mode != PWR_ON)\r
- {\r
- IO1SET |= PWR50; \r
- }\r
- else\r
- {\r
- IO1CLR |= PWR50; \r
+ if (mode != PWR_ON) {\r
+ IO1SET |= PWR50;\r
+ pwr->pwr_50v_state = PWR_OFF;\r
+ } else {\r
+ IO1CLR |= PWR50;\r
+ pwr->pwr_50v_state = PWR_ON;\r
}\r
}\r
\r
-\r
-void pwr_80(char mode) // switch on/off 8V power line\r
+void pwr_80(char mode, struct power *pwr) // switch on/off 8V power line\r
{\r
- if (mode != PWR_ON)\r
- {\r
- IO1SET |= PWR80; \r
- }\r
- else\r
- {\r
- IO1CLR |= PWR80; \r
+ if (mode != PWR_ON) {\r
+ IO1SET |= PWR80;\r
+ pwr->pwr_80v_state = PWR_OFF;\r
+ } else {\r
+ IO1CLR |= PWR80;\r
+ pwr->pwr_80v_state = PWR_ON;\r
}\r
}\r
\r
-\r
- volatile char test =0xF;\r
-\r
void adc_isr(void) __attribute__ ((interrupt));\r
\r
-void adc_isr(void) \r
+void adc_isr(void)\r
{\r
- unsigned char chan =0; \r
+ unsigned char chan =0;\r
unsigned int val =0;\r
\r
chan = (char) ((ADDR>>24) & 0x07);\r
- val = ((ADDR >> 6) & 0x3FF); \r
+ val = ((ADDR >> 6) & 0x3FF);\r
\r
adc_val[chan] = (((val * ADC_CON_CONST + ADC_OFFSET) + adc_val[chan]) >> 1) ;\r
\r
case 1:\r
ADCR = ((ADC_CR_ADC2) | (ADC_CR_CLKS_11) | (ADC_CR_PDN_ON) | (ADC_CR_START_NOW) | (20*ADC_CR_CLK_DIV_1));\r
break;\r
- \r
+\r
case 2:\r
ADCR = ((ADC_CR_ADC3) | (ADC_CR_CLKS_11) | (ADC_CR_PDN_ON) | (ADC_CR_START_NOW) | (20*ADC_CR_CLK_DIV_1));\r
break;\r
- \r
+\r
case 3:\r
ADCR = ((ADC_CR_ADC0) | (ADC_CR_CLKS_11) | (ADC_CR_PDN_ON) | (ADC_CR_START_NOW) | (20*ADC_CR_CLK_DIV_1));\r
- break; \r
+ break;\r
}\r
- \r
+\r
VICVectAddr = 0;\r
}\r
\r
-\r
void init_adc(unsigned rx_isr_vect)\r
{\r
- \r
- PINSEL1 |= ((PINSEL_1 << ADCCH0) | (PINSEL_1 << ADCCH1) | (PINSEL_1 << ADCCH2) | (PINSEL_1 << ADCCH3)); \r
+ PINSEL1 |= ((PINSEL_1 << ADCCH0) | (PINSEL_1 << ADCCH1) | (PINSEL_1 << ADCCH2) | (PINSEL_1 << ADCCH3));\r
\r
((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x32;\r
((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned) adc_isr;\r
ADCR = ((ADC_CR_ADC0) | (ADC_CR_CLKS_11) | (ADC_CR_PDN_ON) | (ADC_CR_START_NOW) | (10*ADC_CR_CLK_DIV_1));\r
}\r
\r
-\r
-void init_pwr(void) // init power lines\r
+void init_pwr(struct power *pwr) // init power lines\r
{\r
IO1DIR |= (PWR33 | PWR50 | PWR80);\r
- pwr_33(PWR_OFF);\r
- pwr_50(PWR_OFF);\r
- pwr_80(PWR_OFF);\r
-}\r
-\r
-\r
-\r
-\r
-\r
-\r
-\r
+ pwr_33(PWR_OFF, pwr);\r
+ pwr_50(PWR_OFF, pwr);\r
+ pwr_80(PWR_OFF, pwr);\r
+}
\ No newline at end of file
#define PWR_ON 1\r
#define PWR_OFF 0\r
\r
-#define ADC_DIV 10\r
-#define ADC_CON_CONST 322\r
-#define ADC_OFFSET 2000\r
+#define ADC_DIV 10\r
+#define ADC_CON_CONST 322\r
+#define ADC_OFFSET 2000\r
\r
+#define PWR33 (1<<22)\r
+#define PWR50 (1<<24)\r
+#define PWR80 (1<<23)\r
\r
- volatile unsigned int adc_val[4];\r
+#define ADC0 (1<<27)\r
+#define ADC1 (1<<28)\r
+#define ADC2 (1<<29)\r
+#define ADC3 (1<<30)\r
+\r
+#define ADCCH0 22\r
+#define ADCCH1 24\r
+#define ADCCH2 26\r
+#define ADCCH3 28\r
+\r
+#define ADC_PIN_0 0x1\r
+#define ADC_PIN_1 0x2\r
+#define ADC_PIN_2 0x4\r
+#define ADC_PIN_3 0x8\r
+\r
+#define ADC_CR_ADC0 0x1\r
+#define ADC_CR_ADC1 0x2\r
+#define ADC_CR_ADC2 0x4\r
+#define ADC_CR_ADC3 0x8\r
+\r
+#define ADC_CR_CLK_DIV_1 (1<<8) // this nuber should be multipied sampe\r
+ // requested divisor 4 ---- clk_div = 4 * ADC_CR_CLK_DIV_1\r
+#define ADC_CR_BURST (1<<16)\r
+#define ADC_CR_CLKS_11 (0<<17)\r
+#define ADC_CR_CLKS_10 (1<<17)\r
+#define ADC_CR_CLKS_9 (2<<17)\r
+#define ADC_CR_CLKS_8 (3<<17)\r
+#define ADC_CR_CLKS_7 (4<<17)\r
+#define ADC_CR_CLKS_6 (5<<17)\r
+#define ADC_CR_CLKS_5 (6<<17)\r
+#define ADC_CR_CLKS_4 (7<<17)\r
+\r
+#define ADC_CR_PDN_ON (1<<21)\r
+\r
+#define ADC_CR_START_OFF (0<<24)\r
+#define ADC_CR_START_NOW (1<<24)\r
+#define ADC_CR_START_P016 (2<<24)\r
+#define ADC_CR_START_P022 (3<<24)\r
+#define ADC_CR_START_MAT01 (4<<24)\r
+#define ADC_CR_START_MAT03 (5<<24)\r
+#define ADC_CR_START_MAT10 (6<<24)\r
+#define ADC_CR_START_MAT11 (7<<24)\r
+\r
+#define ADC_CR_EDGE_RISING (0<<27)\r
+#define ADC_CR_EDGE_FALLING (1<<27)\r
+\r
+#define BATTERY_CH adc_val[0]\r
+#define V33_CH adc_val[1]\r
+#define V50_CH adc_val[2]\r
+#define V80_CH adc_val[3]\r
+\r
+volatile unsigned int adc_val[4];\r
+\r
+struct power {\r
+ char can_33v_req;\r
+ char can_80v_req;\r
+ char can_50v_req;\r
+ char pwr_33v_state;\r
+ char pwr_50v_state;\r
+ char pwr_80v_state;\r
+ char battery_state;\r
+ char alert;\r
+};\r
\r
/** pwr_33 Switch on/off 3,3V power line\r
- * @param mode - PWR_ON/PWR_OFF \r
+ * @param mode - PWR_ON/PWR_OFF\r
*/\r
-void pwr_33(char mode);\r
+void pwr_33(char mode, struct power *pwr);\r
\r
/** pwr_50 Switch on/off 5,0V power line\r
- * @param mode - PWR_ON/PWR_OFF \r
+ * @param mode - PWR_ON/PWR_OFF\r
*/\r
-void pwr_50(char mode);\r
+void pwr_50(char mode, struct power *pwr);\r
\r
/** pwr_80 Switch on/off 8,0V power line\r
- * @param mode - PWR_ON/PWR_OFF \r
+ * @param mode - PWR_ON/PWR_OFF\r
*/\r
-void pwr_80(char mode);\r
+void pwr_80(char mode, struct power *pwr);\r
\r
/** init_pwr inicializes power lines - default: all lines is off\r
*/\r
-void init_pwr(void);\r
-\r
+void init_pwr(struct power *pwr);\r
\r
/** inicializes ADC lines and starts converion (use ISR)\r
* @param rx_isr_vect ISR vector\r
# -*- makefile -*-
-bin_PROGRAMS = eb_sberac_09
+#bin_PROGRAMS = eb_sberac_09
eb_sberac_09_SOURCES = main.c
eb_sberac_09_LIBS = can ebb uart_nozen
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+bin_PROGRAMS = eb_test
+
+eb_test_SOURCES = main.c uar.c fsm_vidle.c fsm.c
+eb_test_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+#endif
--- /dev/null
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+ fsm->current_state = initial_state;
+ fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+ fsm->last_state = fsm->current_state; // set actual state
+ fsm->current_state(fsm, EVENT_DO); // change parameter
+
+ if(fsm->last_state != fsm->current_state){ // if state was changed
+ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
+ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+ EVENT_ENTRY,
+ EVENT_DO,
+ EVENT_EXIT
+};
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+
+struct fsm {
+ state_fcn current_state; // current state
+ state_fcn last_state; // last state
+ int32_t act_pos; // actual position
+ int32_t last_pos; // last position
+ int32_t delta; //indicate change betwen each move(should be used as absolute value)
+ int32_t button1;
+ int32_t button2;
+
+
+ bool trigger_can_send;
+};
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+#include <expansion.h>
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+
+
+
+
+
+// definice stavu automatu
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+
+// stavi init dela se jen po zapnuti
+void fsm_vidle_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ break;
+ case EVENT_DO:
+ fsm->current_state =wait_for_cmd;
+
+
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+// stav cekam na povel
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ break;
+ case EVENT_DO:
+ if (fsm->button1){
+ if (fsm->act_pos<300)
+ {
+ engine_A_pwm(0);
+ fsm->current_state = wait_for_cmd;
+
+
+ }
+ else
+ {
+
+ fsm->current_state = move;
+
+
+ }
+ }
+ else
+ {
+ if (fsm->act_pos>900)
+ {
+ engine_A_pwm(0);
+ fsm->current_state = wait_for_cmd;
+
+
+ }
+ else
+ {
+
+ fsm->current_state = move;
+
+
+ }
+ }
+
+
+
+
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define treshold // treshold of reaction on delta
+static void move(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ break;
+ case EVENT_DO:
+ if (fsm->button1)
+ { engine_A_dir(ENGINE_DIR_BW);
+ if (fsm->act_pos>300)
+ {
+ engine_A_pwm(20);
+
+
+ }
+ else
+ {
+ fsm->current_state = wait_for_cmd;
+ }
+
+ }
+ else
+ { engine_A_dir(ENGINE_DIR_FW);
+ if (fsm->act_pos<900)
+ {
+ engine_A_pwm(20);
+
+
+
+ }
+ else
+ {
+ fsm->current_state = wait_for_cmd;
+ }
+
+ }
+
+
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include <engine.h>
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+#include <expansion.h>
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+
+struct fsm fsm_vidle;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~l~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+
+
+
+void init_motors(void){
+
+ init_engine_A(); // initialization of PWM unit
+ engine_A_en(ENGINE_EN_ON); //enable motor A
+ engine_A_dir(ENGINE_DIR_FW); //set direction
+ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
+/* vhn_init(); */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#define START_PIN 15 // start pin
+#define BUTTON_1_PIN EXPPORT_1
+#define BUTTON_2_PIN EXPPORT_3
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
+#if 0
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+#endif
+#if 0
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ case CAN_VIDLE_CMD:
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+
+ if (req >= 0x150 && req <= 0x3e0) {
+ fsm_vidle.flags &= ~CAN_VIDLE_OUT_OF_BOUNDS;
+ fsm_vidle.can_req_position = req;// save new req position of lift
+ fsm_vidle.can_req_spd = spd;// save new req spd of lift
+ } else
+ fsm_vidle.flags |= CAN_VIDLE_OUT_OF_BOUNDS;
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+#endif
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+// can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+ init_adc(ADC_ISR);
+
+
+}
+/*********************************************************/
+#if 0
+void can_send_status(void)
+{
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_VIDLE_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_vidle.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_vidle.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_vidle.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_vidle.can_response & 0xFF;
+ tx_msg.data[4] = fsm_vidle.flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+}
+#endif
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_vidle_init(struct fsm *fsm, enum event event);
+
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+ /* static int up;
+ if (up == 0)
+ fsm_vidle.can_req_position = 0x380;
+ if (up == 6)
+ fsm_vidle.can_req_position = 0x1e0;
+ up = (up+1)%12;
+ */
+ deb_led_change(LEDG);
+ send_rs_int(fsm_vidle.act_pos);
+ send_rs_str("\n");
+ }
+}
+
+
+#define BUMPER_PIN 17 // bumper pin (SCK1/P0_17)
+#define COLOR_PIN 3 // change color of dress pin (SDA1/P0_3)
+
+#define BUMPER_LEFT 19 // left bumper MOSI1/P0_19
+#define BUMPER_RIGHT 9 // right bumper RXD1/P0_9
+
+#if 0
+void handle_bumper()
+{
+ static uint32_t bumper_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= bumper_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ bumper_time = timer_msec;
+
+
+
+ if (IO0PIN & (1<<BUMPER_PIN))
+ sw &= ~CAN_SWITCH_BUMPER;
+ else
+ sw |= CAN_SWITCH_BUMPER;
+
+ if (IO0PIN & (1<<COLOR_PIN))
+ sw |= CAN_SWITCH_COLOR;
+ else
+ sw &= ~CAN_SWITCH_COLOR;
+
+ if (IO0PIN & (1<<BUMPER_LEFT))
+ sw &= ~CAN_SWITCH_LEFT;
+ else
+ sw |= CAN_SWITCH_LEFT;
+
+ if (IO0PIN & (1<<BUMPER_RIGHT))
+ sw &= ~CAN_SWITCH_RIGHT;
+ else
+ sw |= CAN_SWITCH_RIGHT;
+
+ if (sw & CAN_SWITCH_COLOR)
+ deb_led_off(LEDY);
+ else
+ deb_led_on(LEDY);
+
+ if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT))
+ deb_led_on(LEDR);
+ else
+ deb_led_off(LEDR);
+
+ /* TODO: Put color to the second bit */
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+#endif
+
+void dummy_wait()
+{
+ volatile unsigned int wait = 5000000;
+ while(--wait);
+}
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+ can_msg_t tx_msg;
+ bool obrat=true;
+ bool button_status[3];
+ int i=0,delta=0,j=0,suma=0;
+
+
+ init_periphery();
+
+ /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09 */
+ //SET_PIN(PINSEL1, 1, PINSEL_0);
+ //SET_PIN(PINSEL1, 3, PINSEL_0);
+
+
+
+
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0); // inicializace start pinu
+ SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, 1, PINSEL_0); // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
+ SET_PIN(PINSEL0,BUTTON_1_PIN,PINSEL_0);
+ SET_PIN(PINSEL0,BUTTON_2_PIN,PINSEL_0);
+ SET_PIN(PINSEL1, 3, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
+
+
+ IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
+
+ IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT)); //first shift one on the right pin
+ IO0DIR &= ~((1<<BUTTON_1_PIN) | (1<<BUTTON_2_PIN)); //then invert, cause 0 is desired input
+
+
+
+
+
+ send_rs_str("Vidle started\n");
+
+ // The above send_rs_str is importat - we wait for the first AD conversion to be finished
+ fsm_vidle.act_pos = adc_val[0];
+ button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0; //reading value (inspiration from START_PIN)
+ button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0;
+ fsm_vidle.button1=button_status[1];
+ fsm_vidle.button2=button_status[2];
+ init_fsm(&fsm_vidle, &fsm_vidle_init);
+ engine_A_dir(ENGINE_DIR_FW);
+/* test_vhn(); */
+/* return; */
+
+
+ while(1){
+
+
+
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+ fsm_vidle.last_pos = fsm_vidle.act_pos;
+ fsm_vidle.act_pos = adc_val[0];
+ fsm_vidle.delta=fsm_vidle.act_pos-fsm_vidle.last_pos;
+ button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0; //reading value (inspiration from START_PIN)
+ button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0;
+ // if (button_status[1]) { send_rs_str("pozadavek na uzavreni zavory"); send_rs_str("\n");}
+ // else { send_rs_str("pozadavek na otevreni zavory"); send_rs_str("\n");}
+ // if (button_status[2]) { send_rs_str("pozadavek na ignoraci prekazek"); send_rs_str("\n");}
+ // else { send_rs_str("pozadavek na uhybani prekazkam"); send_rs_str("\n");}
+ fsm_vidle.button1=button_status[1];
+ fsm_vidle.button2=button_status[2];
+ run_fsm(&fsm_vidle);
+
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_vidle.trigger_can_send) { //or when something important happen
+ fsm_vidle.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ // can_send_status();
+
+ }
+
+ //start_button();
+ //handle_bumper();
+
+
+
+
+
+
+
+
+
+
+
+ }
+}
+
+/** @} */
--- /dev/null
+#include <uart.h>
+
+
+
+
+/**
+ * Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+ uart0SendCh(val);
+}
+
+/**
+ * Read one char from uart.
+ */
+char uart_get_char(void)
+{
+ return uart0GetCh();
+}
+
+/**
+ * Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+ init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ * Send string to serial output in ASCII code. .
+ * @param data[] string to print
+ */
+void send_rs_str(const char data[])
+{
+
+ int i = 0;
+ int j = 0;
+
+ for (j = 0; j < 255; j++)
+ {
+ if(data[j] == 0) break;
+ }
+
+ for (i= 0 ; i < j; i++)
+ {
+ uart_send_char(data[i]);
+ }
+}
+
+/**
+ * Send int value to serial output in ASCII code. Removes unused zeros.
+ * @param val value to print
+ */
+void send_rs_int(int val)
+{
+ char dat[8];
+ int i;
+ int pom = 0;
+
+ for(i = 0; i < 8; i++)
+ {
+ dat[i] = (val & 0xF) + 0x30;
+ if(dat[i] > '9')
+ dat[i] += 7;
+ val >>= 4;
+ }
+
+ for(i = 0; i < 8; i++)
+ {
+ if((pom == 0) & (dat[7-i] == '0'))
+ {
+ if((i == 6) | (i == 7))
+ uart_send_char('0');
+ continue;
+ }
+ pom = 1;
+ uart_send_char(dat[7-i]);
+ }
+
+}
--- /dev/null
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+
# -*- makefile -*-
-bin_PROGRAMS = eb_vidle
+#bin_PROGRAMS = eb_vidle
eb_vidle_SOURCES = main.c fsm.c fsm_vidle.c uar.c vhn.c
eb_vidle_LIBS = can ebb
* Sends an event to another FSM, which may run in another
* thread. Event sending is asynchronous, but each FSM has a buffer for
* several events. If this buffer is full, @a event_lost flag is set
- * and the event is forgotten. This should never happen in carefully
+ * and the event is lost. This should never happen in a carefully
* designed application.
*
* @note This macro ensures that it is not possible to send an event
# -*- makefile -*-
-bin_PROGRAMS = hokuyo
+bin_PROGRAMS = hokuyo hokuyo-dump
hokuyo_SOURCES = hokuyo.c
+hokuyo-dump_SOURCES = hokuyo-dump.c
include_HEADERS = hokuyo.h
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "hokuyo.h"
+#include <robottype.h>
+#include <roboorte_robottype.h>
+#include <unistd.h>
+#include <time.h>
+#include <semaphore.h>
+
+sem_t data_printed;
+struct robottype_orte_data orte;
+
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ int i;
+ for (i = 0; i < HOKUYO_ARRAY_SIZE; i++)
+ printf("%d\n", instance->data[i]);
+ sem_post(&data_printed);
+ break;
+ }
+ case DEADLINE:
+ fprintf(stderr, "%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
+
+
+
+int main(int argc, char **argv)
+{
+ int ret;
+ sem_init(&data_printed, 0, 0);
+
+ ret = robottype_roboorte_init(&orte);
+ if (ret < 0) {
+ fprintf(stderr, "robottype_roboorte_init failed\n");
+ return ret;
+ }
+ robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
+
+ sem_wait(&data_printed);
+
+ robottype_roboorte_destroy(&orte);
+
+ return 0;
+}
#include <unistd.h>
#include <time.h>
#include <signal.h>
+#include <error.h>
+#include <errno.h>
+
#define HOKUYO_DEVICE "/dev/ttyACM0" // /dev/ttyACM0
#define COMPILE_TIME_ASSERT(cond, msg) \
int ret;
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
- fprintf(stderr, "hokuyo: urg_connect(%s) failed!\n", device);
+ error(0, errno, "hokuyo: urg_connect(%s) failed: %s errno",
+ device, urg_error(&urg));
urg_disconnect(&urg);
return -1;
}
#define HOKUYO_FINAL_MEASUREMENT 725
#define HOKUYO_START_ANGLE (239.77/2)
-#define HOKUYO_INDEX_TO_DEG(x) (HOKUYO_START_ANGLE-(x)*360.0/HOKUYO_SPLIT_DIVISION)
+#define HOKUYO_INDEX_TO_DEG(x) ((HOKUYO_START_ANGLE-(x)*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION)
#define HOKUYO_INDEX_TO_RAD(x) (HOKUYO_INDEX_TO_DEG(x)/180.0*M_PI)
-#define HOKUYO_DEG_TO_INDEX(d) ((HOKUYO_START_ANGLE-(d))/(360.0/HOKUYO_SPLIT_DIVISION))
+#define HOKUYO_DEG_TO_INDEX(d) ((HOKUYO_START_ANGLE-(d)/HOKUYO_ORIENTATION)/(360.0/HOKUYO_SPLIT_DIVISION))
#endif //HOKUYO_H
/*!
\file
- \brief \83V\83\8a\83A\83\8b\92Ê\90M (Linux, Mac \8eÀ\91\95)
+ \brief �V���A���ʐM (Linux, Mac ���)
- Serial Communication Interface \90§\8cä
+ Serial Communication Interface ����
\author Satofumi KAMIMURA
#include <fcntl.h>
#include <errno.h>
#include <string.h>
+#include <sys/select.h>
#include <time.h>
#include <sys/types.h>
+
//#include <ctype.h>
enum {
}
-/* \90Ú\91± */
+/* �ڑ� */
int serial_connect(serial_t *serial, const char *device, long baudrate)
{
int flags = 0;
serial_initialize(serial);
#ifndef MAC_OS
- enum { O_EXLOCK = 0x0 }; /* Linux \82Å\82Í\8eg\82¦\82È\82¢\82Ì\82Å\83_\83~\81[\82ð\8dì\90¬\82µ\82Ä\82¨\82 */
+ enum { O_EXLOCK = 0x0 }; /* Linux �ł͎g���Ȃ��̂Ń_�~�[���쐬���Ă��� */
#endif
serial->fd_ = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
if (serial->fd_ < 0) {
- /* \90Ú\91±\82É\8e¸\94s */
+ /* �ڑ��Ɏ��s */
strerror_r(errno, serial->error_string_, SerialErrorStringSize);
return SerialConnectionFail;
}
flags = fcntl(serial->fd_, F_GETFL, 0);
fcntl(serial->fd_, F_SETFL, flags & ~O_NONBLOCK);
- /* \83V\83\8a\83A\83\8b\92Ê\90M\82Ì\8f\89\8aú\89» */
+ /* �V���A���ʐM�̏����� */
tcgetattr(serial->fd_, &serial->sio_);
serial->sio_.c_iflag = 0;
serial->sio_.c_oflag = 0;
serial->sio_.c_cc[VMIN] = 0;
serial->sio_.c_cc[VTIME] = 0;
- /* \83{\81[\83\8c\81[\83g\82̕ύX */
+ /* �{�[���[�g�̕ύX */
ret = serial_setBaudrate(serial, baudrate);
if (ret < 0) {
return ret;
}
- /* \83V\83\8a\83A\83\8b\90§\8cä\8d\\91¢\91Ì\82Ì\8f\89\8aú\89» */
+ /* �V���A�������\���̂̏����� */
serial->has_last_ch_ = False;
return 0;
}
-/* \90ؒf */
+/* �ؒf */
void serial_disconnect(serial_t *serial)
{
if (serial->fd_ >= 0) {
}
-/* \83{\81[\83\8c\81[\83g\82Ì\90Ý\92è */
+/* �{�[���[�g�̐ݒ� */
int serial_setBaudrate(serial_t *serial, long baudrate)
{
long baudrate_value = -1;
return SerialSetBaudrateFail;
}
- /* \83{\81[\83\8c\81[\83g\95ύX */
+ /* �{�[���[�g�ύX */
cfsetospeed(&serial->sio_, baudrate_value);
cfsetispeed(&serial->sio_, baudrate_value);
tcsetattr(serial->fd_, TCSADRAIN, &serial->sio_);
}
-/* \91\97\90M */
+/* ���M */
int serial_send(serial_t *serial, const char *data, int data_size)
{
if (! serial_isConnected(serial)) {
fd_set rfds;
struct timeval tv;
- // \83^\83C\83\80\83A\83E\83g\90Ý\92è
+ // �^�C���A�E�g�ݒ�
FD_ZERO(&rfds);
FD_SET(serial->fd_, &rfds);
if (select(serial->fd_ + 1, &rfds, NULL, NULL,
(timeout < 0) ? NULL : &tv) <= 0) {
- /* \83^\83C\83\80\83A\83E\83g\94\90¶ */
+ /* �^�C���A�E�g���� */
return 0;
}
return 1;
require_n = data_size_max - filled;
read_n = read(serial->fd_, &data[filled], require_n);
if (read_n <= 0) {
- /* \93Ç\82Ý\8fo\82µ\83G\83\89\81[\81B\8c»\8dÝ\82Ü\82Å\82Ì\8eó\90M\93à\97e\82Å\96ß\82é */
+ /* �ǂݏo���G���[�B���݂܂ł̎��M���e�Ŗ߂� */
break;
}
filled += read_n;
}
-/* \8eó\90M */
+/* ���M */
int serial_recv(serial_t *serial, char* data, int data_size_max, int timeout)
{
int filled;
return 0;
}
- /* \8f\91\82«\96ß\82µ\82½\82P\95¶\8e\9a\82ª\82 \82ê\82Î\81A\8f\91\82«\8fo\82· */
+ /* �����߂����P�����������A�����o�� */
filled = 0;
if (serial->has_last_ch_ != False) {
data[0] = serial->last_ch_;
buffer_size = ring_size(&serial->ring_);
read_n = data_size_max - filled;
if (buffer_size < read_n) {
- // \83\8a\83\93\83O\83o\83b\83t\83@\93à\82Ì\83f\81[\83^\82Å\91«\82è\82È\82¯\82ê\82Î\81A\83f\81[\83^\82ð\93Ç\82Ý\91«\82·
+ // �����O�o�b�t�@���̃f�[�^�ő����Ȃ����A�f�[�^���ǂݑ���
char buffer[RingBufferSize];
int n = internal_receive(buffer,
ring_capacity(&serial->ring_) - buffer_size,
}
buffer_size = ring_size(&serial->ring_);
- // \83\8a\83\93\83O\83o\83b\83t\83@\93à\82Ì\83f\81[\83^\82ð\95Ô\82·
+ // �����O�o�b�t�@���̃f�[�^���Ԃ�
if (read_n > buffer_size) {
read_n = buffer_size;
}
filled += read_n;
}
- // \83f\81[\83^\82ð\83^\83C\83\80\83A\83E\83g\95t\82«\82Å\93Ç\82Ý\8fo\82·
+ // �f�[�^���^�C���A�E�g�t���œǂݏo��
filled += internal_receive(&data[filled],
data_size_max - filled, serial, timeout);
return filled;
}
-/* \82P\95¶\8e\9a\8f\91\82«\96ß\82· */
+/* �P���������߂� */
void serial_ungetc(serial_t *serial, char ch)
{
serial->has_last_ch_ = True;
# -*- makefile -*-
-bin_PROGRAMS = main_shape_detect
+bin_PROGRAMS = shape_detect_online shape_detect_offline
-main_shape_detect_SOURCES = main.cc
+shape_detect_online_SOURCES = online.cc
+shape_detect_offline_SOURCES = offline.cc
+
+lib_LOADLIBES = shape_detect roboorte robottype orte rt pthread
-main_shape_detect_LIBS = shape_detect
lib_LIBRARIES = shape_detect
shape_detect_SOURCES = shape_detect.cc
-
-shape_detect_LIBS = roboorte robottype orte rt
-
include_HEADERS = shape_detect.h
-0,0,\r
-1,0,\r
-2,0,\r
-3,0,\r
-4,0,\r
-5,0,\r
-6,0,\r
-7,0,\r
-8,0,\r
-9,0,\r
-10,0,\r
-11,0,\r
-12,0,\r
-13,0,\r
-14,0,\r
-15,0,\r
-16,0,\r
-17,0,\r
-18,0,\r
-19,0,\r
-20,0,\r
-21,0,\r
-22,0,\r
-23,0,\r
-24,0,\r
-25,0,\r
-26,0,\r
-27,0,\r
-28,0,\r
-29,0,\r
-30,0,\r
-31,0,\r
-32,0,\r
-33,0,\r
-34,0,\r
-35,0,\r
-36,0,\r
-37,0,\r
-38,0,\r
-39,0,\r
-40,0,\r
-41,0,\r
-42,0,\r
-43,0,\r
44,181,\r
45,187,\r
46,189,\r
+++ /dev/null
-
-#include <shape_detect.h>
-
-// connect Hokuyo
-#define OFFLINE 1
-
-#ifdef OFFLINE
-int main(int argc, char** argv)
-{
- Shape_detect sd;
-
- if (argc < 2) {
- std::cout << "Error: Invalid number of input parameters." << std::endl;
- return 1;
- }
-
- std::vector<int> input_data;
-
- std::ifstream infile(argv[1], std::ios_base::in);
-
- // line input file
- std::string line;
-
- // number from input file
- int number;
-
- int tmp = 1;
-
- while (std::getline(infile, line, ',')) {
- if (tmp) {
- tmp = 0;
- continue;
- }
- if (line != "\n") {
- std::stringstream strStream(line);
- strStream >> number;
- input_data.push_back(number);
- tmp = 1;
- }
- }
-
- std::vector<Shape_detect::Line> output_data;
- // detect line
- sd.shape_detect(input_data, output_data);
- return 0;
-}
-
-#else
-struct robottype_orte_data orte;
-
-void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- Shape_detect sd;
-
- struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
- static int count = 0;
-
- switch (info->status) {
- case NEW_DATA: {
- printf("Scan\n");
- if (++count >= 2) {
- printf("Detect\n");
- count = 0;
-
- std::vector<int> input(HOKUYO_ARRAY_SIZE);
-
- for(unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
- input[i] = (int) instance->data[i];
-
- std::vector<Shape_detect::Line> output;
- sd.shape_detect(input, output);
- }
- break;
- }
- case DEADLINE:
- printf("Deadline\n");
- break;
- }
-}
-
-int robot_init_orte()
-{
- int rv = 0;
-
- rv = robottype_roboorte_init(&orte);
- if (rv) return rv;
-
- robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
- return rv;
-}
-
-int main()
-{
- robot_init_orte();
- return 0;
-}
-
-#endif
-
--- /dev/null
+/**
+ * @file
+ * @author Martin Synek
+ * @author Michal Sojka
+ * @date 11/02/25
+ *
+ * @brief Debug file for testing detection shape.
+ *
+ * More information about using introduced methods
+ * can be found in dokumentation of class Shape_detect (shape_detect.h)
+ *
+ * @ingroup shapedet
+ */
+
+/* Copyright: TODO
+ *
+ */
+
+
+#include <shape_detect.h>
+#include <iostream>
+#include <stdio.h>
+using namespace std;
+
+ostream& operator << (ostream& os, Shape_detect::Point& point)
+{
+ os << "(" << point.x << ", " << point.y << ")";
+ return os;
+}
+
+
+ostream& operator << (ostream& os, Shape_detect::Line& line)
+{
+ os << line.a << "--" << line.b;
+ return os;
+}
+
+ostream& operator << (ostream& os, Shape_detect::Arc& arc)
+{
+ os << arc.center << " r=" << arc.radius;
+ return os;
+}
+
+
+
+void gnuplot(std::vector<Shape_detect::Point> &cartes,
+ std::vector<Shape_detect::Line> &lines,
+ std::vector<Shape_detect::Arc> &arcs)
+{
+ FILE *gnuplot;
+ gnuplot = popen("gnuplot", "w");
+ fprintf(gnuplot, "set grid\n");
+ fprintf(gnuplot, "set nokey\n");
+ fprintf(gnuplot, "set size ratio -1\n"); //1.3333
+ fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
+ fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
+ fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n");
+ fprintf(gnuplot, "set style fill transparent solid 0.2 noborder\n");
+ fprintf(gnuplot, "set palette model RGB functions .8-gray*2, .8-gray*2, .8-gray*2\n");
+
+ fprintf(gnuplot, "plot");
+ for (unsigned i=0; i < arcs.size(); i++) {
+ if (arcs[i].debug)
+ fprintf(gnuplot, "'-' matrix using ($1*%g+%g):($2*%g+%g):3 with image, ",
+ arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.x,
+ arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.y);
+ }
+ if (cartes.size() > 0)
+ fprintf(gnuplot, "'-' with points ls 1");
+ if (lines.size() > 0)
+ fprintf(gnuplot, ", '-' with linespoints ls 2");
+ if (arcs.size() > 0)
+ fprintf(gnuplot, ", '-' with circles ls 2");
+ fprintf(gnuplot, "\n");
+
+ // Draw accumulators
+ for (int i = 0; i < (int) arcs.size(); i++) {
+ Shape_detect::Arc &a = arcs[i];
+ if (!a.debug)
+ continue;
+ for (int y=0; y < a.debug->acc_size; y++) {
+ for (int x=0; x < a.debug->acc_size; x++) {
+ fprintf(gnuplot, "%d ", a.debug->bitmap[y*a.debug->acc_size+x]);
+ }
+ fprintf(gnuplot, "\n");
+ }
+ fprintf(gnuplot, "e\ne\n");
+ }
+
+ if (cartes.size() > 0) {
+ for (int i = 0; i < (int) cartes.size(); i++) {
+ fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+
+ if (lines.size() > 0) {
+ for (int i = 0; i < (int) lines.size(); i++) {
+ fprintf(gnuplot, "%f %f\n%f %f\n\n",
+ lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+ if (arcs.size() > 0) {
+ // Draw circles
+ for (int i = 0; i < (int) arcs.size(); i++) {
+ fprintf(gnuplot, "%f %f %f\n",
+ arcs[i].center.x, arcs[i].center.y, arcs[i].radius);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+
+ fflush(gnuplot);
+ getchar();
+
+ pclose(gnuplot);
+}
+
+void get_laser_scan(unsigned short laser_scan[], const char *fname)
+{
+
+ std::ifstream infile(fname, std::ios_base::in);
+
+ // line input file
+ std::string line;
+
+ // number from input file
+ unsigned short number;
+
+ int idx = 0;
+
+ while (std::getline(infile, line, '\n')) {
+ std::stringstream strStream(line);
+ strStream >> number;
+ // Ignore out of range data (generated by simulator in robomon)
+ if (number >= 4000)
+ number = 0;
+ laser_scan[idx] = number;
+ idx++;
+ }
+}
+
+/**
+ * There are detected line segments in input file (laser scan data)
+ * @param argv name input file with measured data
+ * @ingroup shapedet
+ */
+int main(int argc, char** argv)
+{
+ if (argc < 2 || strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) {
+ std::cout << "Help:" << std::endl;
+ std::cout << "Example: ./shape_detect_offline -g -f file_name -n seq_length" << std::endl;
+ std::cout << "-g: Gnuplot output." << std::endl;
+ std::cout << "-f file_name example: seq-scan-<NUMBER>." << std::endl;
+ std::cout << "-n seq_length: Maximal length sequence file <NUMBER>." << std::endl;
+ std::cout << std::endl;
+
+ return 0;
+ }
+
+ Shape_detect sd;
+
+ bool plot = false;
+ char fname[50];
+ int number_file = 0;
+ unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
+
+ for (int i = 1; i < argc; i++) {
+ if (strcmp(argv[i], "-g") == 0)
+ plot = true;
+ else if (strcmp(argv[i], "-f") == 0) {
+ i++;
+ strcpy(fname, argv[i]);
+ } else if (strcmp(argv[i], "-n") == 0) {
+ i++;
+ number_file = atoi(argv[i]);
+ } else {
+ std::cout << "Invalid input parameter: " << argv[i] << "." << std::endl;
+ return 1;
+ }
+ }
+
+ std::vector<Shape_detect::Line> lines;
+ std::vector<Shape_detect::Arc> arcs;
+ std::vector<Shape_detect::Arc> result;
+
+ if (number_file == 0) {
+ std::cout << "Open file: " << fname << std::endl;
+ get_laser_scan(laser_scan, fname);
+
+ sd.prepare(laser_scan);
+ sd.line_detect(lines);
+ sd.arc_detect(result);
+ } else {
+ for (int i = 1; i <= number_file; i++) {
+ char name[50];
+ if (i < 10)
+ sprintf(name, "%s0%d", fname, i);
+ else
+ sprintf(name, "%s%d", fname, i);
+
+ std::cout << "Open file: " << name << std::endl;
+
+ get_laser_scan(laser_scan, name);
+
+ sd.prepare(laser_scan);
+
+ if (i == 1) {
+ sd.line_detect(lines);
+ sd.arc_detect(result);
+ continue;
+ }
+
+ sd.arc_detect(arcs);
+ result = sd.arcs_compare(result, arcs, 8);
+
+ if (result.size() == 0)
+ break;
+ }
+ }
+
+ std::cout << std::endl << "Result:" << std::endl;
+
+ for (unsigned i = 0; i < lines.size(); i++)
+ cout << "Line: " << lines[i] << endl;
+
+ for (unsigned i = 0; i < result.size(); i++)
+ cout << "Arc: " << result[i] << endl;
+
+ if (plot)
+ gnuplot(sd.getCartes(), lines, result);
+
+ return 0;
+}
+
--- /dev/null
+/**
+ * @file
+ * @author Martin Synek
+ * @author Michal Sojka
+ * @date 11/02/25
+ *
+ * @brief Debug file for testing detection shape.
+ *
+ * More information about using introduced methods
+ * can be found in dokumentation of class Shape_detect (shape_detect.h)
+ *
+ * @ingroup shapedet
+ */
+
+#include <shape_detect.h>
+#include <roboorte_robottype.h>
+
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ Shape_detect sd;
+
+ struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
+ static int count = 0;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ printf("Scan\n");
+ if (++count >= 2) {
+ printf("Detect\n");
+ count = 0;
+
+ std::vector<Shape_detect::Line> output;
+ sd.prepare(instance->data);
+ sd.line_detect(output);
+ }
+ break;
+ }
+ case DEADLINE:
+ printf("Deadline\n");
+ break;
+ }
+}
+
+struct robottype_orte_data orte;
+
+int robot_init_orte()
+{
+ int rv = 0;
+
+ rv = robottype_roboorte_init(&orte);
+ if (rv) return rv;
+
+ robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
+ return rv;
+}
+
+int main()
+{
+ robot_init_orte();
+ return 0;
+}
+/**
+ * @file shape_detect.cc
+ * @author Martin Synek
+ * @author Michal Sojka
+ * @date 11/02/25
+ *
+ * @ingroup shapedet
+ */
+
+/* Copyright: TODO
+ *
+ */
+
#include "shape_detect.h"
-#ifdef GNUPLOT
-void Shape_detect::plot_line(int begin, int end, int maxdist, std::vector<Point> &cartes, std::vector<Shape_detect::Line> &lines)
+Shape_detect::Shape_detect()
{
- fprintf(gnuplot, "set grid\n");
- fprintf(gnuplot, "set nokey\n");
- fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
- fprintf(gnuplot, "plot 'cartes1' with points ls 2, 'cartes2' with points ls 3");
-
- fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 4 with linespoints",cartes[begin].x, cartes[begin].y, cartes[end].x, cartes[end].y);
- fprintf(gnuplot, ", \"< echo \'%f %f\'\" ls 1 pointsize 3 with points",cartes[maxdist].x, cartes[maxdist].y);
+ Shape_detect::Line_min_points = 7;
+ Shape_detect::Line_error_threshold = 20;
+ Shape_detect::Max_distance_point = 300;
- for (int i = 0; i < (int) lines.size(); i++) {
- fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 1 with linespoints",lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
- }
+ Shape_detect::Radius = 100;
+ Shape_detect::Scale = 10;
+ Shape_detect::Arc_min_points = 10;
+ Shape_detect::Arc_max_distance = 2000;
- fprintf(gnuplot, "\n");
- fflush(gnuplot);
- getchar();
+ bresenham_circle(circle);
}
-void Shape_detect::plot_shape_detect(std::vector<Shape_detect::Line> &lines, std::vector<Point> &cartes)
+Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point,
+ float radius, float scale, int arc_min_points, int arc_max_distance)
{
- fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
- fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
-
-#ifdef OFFLINE
- fprintf(gnuplot, "plot");
-#else
- fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000]");
-#endif
- fprintf(gnuplot, "'-' with points ls 1, "); // points
- fprintf(gnuplot, "'-' with linespoints ls 2"); // lines
- fprintf(gnuplot, "\n");
-
- // points data
- for (int i = 0; i < (int) cartes.size(); i++) {
- fprintf(gnuplot, "%g %g\n",cartes[i].x, cartes[i].y);
- }
- fprintf(gnuplot, "e\n");
+ Shape_detect::Line_min_points = line_min_points;
+ Shape_detect::Line_error_threshold = line_error_threshold;
+ Shape_detect::Max_distance_point = max_distance_point;
- // lines data
- for (int i = 0; i < (int) lines.size(); i++) {
- fprintf(gnuplot, "%f %f\n%f %f\n\n",
- lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
- }
- fprintf(gnuplot, "e\n");
- fflush(gnuplot);
-}
+ Shape_detect::Radius = radius;
+ Shape_detect::Scale = scale;
+ Shape_detect::Arc_min_points = arc_min_points;
+ Shape_detect::Arc_max_distance = arc_max_distance;
-void Shape_detect::gnuplot_init()
-{
- gnuplot = popen("gnuplot", "w");
- fprintf(gnuplot, "set grid\n");
- fprintf(gnuplot, "set nokey\n");
-#ifdef OFFLINE
- fprintf(gnuplot, "set size ratio 1.3333\n");
-#endif
+ bresenham_circle(circle);
}
-#endif // GNUPLOT
-
-Shape_detect::Shape_detect (void) {};
inline Shape_detect::Point Shape_detect::intersection_line(const Shape_detect::Point point, const General_form gen)
{
return tmp;
}
-int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, std::vector<Shape_detect::Point> &cartes, General_form &gen)
+inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
+{
+ return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+}
+
+std::vector<Shape_detect::Arc> Shape_detect::arcs_compare(std::vector<Shape_detect::Arc> &first, std::vector<Shape_detect::Arc> &second, int eps)
+{
+ std::vector<Shape_detect::Arc> result;
+
+ if (first.size() < 1 && second.size() < 1) {
+ return result;
+ }
+ if (first.size() < 1 && second.size() > 0) {
+ return second;
+ }
+ if (first.size() > 0 && second.size() < 1) {
+ return first;
+ }
+
+ for (unsigned int i = 0; i < first.size(); i++) {
+ for (unsigned int j = 0; j < second.size(); j++) {
+ if (point_distance(first[i].center, second[j].center) < eps) {
+ result.push_back(first[i]);
+ break;
+ }
+ }
+ }
+
+ return result;
+}
+
+void Shape_detect::bresenham_circle(std::vector<Shape_detect::Point> &circle)
+{
+ int d;
+
+ int scale_radius = abs(Shape_detect::Radius / Shape_detect::Scale);
+
+ int x = 0;
+ int y = scale_radius;
+
+ Shape_detect::Point tmp;
+
+ tmp.x = x;
+ tmp.y = y;
+
+ circle.push_back(tmp);
+
+ d = 3 - (2 * scale_radius);
+
+ for (x = 0; x <= y; x++) {
+ if (d < 0) {
+ y = y;
+ d = (d + (4 * x) + 6);
+ } else {
+ y--;
+ d = d + (4 * (x - y) + 10);
+ }
+
+ tmp.x = x;
+ tmp.y = y;
+
+ circle.push_back(tmp);
+ }
+
+ return;
+}
+
+void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs)
+{
+ float max_x, max_y, min_x, min_y;
+
+ max_x = cartes[begin].x;
+ min_x = max_x;
+
+ max_y = cartes[begin].y;
+ min_y = max_y;
+
+ for (int i = begin; i <= end; i++) {
+ //std::cout << cartes[i].x << " -- " << cartes[i].y << " || ";
+ if (cartes[i].x > max_x)
+ max_x = cartes[i].x;
+ else if (cartes[i].x < min_x)
+ min_x = cartes[i].x;
+
+ if (cartes[i].y > max_y)
+ max_y = cartes[i].y;
+ else if (cartes[i].y < min_y)
+ min_y = cartes[i].y;
+ }
+
+ const float center_x = (max_x - min_x) / 2 + min_x;
+ const float center_y = (max_y - min_y) / 2 + min_y;
+
+ const int asize = 600 / Shape_detect::Scale;
+ const int amid = asize / 2;
+
+ Shape_detect::Arc arc;
+ //arc.debug = 0;
+ arc.debug = new (struct arc_debug);
+
+ if (arc.debug) {
+ arc.debug->bitmap = new int[asize * asize];
+ arc.debug->acc_size = asize;
+ arc.debug->acc_origin.x = center_x - amid * Shape_detect::Scale;
+ arc.debug->acc_origin.y = center_y - amid * Shape_detect::Scale;
+ arc.debug->acc_scale = Shape_detect::Scale;
+ }
+
+ int accumulator[asize][asize];
+
+ memset(accumulator, 0, sizeof(accumulator));
+
+ for (int i = begin; i <= end; i++) {
+ int xc = floor((cartes[i].x - center_x) / Shape_detect::Scale);
+ int yc = floor((cartes[i].y - center_y) / Shape_detect::Scale);
+
+ for (unsigned int i = 0; i < circle.size(); i++) {
+ int par[8];
+
+ par[0] = amid + yc + (int) circle[i].x;
+ par[1] = amid + yc - (int) circle[i].x;
+ par[2] = amid + xc + (int) circle[i].x;
+ par[3] = amid + xc - (int) circle[i].x;
+ par[4] = amid + yc + (int) circle[i].y;
+ par[5] = amid + yc - (int) circle[i].y;
+ par[6] = amid + xc + (int) circle[i].y;
+ par[7] = amid + xc - (int) circle[i].y;
+
+ if (par[0] > 0 && par[0] < asize && par[6] > 0 && par[6] < asize)
+ accumulator[par[0]][par[6]] += 1;
+
+ if (par[1] > 0 && par[1] < asize && par[7] > 0 && par[7] < asize)
+ accumulator[par[1]][par[7]] += 1;
+
+ if (par[1] > 0 && par[1] < asize && par[6] > 0 && par[6] < asize)
+ accumulator[par[1]][par[6]] += 1;
+
+ if (par[0] > 0 && par[0] < asize && par[7] > 0 && par[7] < asize)
+ accumulator[par[0]][par[7]] += 1;
+
+ if (par[4] > 0 && par[4] < asize && par[2] > 0 && par[2] < asize)
+ accumulator[par[4]][par[2]] += 1;
+
+ if (par[5] > 0 && par[5] < asize && par[3] > 0 && par[3] < asize)
+ accumulator[par[5]][par[3]] += 1;
+
+ if (par[5] > 0 && par[5] < asize && par[2] > 0 && par[2] < asize)
+ accumulator[par[5]][par[2]] += 1;
+
+ if (par[4] > 0 && par[4] < asize && par[3] > 0 && par[3] < asize)
+ accumulator[par[4]][par[3]] += 1;
+ }
+ }
+
+ Shape_detect::Point center;
+
+ center.x = 0;
+ center.y = 0;
+
+ arc.radius = Shape_detect::Radius; //radius [mm]
+
+ int max = 0;
+
+ for (int i = 0; i < asize; i++) {
+ for (int j = 0; j < asize; j++) {
+ if (accumulator[i][j] > max) {
+
+ max = accumulator[i][j];
+
+ center.x = (j - amid) * Shape_detect::Scale + center_x;
+ center.y = (i - amid) * Shape_detect::Scale + center_y;
+ }
+ }
+ }
+
+ Shape_detect::Point origin;
+
+ origin.x = 0.0;
+ origin.y = 0.0;
+
+ if (max > (end - begin) / 3 && point_distance(origin, center) > point_distance(origin, cartes[end])) {
+ arc.center = center;
+ memcpy(arc.debug->bitmap, accumulator, sizeof(accumulator));
+ arcs.push_back(arc);
+ }
+
+ return;
+}
+
+int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, General_form &gen)
{
int number_points = abs(end-begin) + 1;
float sum_x = 0;
float sum_y = 0;
-
+
for (int i = begin; i <= end; i++) {
sum_x = sum_x + cartes[i].x;
sum_y = sum_y + cartes[i].y;
float b1 = med_y - m1*med_x;
float b2 = med_y - m2*med_x;
-
+
// maximum error
r = 0;
unsigned ir = -1;
// distance point from the line (A = m1, B = -1, C = b1)
dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
-
+
if (dist1 > r1) {
r1 = dist1;
ir1 = i;
ir = i;
}
}
-
+
if (r < r1) {
gen.a = m1;
gen.c = b1;
return 0;
}
-// line recursive fitting
-void Shape_detect::line_fitting(int begin, int end, std::vector<Shape_detect::Point> &cartes, std::vector<Shape_detect::Line> &lines)
+ // line recursive fitting
+void Shape_detect::line_fitting(int begin, int end, std::vector<Shape_detect::Line> &lines)
{
- if ((end - begin) < LINE_MIN_POINTS) return;
+ if ((end - begin) < Shape_detect::Line_min_points) return;
float r;
General_form gen;
- if (perpendicular_regression(r, begin, end, cartes, gen)) return; // r = 0
+ if (perpendicular_regression(r, begin, end, gen)) return; // r = 0
- if (r < LINE_ERROR_THRESHOLD) {
+ if (r < Shape_detect::Line_error_threshold) {
Shape_detect::Line tmp;
tmp.a = intersection_line(cartes[begin], gen);
} else {
// Ax+By+C=0
// normal vector: n[n_x, -n_y]
-
+
float n_x = cartes[begin].y - cartes[end].y;
float n_y = cartes[begin].x - cartes[end].x;
}
}
- //plot_line(begin, end, line_break_point, cartes, lines);
-
- if (dist_max > LINE_ERROR_THRESHOLD) {
- line_fitting(begin, line_break_point, cartes, lines);
- line_fitting(line_break_point, end, cartes, lines);
+ if (dist_max > Shape_detect::Line_error_threshold) {
+ line_fitting(begin, line_break_point, lines);
+ line_fitting(line_break_point, end, lines);
}
- } // end if (r <= LINE_ERROR_THRESHOLD)
+ } // end if (r <= Line_error_threshold)
return;
}
// polar to cartesian coordinates
-void Shape_detect::polar_to_cartes(const std::vector<int> &input_data, std::vector<Shape_detect::Point> &cartes)
+void Shape_detect::polar_to_cartes(const unsigned short laser_scan[])
{
Shape_detect::Point point;
float fi;
int r;
- for (int i = 0; i < (int) input_data.size()-1; i++) {
- r = (input_data[i] <= 19) ? 0 : input_data[i];
+ for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE; i++) {
+ r = (laser_scan[i] <= 19) ? 0 : laser_scan[i];
+
+ fi = HOKUYO_INDEX_TO_RAD(i);
- if (r > 0) {
- fi = HOKUYO_INDEX_TO_RAD(i);
+ if (r > 0 && fi > (-1 * HOKUYO_RANGE_ANGLE_LEFT / 180.0 * M_PI) && fi < (HOKUYO_RANGE_ANGLE_RIGHT / 180.0 * M_PI)) {
+ fi = HOKUYO_INDEX_TO_RAD(i);
point.x = r * cos(fi);
point.y = r * sin(fi);
cartes.push_back(point);
}
}
+
+ //std::cout << "velikost cartes: " << cartes.size() << std::endl;
+
}
-inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
+std::vector<Shape_detect::Point> &Shape_detect::getCartes()
{
- return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+ return cartes;
+}
+
+void Shape_detect::prepare(const unsigned short laser_scan[])
+{
+ cartes.clear();
+ polar_to_cartes(laser_scan);
}
-void Shape_detect::shape_detect(const std::vector<int> &input_data, std::vector<Shape_detect::Line> &lines)
+void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
{
-#ifdef GNUPLOT
- gnuplot_init();
-#endif
- // polar coordinates to cartesian coordinates
- std::vector<Shape_detect::Point> cartes;
- polar_to_cartes(input_data, cartes);
+ int cartes_size = cartes.size();
+
+ int end, start = 0;
+ while (start < cartes_size) {
+ Shape_detect::Point tmp_start = cartes[start];
+
+ end = start + 1;
+
+ while (point_distance(cartes[end-1], cartes[end]) < 100
+ && end < cartes_size
+ && cartes[end].x < Shape_detect::Arc_max_distance
+ && cartes[end].y < Shape_detect::Arc_max_distance) {
+ end++;
+ }
+
+ end --;
+
+ if (point_distance(tmp_start, cartes[end]) < (2 * Shape_detect::Radius) && (end - start > Shape_detect::Arc_min_points))
+ hough_transform_arc(start, end, arcs);
+
+ start = end + 1;
+ }
+}
+
+void Shape_detect::line_detect(std::vector<Shape_detect::Line> &lines)
+{
int cartes_size = cartes.size();
int end, start = 0;
while (start < cartes_size) {
end = start + 1;
- while (point_distance(cartes[end-1], cartes[end]) < MAX_DISTANCE_POINT && end < cartes_size)
+ while (point_distance(cartes[end-1], cartes[end]) < Shape_detect::Max_distance_point && end < cartes_size)
end++;
end--;
- line_fitting(start, end, cartes, lines);
+ line_fitting(start, end, lines);
start = end + 1;
}
-#ifdef GNUPLOT
- plot_shape_detect(lines, cartes);
- getchar();
- pclose(gnuplot);
-#endif
}
+/**
+ * @file shape_detect.h
+ * @author Martin Synek
+ * @author Michal Sojka
+ * @date 11/02/25
+ *
+ * @brief Shape detection from laser scan data
+ */
+
+/* Copyright: TODO
+ *
+ */
+
+/**
+\defgroup shapedet Shape detection
+
+Library @a shape_detect is used for detection of line segments in data
+measured by a laser scanner.
+
+Content:
+- \ref shapedet_general
+- \ref shapedet_debug_mode
+- \ref shapedet_example
+- \ref shapedet_ref
+
+\section shapedet_general Introduction
+
+The input of the algorithm is represented by array laser_scan (type
+unsigned short) which contains data expressed in polar coordinates.
+The first step is data conversion from polar coordinates to vector
+whose points are expressed in cartesian coordinates. The section
+includes filtering of errors in the scanned data which are represented
+by values lower then 20.
+
+In the next is divided pointvector in dependence on parameter @a max_distance_point.
+Some line segment is searched by recursion in the individual parts of pointvector
+by perpendicular line regression.
+The line segment is detected or set is divided to achievement of parametersize
+line_min_points.
+
+The founded line is written into output vector lines. The method line_detect
+is finished after research of all vectorparts.
+
+\section shapedet_debug_mode Debugging
+
+The debug mode permits detection of segment lines with connected laser
+scanner or from scanned data, which are saved in a file. There is a
+program @a shape_detect_offline, which takes the measured data
+(generated for example by @a hokuyo-dump command) and writes out the
+detected lines. It can also plot the results using gnuplot (-g
+option).
+
+\section shapedet_example Example
+
+In the file main.cc are introduced examples of using class Shape_detect.
+The first step is creation of classinstance by constructor calling
+(default setting for Hokuyo or with applicable parameters). After that it is possible
+preparation of output vector for saving founded segment line and calling method
+shape_detect with applicable parameters.
+
+The first example is work illustration with connected laser scanner Hokuyo.
+
+\include hokuyo/shape-detect/online.cc
+
+\section shapedet_ref References
+
+Fast line, arc/circle and leg detection from laser scan data in a Player driver,
+http://w3.ualg.pt/~dcastro/a1738.pdf
+
+Mathpages, Perpendicular regression of a line,
+http://mathpages.com/home/kmath110.htm
+
+ */
+
#ifndef SHAPE_DETECT
#define SHAPE_DETECT
#include <hokuyo.h>
#include <robot.h>
#include <robomath.h>
+#include <robodim.h>
#include <robottype.h>
#include <roboorte_robottype.h>
-// gnuplot graph
-//#define GNUPLOT 0
-
-// debug mode with connect Hokuyo
-#define OFFLINE 1
-
-#define LINE_MIN_POINTS 7
-#define LINE_ERROR_THRESHOLD 20
-#define MAX_DISTANCE_POINT 300
-
+/**
+ * There are detected line segments in input array of measured data (laser_scan)
+ * by using perpendicular line regression.
+ * The output is formed by vector of type Line (so detected segment line - coordinates endpoints).
+ * @ingroup shapedet
+ */
class Shape_detect
{
public:
+ /**
+ * The constructor with default setting of detection properties (for Hokuyo).
+ * Line_min_points = 7
+ * Line_error_threshold = 20
+ * Max_distance_point = 300
+ * Radius = 100
+ * Scale = 10
+ * Arc_min_points = 10
+ * Arc_max_distance = 1000
+ * @ingroup shapedet
+ */
Shape_detect (void);
- typedef struct {float a,b,c;} General_form; // Line equation - General form
- typedef struct {float x,y;} Point;
- typedef struct {Point a,b;} Line;
+ /**
+ * The constructor for other setting than default setting of detection properties.
+ * @param line_min_points the minimal number of points which can create segment line.
+ * @param line_error_threshold the maximal pointerror from segment line of regression.
+ * @param max_distance_point the maximal Euclidean distance of point.
+ * @param radius the radius detected arc.
+ * @param scale is precision detected center of arc.
+ * @param arc_min_points the minimal number of points which can create arc.
+ * @param arc_max_distance the maximal distance detected arc from origin coordinates [0; 0]
+ * @ingroup shapedet
+ */
+ Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point,
+ float radius, float scale, int arc_min_points, int arc_max_distance);
- void shape_detect(const std::vector<int> &input_data, std::vector<Line> &lines);
+ /**
+ * General equation of line -> Ax + By + C = 0.
+ * Is used for calculation of lines intersection.
+ * @ingroup shapedet
+ */
+ typedef struct {float a,b,c;} General_form;
- private:
-#ifdef GNUPLOT
- FILE *gnuplot;
+ /**
+ * Point expressed in cartesian coordinates.
+ * @ingroup shapedet
+ */
+ typedef struct {
+ float x; /**< x coordinates point. */
+ float y; /**< y coordinates point. */
+ } Point;
+
+ /**
+ * Line defined by two points which are expressed in cartesian coordinates.
+ * @ingroup shapedet
+ */
+ typedef struct {
+ Point a; /**< start point from a line. */
+ Point b; /**< end point from a line. */
+ } Line;
+
+ struct arc_debug {
+ Point acc_origin;
+ float acc_scale;
+ int acc_size;
+ int *bitmap;
+ };
+
+ /**
+ * Arc defined by TODO.
+ * @ingroup shapedet
+ */
+ typedef struct {
+ Point center;
+ Point begin;
+ Point end;
+ float radius;
+ struct arc_debug *debug;
+ } Arc;
+
+ /**
+ * TODO
+ * @param [in] laser_scan contains laser scanned data.
+ * @ingroup shapedet
+ */
+ void prepare(const unsigned short laser_scan[]);
+
+ /**
+ * Returns laser_scan data set by prepare converted to cartesian coordinates
+ * @ingroup shapedet
+ */
+ std::vector<Point> &getCartes();
+
- void plot_line(int begin, int end, int maxdist, std::vector<Point> &cartes, std::vector<Line> &lines);
- void plot_shape_detect(std::vector<Line> &lines, std::vector<Point> &cartes);
- void gnuplot_init();
-#endif
+ /**
+ * There are detected line segments in input array of measured
+ * data by using perpendicular line regression.
+ * @param [out] &lines vector which contains detected lines.
+ * @ingroup shapedet
+ */
+ void line_detect(std::vector<Line> &lines);
+
+ /**
+ * There are detected line segments in input array of measured
+ * data.
+ * @param [out] &arcs vector which contains detected arcs.
+ * @ingroup shapedet
+ */
+ void arc_detect(std::vector<Arc> &arcs);
+ /**
+ * Is uset for comparing of two detected arcs vectors.
+ * @param &first is vector for comparing.
+ * @param &second is vector for comparing.
+ * @param eps is pertubation measured center of arc.
+ * @return vector equality center of arc.
+ * @ingroup shapedet
+ */
+ std::vector<Arc> arcs_compare(std::vector<Arc> &first, std::vector<Arc> &second, int eps);
+
+ private:
+ /**
+ * The minimal number of points which can create segment line.
+ * @ingroup shapedet
+ */
+ int Line_min_points;
+
+ /**
+ * The maximal pointerror from segment line of regression.
+ * @ingroup shapedet
+ */
+ int Line_error_threshold;
+
+ /**
+ * The maximal Euclidean distance of point.
+ * @ingroup shapedet
+ */
+ int Max_distance_point;
+
+ /**
+ * The radius detected arc.
+ * @ingroup shapedet
+ */
+ float Radius; // [mm]
+
+ /**
+ * The precision detected center of arc.
+ * @ingroup shapedet
+ */
+ float Scale; // [mm]
+
+ /**
+ * The minimal number of points which can create arc.
+ * @ingroup shapedet
+ */
+ int Arc_max_distance; //[mm]
+
+ /**
+ * The maximal distance detected arc from origin coordinates [0; 0]
+ * @ingroup shapedet
+ */
+ int Arc_min_points;
+
+ std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
+
+ std::vector<Shape_detect::Point> circle;
+
+ /**
+ * Calculation of lines intersection.
+ * @param point which pertains to line.
+ * @param gen is general equation of line.
+ * @return point of intersection.
+ * @ingroup shapedet
+ */
inline Point intersection_line(const Point point, const General_form gen);
- int perpendicular_regression(float &r, const int begin, const int end, std::vector<Point> &cartes, General_form &gen);
-
- void line_fitting(int begin, int end, std::vector<Point> &cartes, std::vector<Line> &lines);
+ /**
+ * Calculating perpendicular regression of a line in input range index points.
+ * @param [out] &r is minimal distance between point and found line.
+ * @param [in] begin is start point.
+ * @param [in] end is last point.
+ * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
+ * @param [out] &gen is general equation of found line.
+ * @return 0 for right course else 1.
+ * @ingroup shapedet
+ */
+ int perpendicular_regression(float &r, const int begin, const int end, General_form &gen);
+
+ /**
+ * In case the input range points does not line. Is range divided and to single parts is again applied line_fitting function.
+ * @param [in] begin is start point.
+ * @param [in] end is last point.
+ * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
+ * @param [out] &lines is vector with detecting lines.
+ * @ingroup shapedet
+ */
+ void line_fitting(int begin, int end, std::vector<Line> &lines);
- void polar_to_cartes(const std::vector<int> &input_data, std::vector<Point> &cartes);
+ /**
+ * Convert vector expressed in polar coordinates to vector expressed in cartesian coordinates.
+ * @param laser_scan laser scanned data expressed in polar coordinates.
+ * @param &cartes is vector whose points are expressed in cartesian coordinates.
+ * @ingroup shapedet
+ */
+ void polar_to_cartes(const unsigned short laser_scan[]);
+ /**
+ * Calculation of distance between points which are expressed in cartesian coordinates.
+ * @param a is first point.
+ * @param b is second point.
+ * @return distance between points.
+ * @ingroup shapedet
+ */
inline float point_distance(Point a, Point b);
+
+ void bresenham_circle(std::vector<Point> &circle);
+
+ // radius [mm], scale [mm]
+ void hough_transform_arc(int begin, int end, std::vector<Arc> &arcs);
};
#endif // SHAPE_DETECT
switch(id) {
case BT1:
if(state) {
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ //act ON
+ act_jaws(CATCH);
+ printf("jaws CATCH\n");
} else {
;//act OFF
}
break;
case BT2:
if(state) {
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ act_lift(0, 0, 1);
+ //act ON
} else {
+ act_lift(0, 0, 0);
;//act OFF
}
break;
case AXIS_R:
break;
case AXIS_S1:
+ switch (value) {
+ case 32767:
+ act_jaws(CLOSE);
+ printf("jaws CLOSE\n");
+ break;
+ case -32767:
+ act_jaws(OPEN);
+ printf("jaws OPEN\n");
+ break;
+ case 0:
+ printf("jaws stop\n");
+ break;
+ default:
+ printf("error!\n");
+ break;
+ }
break;
case AXIS_S2:
+ switch (value) {
+ case 32767:
+ act_lift(DOWN, 0, 0);
+ printf("lift DOWN\n");
+ break;
+ case -32767:
+ act_lift(UP, 0, 0);
+ printf("lift UP\n");
+ break;
+ case 0:
+ printf("lift stop\n");
+ break;
+ default:
+ printf("error!\n");
+ break;
+ }
break;
default:
printf("Unknown axis changed!\n");
signal(SIGINT, int_handler);
- orte.strength = 30;
+ orte.strength = 2;
/* orte initialization */
if (robottype_roboorte_init(&orte)) {
/* creating publishers */
robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte);
- robottype_publisher_vidle_cmd_create(&orte, send_dummy_cb, &orte);
-
+ robottype_publisher_lift_cmd_create(&orte, send_dummy_cb, &orte);
+ robottype_publisher_jaws_cmd_create(&orte, send_dummy_cb, &orte);
+
if(open_joystick(joy_name, &num_of_axis, &num_of_buttons) == -1) {
printf("Joystick not found, exiting...\n");
return -1;
VPATH = ../../motion
-LIBPATH=$(PWD)/../../../build/linux/_compiled/lib
-INCPATH=$(PWD)/../../../build/linux/_compiled/include
+LIBPATH=$(PWD)/../../../build/host/_compiled/lib
+INCPATH=$(PWD)/../../../build/host/_compiled/include
CXXFLAGS += -I$(VPATH) -L$(LIBPATH) -I$(INCPATH) -g
all: sf_posreg.mexglx sf_trgen.mexglx sf_mcl.mexglx
-// Copyright 2009 Michal Sojka <sojkam1@fel.cvut.cz>
+// Copyright 2009, 2012 Michal Sojka <sojkam1@fel.cvut.cz>
// Copyright 2009 Petr Beneš
//
// This file is part of Trgen library.
TrajectorySegment* Arc::splitAt(double distance, Point *newEnd) {
if (distance <= 0 || distance >= length)
- return NULL;
+ return 0;
getPointAt(distance, newEnd);
Arc *ns = new Arc(*this);
-// Copyright 2009 Michal Sojka <sojkam1@fel.cvut.cz>
+// Copyright 2009, 2012 Michal Sojka <sojkam1@fel.cvut.cz>
// Copyright 2009 Petr Beneš
//
// This file is part of Trgen library.
TrajectorySegment* Turn::splitAt(double distance, Point *newEnd) {
if (distance <= 0 || distance >= fabs(turnBy)) {
dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
- return NULL;
+ return 0;
}
Turn *ns = new Turn(*this);
TrajectorySegment* Turn::splitAtByTime(double time, Point *newEnd) {
if (time <= t1 || time >= t2) {
dbgPrintf("splitAt: distance=%g turnBy=%g\n", time, turnBy);
- return NULL;
+ return 0;
}
Turn *ns = new Turn(*this);
brushless_SOURCES = brushless.c cmd_pxmc.c
#position_led_LIBS = boot_fn misc pxmc system_stub arch_drivers sci_channels excptvec m
-brushless_LIBS = boot_fn misc cmdproc cmdprocio pxmcbsp pxmc system_stub excptvec arch_drivers sci_channels candriver
+brushless_LIBS = boot_fn misc cmdproc cmdprocio pxmcbsp pxmc system_stub excptvec arch_drivers sci_channels candriver misc
#endif
+/*
+ * Copyright (C) 2006-2011 Department of Control Engineering, FEE,
+ * Czech Technical University in Prague, Czech Republic,
+ * http://dce.fel.cvut.cz/
+ *
+ * Copyright (C) 2006-2011 Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
/* procesor H8S/2638 ver 1.1 */
#include <types.h>
#include <cpu_def.h>
mcs->pxms_ene=mcs->pxms_me/3;
printf("ptindx hal ap ptofs\n");
+ /* schedule adjustment of phase table from index/mark signal */
+ mcs->pxms_cfg |= PXMS_CFG_I2PT_m;
+
/* Move motor to the initial position */
- mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
+ mcs->pxms_flg = (PXMS_PHA_m * 0)| PXMS_PTI_m; /* Do not update ptindex acording to HALs */
mcs->pxms_ptindx=0; /* Set fixed index to phase tables*/
motor_do_output(mcs); /* Apply mag. field accoring to our index */
last_time = pxmc_msec_counter;
mcs->pxms_flg = 0;
pxmc_call(mcs, mcs->pxms_do_inp); /* Read IRCs */
mcs->pxms_ptindx=ptindx;/* Set fixed index to phase tables*/
- mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
+ mcs->pxms_flg = (PXMS_PHA_m * 0) | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
motor_do_output(mcs); /* Apply mag. field accoring to our index */
last_time = pxmc_msec_counter;
while (pxmc_msec_counter < last_time + 4); /* Wait for motor to move */
}
}
}
+
+ /* enable adjustment of phase table from index/mark signal */
+ mcs->pxms_cfg |= PXMS_CFG_I2PT_m;
+
mcs->pxms_ene=0;
mcs->pxms_ptvang = vang;
motor_do_output(mcs);
return 0;
}
+/* selection of debug messages */
+
+#define DBGPF_AXES 0x00ff /* mask for all axes */
+#define DBGPF_PER_TIME 0x0100 /* periodic time print */
+#define DBGPF_PER_POS 0x0800 /* periodic position info */
+#define DBGPF_CMD_PROC 0x1000 /* command proccessing */
+unsigned dbg_prt_flg=0;
+
+typedef struct dbg_prt_des{
+ unsigned mask;
+ char *name;
+}dbg_prt_des_t;
+
+const dbg_prt_des_t dbg_prt_des[]={
+ {~0,"all"},
+ {1,"A"},{2,"B"},
+ {DBGPF_PER_TIME,"time"},
+ {DBGPF_PER_POS, "pos"},
+ {DBGPF_CMD_PROC,"cmd"},
+ {0,NULL}
+};
+
+
+void run_dbg_prt(cmd_io_t *cmd_io)
+{
+ char s[20];
+ int i;
+ pxmc_state_t *mcs;
+
+ if(dbg_prt_flg & DBGPF_PER_POS) {
+ char reg_ascii_id[4];
+ short reg_mask;
+ reg_ascii_id[1]='!';
+ reg_ascii_id[2]=0;
+
+ for(i=0,reg_mask=1;i<pxmc_main_list.pxml_cnt; i++,reg_mask<<=1){
+ if(dbg_prt_flg®_mask&DBGPF_AXES){
+ reg_ascii_id[0]='A'+i;
+ cmd_io_puts(cmd_io,reg_ascii_id);
+ mcs=pxmc_main_list.pxml_arr[i];
+ i2str(s,mcs->pxms_ap>>PXMC_SUBDIV(mcs),8,0);
+ cmd_io_puts(cmd_io,s);
+ cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_ERR_m?" E":
+ mcs->pxms_flg&PXMS_BSY_m?" B":" -");
+
+ cmd_io_puts(cmd_io," hal");
+ i2str(s,mcs->pxms_hal,2,0);
+ cmd_io_puts(cmd_io,s);
+
+ cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_PHA_m?" A":
+ mcs->pxms_flg&PXMS_PTI_m?" I":" -");
+
+ cmd_io_puts(cmd_io," i");
+ i2str(s,mcs->pxms_ptindx,4,0);
+ cmd_io_puts(cmd_io,s);
+
+ cmd_io_puts(cmd_io," o");
+ i2str(s,mcs->pxms_ptofs,6,0);
+ cmd_io_puts(cmd_io,s);
+
+ cmd_io_puts(cmd_io,"\r\n");
+ }
+ }
+ }
+}
+
+int cmd_do_switches(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ unsigned val,*pval;
+ long l;
+ dbg_prt_des_t *pdes,*pd;
+ char *ps;
+ char str[20];
+ char pm_flag;
+ pval=(unsigned*)(des->info[0]);
+ pdes=(dbg_prt_des_t*)(des->info[1]);
+ ps=(des->mode&CDESM_OPCHR)?param[3]:param[1];
+ val=*pval;
+ if(*ps=='?'){
+ while(pdes->name){
+ printf(" %s",pdes->name);
+ pdes++;
+ }
+ printf("\n");
+ }
+ while(*ps){
+ si_skspace(&ps);
+ if(!*ps) break;
+ pm_flag=0;
+ if(*ps=='+'){ps++;}
+ else if(*ps=='-'){pm_flag=1;ps++;}
+ else val=0;
+ if(isdigit((uint8_t)*ps)){if(si_long(&ps,&l,0)<0) return -1;}
+ else{
+ si_alnumn(&ps,str,20);
+ pd=pdes;
+ do{
+ if(!pd->name) return -1;
+ if(!strcmp(pd->name,str)){
+ l=pd->mask; break;
+ }
+ pd++;
+ }while(1);
+ }
+ if(pm_flag) val&=~l;
+ else val|=l;
+ }
+ *pval=val;
+ return 0;
+}
+
+int cmd_do_pthalign(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ int res;
+
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+
+ if(*param[2]=='?') {
+ return cmd_opchar_replong(cmd_io, param, (long)mcs->pxms_ptmark, 0, 0);
+ }
+
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+
+ res = motor_pthalalign(mcs, 20);
+
+ if(res < 0)
+ return -CMDERR_BADDIO;
+
+ return 0;
+}
+
+
int runtime_display = 0;
int slowgo = 0;
cmd_do_setvang};
cmd_des_t const cmd_des_halindex={0, 0,"HALTEST?","show hal state with respect to ptindex",
cmd_do_haltest};
+cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run alignement of phase according to HAL",
+ cmd_do_pthalign};
cmd_des_t const cmd_des_setshift={0, 0,"SETSHIFT?","changes pxms_ptshift",
cmd_do_setshift};
cmd_des_t const cmd_des_quit={0, 0,"QUIT","exit",
cmd_des_t const cmd_des_canst={0, 0,"CANST","Print CAN controller status",
cmd_do_canst};
+cmd_des_t const cmd_des_dprint={0, 0,"dprint","enable debug messages to print, use + - to add remove, list types ?",
+ cmd_do_switches,
+ {(char*)&dbg_prt_flg,(char*)dbg_prt_des}};
+
cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
cmd_do_setflags};
&cmd_des_setindex,
&cmd_des_setvang,
&cmd_des_halindex,
+ &cmd_des_pthalign,
&cmd_des_setshift,
&cmd_des_showene,
&cmd_des_nopxmc,
&cmd_des_disp,
&cmd_des_slowgo,
&cmd_des_canst,
+ &cmd_des_dprint,
(cmd_des_t*)1,
(cmd_des_t*)cmd_stm_default,
NULL
break;
}
- case CAN_CORR_TRIG:
- odometry_triggered = true;
- trig_seq = msg_rcv.data[0];
- blink_corr_trig();
- break;
}
led_can_rec(5);
void _print(char *ptr);
int main()
{
+ int dbg_prt_last_msec;
+
cli();
excptvec_initfill(unhandled_exception, 0);
sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
DEB_LED_ON(LED_RESET);
wdg_enable(6); /* 420 ms */
sti();
- _print("CPU initialized\r\n");
+
+ _print("CPU initialized\r\n\r\n");
+
+ printf("Eurobot motor control application.\n"
+ "Copyright (C) 2005-2011 PiKRON s.r.o., P. Pisa, M. Sojka and others.\n"
+ "This is free software; see the source code for copying conditions.\n"
+ "There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or\n"
+ "FITNESS FOR A PARTICULAR PURPOSE.\n\n");
+
pxmc_initialize();
printf("PXMC initialized (motor: %s)", pxmc_variant);
printf("\n");
- int32_t receive_id[] = { CAN_CORR_TRIG, CAN_MOTION_CMD, CAN_MOTION_RESET, -1 };
+ pxmc_set_const_out(&mcs_left,0);
+ pxmc_set_const_out(&mcs_right,0);
+
+ int32_t receive_id[] = { CAN_MOTION_CMD, CAN_MOTION_RESET, -1 };
canInit(0, 1000000, receive_id);
printf("CAN initialized\n");
+ dbg_prt_last_msec = pxmc_msec_counter;
+
do {
+ int res;
+
wdg_clear();
handle_can_receive();
handle_odometry_send();
handle_status_send();
handle_motor_errors();
- cmd_processor_run(&cmd_io_rs232_line, cmd_list_default); // run command processor on serial line
+ res = cmd_processor_run(&cmd_io_rs232_line, cmd_list_default); // run command processor on serial line
+
+ if(!res && ((int)(pxmc_msec_counter - dbg_prt_last_msec) > 2000) &&
+ !cmd_io_rs232_line.priv.ed_line.in->lastch) {
+ cmd_io_t *cmd_io = &cmd_io_rs232_line;
+ dbg_prt_last_msec = pxmc_msec_counter;
+
+ if (cmd_io->priv.ed_line.io_stack)
+ cmd_io = cmd_io->priv.ed_line.io_stack;
+ run_dbg_prt(cmd_io);
+ }
handle_leds();
bin_PROGRAMS = odometry
odometry_SOURCES = brushless.c cmd_pxmc.c
-odometry_LIBS = boot_fn misc cmdproc cmdprocio pxmcbsp pxmc system_stub excptvec arch_drivers sci_channels candriver
+odometry_LIBS = boot_fn misc cmdproc cmdprocio pxmcbsp pxmc misc system_stub excptvec arch_drivers sci_channels candriver
#endif
+/*
+ * Copyright (C) 2006-2011 Department of Control Engineering, FEE,
+ * Czech Technical University in Prague, Czech Republic,
+ * http://dce.fel.cvut.cz/
+ *
+ * Copyright (C) 2006-2011 Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
/* procesor H8S/2638 ver 1.1 */
#include <types.h>
#include <cpu_def.h>
DEB_LED_ON(LED_RESET);
wdg_enable(6); /* 420 ms */
sti();
- _print("CPU initialized\r\n");
+ _print("CPU initialized\r\n\r\n");
+ printf("Eurobot odometry application.\n"
+ "Copyright (C) 2005-2011 PiKRON s.r.o., P. Pisa, M. Sojka and others.\n"
+ "This is free software; see the source code for copying conditions.\n"
+ "There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or\n"
+ "FITNESS FOR A PARTICULAR PURPOSE.\n\n");
+
pxmc_initialize();
printf("PXMC odometry initialized (motor: %s)", pxmc_variant);
printf("\n");
#define __MAP_H
#include <stdbool.h>
+#include <robodim.h>
/**
* @defgroup maplib Library to manage the map
/** @name Map constaints */
/**@{*/
-#define MAP_WIDTH 30 /**< Field width*/
-#define MAP_HEIGHT 21 /**< Field height*/
-#define MAP_CELL_SIZE_MM 100 /**< Size of a cell in mm. The cell is a square. */
-#define MAP_CELL_SIZE_M (MAP_CELL_SIZE_MM/1000.0)
-#define MAP_PLAYGROUND_WIDTH_MM (MAP_WIDTH*MAP_CELL_SIZE_MM) /**< Playground width depends on width and cell size. */
-#define MAP_PLAYGROUND_WIDTH_M (MAP_PLAYGROUND_WIDTH_MM/1000.0)
-#define MAP_PLAYGROUND_HEIGHT_MM (MAP_HEIGHT*MAP_CELL_SIZE_MM) /**< Playground width depends on height and cell size. */
-#define MAP_PLAYGROUND_HEIGHT_M (MAP_PLAYGROUND_HEIGHT_MM/1000.0)
+#define MAP_CELL_SIZE_MM 50 /**< Size of a cell in mm. The cell is a square. */
+#define MAP_CELL_SIZE_M (MAP_CELL_SIZE_MM/1000.0)
+#define MAP_WIDTH (PLAYGROUND_WIDTH_MM / MAP_CELL_SIZE_MM) /**< Field width*/
+#define MAP_HEIGHT (PLAYGROUND_HEIGHT_MM / MAP_CELL_SIZE_MM) /**< Field height*/
+#define MAP_PLAYGROUND_WIDTH_MM (MAP_WIDTH*MAP_CELL_SIZE_MM) /**< Playground width depends on width and cell size. */
+#define MAP_PLAYGROUND_WIDTH_M (MAP_PLAYGROUND_WIDTH_MM/1000.0)
+#define MAP_PLAYGROUND_HEIGHT_MM (MAP_HEIGHT*MAP_CELL_SIZE_MM) /**< Playground width depends on height and cell size. */
+#define MAP_PLAYGROUND_HEIGHT_M (MAP_PLAYGROUND_HEIGHT_MM/1000.0)
/**@}*/
/**
return PP_ERROR_MAP_NOT_INIT;
}
- add_safety_margin();
+ //add_safety_margin();
// If the goal map is not free, abort.
if (!ShmapIsFreePoint(xgoal_real, ygoal_real)) {
test_PROGRAMS += testpathplan
testpathplan_SOURCES = testpathplan.c
testpathplan_LIBS = pathplan rt map m rbtree shist
+
+test_PROGRAMS += testastar
+testastar_SOURCES = testastar.c
+testastar_LIBS = pathplan map m
+
--- /dev/null
+#define _ISOC99_SOURCE
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include "path_planner.h"
+#include <time.h>
+#include <sys/time.h>
+#include <math.h>
+#include <shist.h>
+
+#define OBST_COUNT 4
+
+void print_map(char *label)
+{
+ int x, y;
+ struct map *map = ShmapIsMapInit();
+
+ if (label)
+ printf("%s\n", label);
+
+ for (y=0; y<MAP_HEIGHT-1; y++) {
+ for (x=0; x<MAP_WIDTH; x++) {
+ char c;
+ if (ShmapIsFreeCell(x, y)) {
+ c = '.';
+ if (map->cells[y][x].flags & MAP_FLAG_PATH) {
+ c = 'p';
+ }
+ } else
+ c = '#';
+ putchar(c);
+ }
+ putchar('\n');
+ }
+}
+
+void randomize_obstacles(bool first)
+{
+ static struct obstacle {
+ double x1, y1, x2, y2;
+ } o[OBST_COUNT];
+ int i;
+ if (!first) {
+ for (i=0; i<OBST_COUNT; i++) {
+ ShmapSetRectangleFlag(o[i].x1, o[i].y1, o[i].x2, o[i].y2, 0, MAP_FLAG_WALL);
+ }
+ }
+ for (i=0; i<OBST_COUNT; i++) {
+ double x, y, w, h;
+ x = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ y = (rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0;
+ w = (rand()%1000)/1000.0;
+ h = (rand()%1000)/1000.0;
+ o[i].x1 = x-w/2;
+ o[i].y1 = y-h/2;
+ o[i].x2 = x+w/2;
+ o[i].y2 = y+h/2;
+/* printf("Obst%d (%5.2f,%5.2f) (%5.2f,%5.2f)\n", i, o[i].x1, o[i].y1, o[i].x2, o[i].y2); */
+/* printf("Obst%d (%5.2f,%5.2f) w=%5.2f h=%5.2f\n", i, x, y, w, h); */
+ ShmapSetRectangleFlag(o[i].x1, o[i].y1, o[i].x2, o[i].y2, MAP_FLAG_WALL, 0);
+ }
+ print_map("Obstacles:");
+}
+
+
+
+int main(int argc, char *argv[])
+{
+ double startx, starty, goalx, goaly, angle;
+ PathPoint * path;
+ int i, val;
+ FILE * data =NULL;
+
+ srand(time(0));
+
+ /* histogram parameters */
+ if (argc == 2) {
+ data = fopen( argv[1], "w" );
+ if( data )
+ printf( "Storing data at file : %s\n", argv[1] );
+ else
+ {
+ printf( "Open file failed : %s\n", argv[1] );
+ return 1;
+ }
+ }
+
+ // Init Shared Map Memory
+ ShmapInit(1);
+
+ // Create some obstacles
+ ShmapSetRectangleFlag(0.0, 0.0, 0.2, 2.1, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.2, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(0.0, 2.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(2.9, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
+ randomize_obstacles(true);
+ do {
+ startx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ starty = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+ } while (!ShmapIsFreePoint(startx, starty));
+
+ /* Main boucle */
+ for(i=1; i<2; i++){
+ if (i%100 == 0) {
+ randomize_obstacles(false);
+ }
+ do {
+ goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+ } while (!ShmapIsFreePoint(goalx, goaly));
+
+ val = path_planner(startx, starty, goalx, goaly , &path, &angle);
+
+ print_map("Found path:");
+
+ startx=goalx;starty=goaly;
+ }
+
+ //Close File
+ if (data){
+ if( !fclose(data) )
+ printf( "Data file closed : %s\n", argv[1] );
+ else
+ {
+ printf( "Error: file not closed : %s\n", argv[1] );
+ return 1;
+ }
+ }
+ // Free Memory
+ //ShmapFree();
+ return 0;
+}
+
+
-Subproject commit dcb49770b8aaa099920fd387b96203811788b8d7
+Subproject commit af05c4b6c7a564d7d4291978d00d677ffe8b1066
* (pecam1, 2009: not sure, if the sense is right for laser MCL;
* anyway, I hope, we will be happy without the laser)
*/
-const struct beacon_pos beacon_green[BEACON_CNT] = {
- { 3.062, -0.05}, /* EB2009: one side is 10mm only plexiglass */
- {-0.062, 1.05}, /* the rest is 22mm wood */
- { 3.062, 2.162},
-};
-
-const struct beacon_pos beacon_red[BEACON_CNT] = {
- /* beacons are rotated, not mirrored! */
- {-0.062, 2.162},
- { 3.062, 1.05},
- {-0.062, -0.05},
-};
const struct bonus_pos bonus[BONUS_CNT] = { //bonus[1].x
{975, 1575},
{1850, 1750},
{1850, 350},
{2200, 1400},
- {2200, 350},
- {200, 250}, // left dispensing
- {200, 550},
- {200, 850},
- {2800, 250}, // right dispensing
- {2800, 550},
- {2800, 850},
+ {2200, 350},
+ {200, 290}, // left dispensing
+ {200, 290 + 1*280},
+ {200, 290 + 2*280},
+ {2800, 290}, // right dispensing
+ {2800, 290 + 1*280},
+ {2800, 290 + 2*280},
};
const struct queen_pos queen[QUEEN_CNT] = {
- {200, 1450},
- {2800, 1450},
+ {200, 290 + 3*280},
+ {2800, 290 + 3*280},
};
const struct king_pos king[KING_CNT] = {
- {200, 1150},
- {2800, 1150},
+ {200, 290 + 4*280},
+ {2800, 290 + 4*280},
+};
+
+const struct square_center_red red_sq[SQ_CNTR] = {
+ {0.45 + 0.175 + 5*0.35, 5*0.35 + 0.175}, // 1
+ {0.45 + 0.175 + 5*0.35, 3*0.35 + 0.175}, // 2
+ {0.45 + 0.175 + 5*0.35, 1*0.35 + 0.175}, // 3
+ {0.45 + 0.175 + 4*0.35, 4*0.35 + 0.175}, // 4
+ {0.45 + 0.175 + 4*0.35, 2*0.35 + 0.175}, // 5
+ {0.45 + 0.175 + 3*0.35, 5*0.35 + 0.175}, // 6
+ {0.45 + 0.175 + 3*0.35, 3*0.35 + 0.175}, // 7
+ {0.45 + 0.175 + 3*0.35, 1*0.35 + 0.175}, // 8
+ {0.45 + 0.175 + 2*0.35, 4*0.35 + 0.175}, // 9
+ {0.45 + 0.175 + 2*0.35, 2*0.35 + 0.175}, // 10
+ {0.45 + 0.175 + 1*0.35, 5*0.35 + 0.175}, // 11
+ {0.45 + 0.175 + 1*0.35, 3*0.35 + 0.175}, // 12
+ {0.45 + 0.175 + 1*0.35, 1*0.35 + 0.175}, // 13
+ {0.45 + 0.175 + 0*0.35, 4*0.35 + 0.175}, // 14
+ {0.45 + 0.175 + 0*0.35, 2*0.35 + 0.175}, // 15
+};
+
+const struct square_center_blue blue_sq[SQ_CNTR] = {
+ {0.45 + 0.175 + 0*0.35, 5*0.35 + 0.175}, // 1
+ {0.45 + 0.175 + 0*0.35, 3*0.35 + 0.175}, // 2
+ {0.45 + 0.175 + 0*0.35, 1*0.35 + 0.175}, // 3
+ {0.45 + 0.175 + 1*0.35, 4*0.35 + 0.175}, // 4
+ {0.45 + 0.175 + 1*0.35, 2*0.35 + 0.175}, // 5
+ {0.45 + 0.175 + 2*0.35, 5*0.35 + 0.175}, // 6
+ {0.45 + 0.175 + 2*0.35, 3*0.35 + 0.175}, // 7
+ {0.45 + 0.175 + 2*0.35, 1*0.35 + 0.175}, // 8
+ {0.45 + 0.175 + 3*0.35, 4*0.35 + 0.175}, // 9
+ {0.45 + 0.175 + 3*0.35, 2*0.35 + 0.175}, // 10
+ {0.45 + 0.175 + 4*0.35, 5*0.35 + 0.175}, // 11
+ {0.45 + 0.175 + 4*0.35, 3*0.35 + 0.175}, // 12
+ {0.45 + 0.175 + 4*0.35, 1*0.35 + 0.175}, // 13
+ {0.45 + 0.175 + 5*0.35, 4*0.35 + 0.175}, // 14
+ {0.45 + 0.175 + 5*0.35, 2*0.35 + 0.175}, // 15
};
+// const struct beacon_pos beacon_green[BEACON_CNT] = {
+// { 3.062, -0.05}, /* EB2009: one side is 10mm only plexiglass */
+// {-0.062, 1.05}, /* the rest is 22mm wood */
+// { 3.062, 2.162},
+// };
+//
+// const struct beacon_pos beacon_red[BEACON_CNT] = {
+// /* beacons are rotated, not mirrored! */
+// {-0.062, 2.162},
+// { 3.062, 1.05},
+// {-0.062, -0.05},
+// };
/*
* FIXME: update robot's dimensions !!!
* and update the pitcture
*
- * ROBOT DIMENSIONS
+ * ROBOT DIMENSIONS FOR COMPETITION 2011, Play Chess!
*
- * ^ +--------------------------+
- * | | | | |
- * ^ | | ------- |
- * RR| | | : |
- * v | | : __ |
- * W| | + Center |__| |
- * | | AB : AF HOK |
- * | |<---->:<----------------->|
- * | | ------- |
- * | | | | |
- * v +--------------------------+
- * <-------> <-->
- * V WR
- *
- * Sx - Sharp sensor
+ * ^ +--------------------------+@------\
+ * | | | | | \
+ * ^ | | ------- | .....
+ * RR| | | HOK : | . .
+ * v | | __ : | . .
+ * W| | |__| |. .
+ * | | AF : AB | . .
+ * | |<---->:<----------------->| . .
+ * | | ------- | .......
+ * | | | | | /
+ * v +--------------------------+@-------/
+ * <<= <--> <--------->
+ * direction WR JA
+ * of motion <------------>
+ * PW
*/
/* FIXME update robot's dimensions!!! */
-#define ROBOT_WIDTH_MM 280 /* W*/
+#define ROBOT_WIDTH_MM 310.0 /* W*/
#define ROBOT_WIDTH_M (ROBOT_WIDTH_MM/1000.0)
-#define ROBOT_ROTATION_RADIUS_MM ((188)/2) /* RR */
+#define ROBOT_ROTATION_RADIUS_MM ((230.0)/2.0) /* RR */
#define ROBOT_ROTATION_RADIUS_M (ROBOT_ROTATION_RADIUS_MM/1000.0)
-#define ROBOT_WHEEL_RADIUS_MM 38 /* WR */ // FIXME!! (this doesn't seem right to me .. Filip)
+#define ROBOT_WHEEL_RADIUS_MM 38.0 /* WR */
#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM/1000.0)
-#define ROBOT_AXIS_TO_BACK_MM 230 /* AB */
+#define ROBOT_AXIS_TO_BACK_MM 50.0 /* AB */
#define ROBOT_AXIS_TO_BACK_M (ROBOT_AXIS_TO_BACK_MM/1000.0)
-#define ROBOT_AXIS_TO_FRONT_MM 65 /* AF */
+#define ROBOT_AXIS_TO_FRONT_MM 210.0 /* AF */
#define ROBOT_AXIS_TO_FRONT_M (ROBOT_AXIS_TO_FRONT_MM/1000.0)
-#define ROBOT_AXIS_TO_BRUSH_MM 50 /* ABE */ // FIXME: update the value and rename (no brushes)
-#define ROBOT_AXIS_TO_BRUSH_M (ROBOT_AXIS_TO_BRUSH_MM/1000.0)
-#define HOKUYO_CENTER_OFFSET_MM 35 // FIXME
+#define ROBOT_HEIGHT_MM (ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM)
+#define ROBOT_HEIGHT_M (ROBOT_HEIGHT_MM / 1000.0)
+
+#define HOKUYO_CENTER_OFFSET_MM 180.0
#define HOKUYO_CENTER_OFFSET_M (HOKUYO_CENTER_OFFSET_MM/1000.0)
-#define ODOMETRY_WHEEL_RADIUS_MM 30
+#define HOKUYO_RANGE_ANGLE_LEFT 70.0 /* view angle in degrees from center axis */
+#define HOKUYO_RANGE_ANGLE_RIGHT 70.0
+#define HOKUYO_ORIENTATION (1) /* 1 = screws up, -1 = screws down */
+
+#define ODOMETRY_WHEEL_RADIUS_MM 30.0
#define ODOMETRY_WHEEL_RADIUS_M (ODOMETRY_WHEEL_RADIUS_MM/1000.0)
-#define ODOMETRY_ROTATION_RADIUS_MM (246/2)
+#define ODOMETRY_ROTATION_RADIUS_MM (284.0/2.0)
#define ODOMETRY_ROTATION_RADIUS_M (ODOMETRY_ROTATION_RADIUS_MM/1000.0)
-#define ROBOT_VIDLE_LENGTH_M 0.2 /* V */ /* FIXME: Measure the right value */
+
+#define ROBOT_JAWS_LENGHT_MM 130.0 /* JA */
+#define ROBOT_JAWS_LENGHT_M (ROBOT_JAWS_LENGHT_MM/1000.0)
+#define ROBOT_LENGHT_WITH_PANWS_MM 170.0 /* PW */
+#define ROBOT_LENGHT_WITH_PAWNS_M (ROBOT_LENGHT_WITH_PANWS_MM/1000.0)
+
+#define ROBOT_AXIS_TO_FIGURE_CENTER_MM 300.0
+#define ROBOT_AXIS_TO_FIGURE_CENTER_M (ROBOT_AXIS_TO_FIGURE_CENTER_MM/1000.0)
+
+#define ROBOT_START_X_MM ROBOT_AXIS_TO_BACK_MM
+#define ROBOT_START_X_M (ROBOT_START_X_MM / 1000.0)
+#define ROBOT_START_Y_MM (PLAYGROUND_HEIGHT_MM - (ROBOT_WIDTH_MM / 2.0))
+#define ROBOT_START_Y_M (ROBOT_START_Y_MM / 1000.0)
+#define ROBOT_START_ANGLE_DEG 0
+
+/* maxon motor parameters ratio */
+/* DO NOT change this if not shure what you are doing !!! */
+#define ROBOT_MOTOR_GEARBOX_RATIO 29.0
+#define ROBOT_MOTOR_IRC_RESOLUTION 512.0
/**
* PLAYGROUND DIMENSIONS
- *
+ *
* S2 R3
* +---------------------------------+---------------------------------+
* ^ | | [W,H]|
*/
#define PLAYGROUND_WIDTH_MM 3000
#define PLAYGROUND_WIDTH_M (PLAYGROUND_WIDTH_MM/1000.0)
-#define PLAYGROUND_HEIGHT_MM 2100
+#define PLAYGROUND_HEIGHT_MM 2000
#define PLAYGROUND_HEIGHT_M (PLAYGROUND_HEIGHT_MM/1000.0)
#define SLOPE_TO_RIM_MM 740
#define BLOCK_WIDTH_MM 400
#define BLOCK_HEIGHT_MM 22
-#define STARTAREA_WIDTH_MM 400
-#define STARTAREA_HEIGHT_MM 400
+#define STARTAREA_WIDTH_MM 500
+#define STARTAREA_HEIGHT_MM 500
#define DISPENSING_WIDTH_MM 400
#define DISPENSING_HEIGHT_MM 1678
/* queen */
#define QUEEN_CNT 2
+#define SQ_CNTR 15
+
+
struct beacon_pos {
float x, y;
};
int x, y;
};
+struct square_center_red {
+ float x, y;
+};
+
+struct square_center_blue {
+ float x, y;
+};
+
extern const struct beacon_pos beacon_green[BEACON_CNT];
extern const struct beacon_pos beacon_red[BEACON_CNT];
extern const struct bonus_pos bonus[BONUS_CNT];
extern const struct king_pos king[KING_CNT];
extern const struct queen_pos queen[QUEEN_CNT];
+extern const struct square_center_red red_sq[SQ_CNTR];
+extern const struct square_center_blue blue_sq[SQ_CNTR];
+
/*
* Position of Shapr sensors on the robot with respect to the robot center
* (not used in EB2009)
robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
bin_PROGRAMS += competition
-competition_SOURCES = competition.cc common-states.cc \
- strategy_opp_corn.cc strategy_opp_oranges.cc \
- strategy_12_oranges.cc strategy_six_oranges.cc
+competition_SOURCES = competition2012.cc \
+ common-states.cc strategy_get_central_buillon.cc \
+ strategy_homologation.cc strategy_odo_calibration.cc
bin_PROGRAMS += homologation
-homologation_SOURCES = homologation.cc
+homologation_SOURCES = homologation2012.cc
# Library with general support functions for the robot
lib_LIBRARIES += robot
robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \
- motion-control.cc map_handling.c \
- match-timing.c eb2010misc.cc
+ motion-control.cc map_handling.cc \
+ match-timing.c
+
robot_GEN_SOURCES = roboevent.c
include_GEN_HEADERS += roboevent.h
lib_LIBRARIES += actlib
actlib_SOURCES = actuators.c
-lib_LIBRARIES += cornslib
-cornslib_SOURCES = corns_configs.c
-
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m \
orte pathplan sharp map fsm rbtree motion robodim \
- actlib cornslib ulut
+ actlib ulut shape_detect
# Automatic generation of event definition files
include-pass_HOOKS = roboevent.c roboevent.h
static struct robottype_orte_data *orte = NULL;
+static uint16_t jaw_left_last_request;
+static uint16_t jaw_right_last_request;
+static uint16_t lift_last_request;
+
void act_init(struct robottype_orte_data *ortedata)
{
orte = ortedata;
-}
-
-// FIXME: obsolete (?)
-void act_hokuyo(unsigned char angle)
-{
- orte->hokuyo_pitch.angle = angle;
- ORTEPublicationSend(orte->publication_hokuyo_pitch);
+ act_jaws(OPEN);
+ act_lift(0, 0, 1);
}
void act_camera_on(void)
ORTEPublicationSend(orte->publication_camera_control);
}
-static uint16_t vidle_last_request;
-
-void act_vidle(uint16_t position, char speed)
+void act_lift(uint16_t req_pos, char speed, char homing)
{
- orte->vidle_cmd.req_pos = position;
- orte->vidle_cmd.speed = speed;
+
+ orte->lift_cmd.req_pos = req_pos;
+ orte->lift_cmd.speed = speed;
+ orte->lift_cmd.homing = homing;
/* Remember the request so that we change check for matching
* response in rcv_vidle_status_cb() */
- vidle_last_request = position;
- ORTEPublicationSend(orte->publication_vidle_cmd);
+ lift_last_request = req_pos;
+ ORTEPublicationSend(orte->publication_lift_cmd);
+}
+
+void act_jaws(jaws_cmds cmd)
+{
+ switch (cmd) {
+ case OPEN:
+ orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
+ usleep(300000);
+ orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
+ break;
+ case CLOSE:
+ orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE;
+ usleep(300000);
+ orte->jaws_cmd.req_pos.right = JAW_RIGHT_CLOSE;
+ break;
+ case CATCH:
+ orte->jaws_cmd.req_pos.left = JAW_LEFT_CATCH;
+ orte->jaws_cmd.req_pos.right = JAW_RIGHT_CATCH;
+ break;
+ default:
+ orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
+ orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
+ }
+}
+
+uint16_t act_jaw_left_get_last_reqest(void)
+{
+ return jaw_left_last_request;
+}
+
+uint16_t act_jaw_right_get_last_reqest(void)
+{
+ return jaw_right_last_request;
}
-uint16_t act_vidle_get_last_reqest(void)
+uint16_t act_lift_get_last_reqest(void)
{
- return vidle_last_request;
+ return lift_last_request;
}
#define HOKUYO_PITCH_HORIZONTAL 0xC0
#define HOKUYO_PITCH_MIN 0x00
-#define VIDLE_UP 0x3c0
-#define VIDLE_MIDDLE 0x250
-#define VIDLE_LOAD_PREPARE 0x178
-#define VIDLE_DOWN 0x170
+#include <stdint.h>
-#define VIDLE_FAST_SPEED 0x00
-#define VIDLE_MEDIUM_SPEED 0x0a
+typedef enum {
+ OPEN,
+ CLOSE,
+ CATCH
+} jaws_cmds;
-#include <stdint.h>
+#define JAW_LEFT_OPEN 0xff
+#define JAW_RIGHT_OPEN 0x40
+
+#define JAW_LEFT_CLOSE 0x00
+#define JAW_RIGHT_CLOSE 0xff
+
+#define JAW_LEFT_CATCH 0x80
+#define JAW_RIGHT_CATCH 0xb0
+
+#define UP 0x140
+#define DOWN 0x0
#ifdef __cplusplus
extern "C" {
void act_init(struct robottype_orte_data *ortedata);
-void act_hokuyo(unsigned char angle); // FIXME obsolete (?)
void act_camera_on(void);
void act_camera_off(void);
-void act_vidle(uint16_t position, char speed);
-uint16_t act_vidle_get_last_reqest(void);
+void act_lift(uint16_t req_pos, char speed, char homing);
+void act_jaws(jaws_cmds cmd);
+
+uint16_t act_jaw_left_get_last_reqest(void);
+uint16_t act_jaw_right_get_last_reqest(void);
+uint16_t act_lift_get_last_reqest(void);
#ifdef __cplusplus
}
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
#include "actuators.h"
+#include <sharp.h>
#include <trgen.h>
#include "match-timing.h"
-#include "eb2010misc.h"
#include <stdbool.h>
#include <ul_log.h>
name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
-static void set_initial_position()
+void set_initial_position()
{
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+ 0);
}
-static void actuators_home()
+void actuators_home()
{
- static int tmp = 0;
- act_vidle(VIDLE_UP - tmp, VIDLE_FAST_SPEED);
- tmp = 1 - tmp; // Force movement (we need to change the target position)
+ act_jaws(CLOSE);
+
+// bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
+ // drive lift to home position
+ act_lift(0, 0, 1);
+ // unset the homing request
+ //act_lift(0, 0, 0);
}
void start_entry()
robot.check_turn_safety = false;
pthread_create(&thid, NULL, timing_thread, NULL);
start_timer();
- act_camera_on();
}
// We set initial position periodically in order for it to be updated
void start_exit()
{
- robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
- act_camera_off();
+
}
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
+bool read_sharp()
+{
+ int sharp_data = robot.orte.jaws_status.act_pos.left;
+ int sharp_dist = 0;
+ sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
+ printf("sharp data: %dmm\n", sharp_dist);
+ return (sharp_dist <= 150 ? true : false);
+}
+
+void inline enable_bumpers(bool enabled)
+{
+ robot.use_left_bumper = enabled;
+ robot.use_right_bumper = enabled;
+ robot.use_back_bumpers = enabled;
+}
-struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
-#define VIDLE_TIMEOUT 2000
/************************************************************************
- * States that form the "collect some oranges" subautomaton. Calling automaton
- * SHOULD ALWAYS call the "approach_the_slope" state.
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
-bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) {
- bool ret;
- if (which_slope == MINE) {
- ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
- } else if (which_slope == OPPONENTS) {
- ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
- } else {
- ul_logfatal("ERROR: unknown side;");
-#warning Remove the next line
- robot_exit();
- }
- return ret;
-}
-
-static struct slope_approach_style *slope_approach_style_p;
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
-/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
-FSM_STATE(approach_the_slope)
+FSM_STATE(approach_central_buillon)
{
switch(FSM_EVENT) {
- case EV_ENTRY: {
- slope_approach_style_p = (struct slope_approach_style *) FSM_EVENT_PTR;
- if (slope_approach_style_p == NULL) {
- ul_logfatal("\n\nit is not allowed to call the approach_the_slope state with NULL data!!\n\n");
-#warning remove the next line
- robot_exit();
- }
- double x, y, phi;
- robot_get_est_pos_trans(&x, &y, &phi);
- /* decide */
- bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(slope_approach_style_p->which_side, x, y);
- /* if necessary, approach the slope */
- if (ready_to_climb_the_slope) {
- FSM_TRANSITION(climb_the_slope);
- } else {
- robot_goto_trans(
- x_coord(0.3, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
- ARRIVE_FROM(DEG2RAD(0), 0.02),
- &tcFast);
- }
- break;
- }
+ case EV_ENTRY:
+ robot_trajectory_new(&tcVeryFast); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.6,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 1.2);
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 0.7);
+ robot_trajectory_add_final_point_trans(
+ 1.0,
+ 0.45,
+ ARRIVE_FROM(DEG2RAD(24),0.1));
+// robot_trajectory_add_final_point_trans(
+// 1.3,
+// 0.58,
+// NO_TURN());
+ break;
case EV_MOTION_DONE:
- FSM_TRANSITION(climb_the_slope);
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
break;
- case EV_START:
case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ FSM_TRANSITION(catch_central_buillon);
+ break;
+ default:
break;
}
}
-void inline enable_switches(bool enabled)
+FSM_STATE(catch_central_buillon)
{
- robot.use_left_switch = enabled;
- robot.use_right_switch = enabled;
- robot.use_back_switch = enabled;
-}
-
-FSM_STATE(climb_the_slope)
-{
- struct TrajectoryConstraints tc;
switch(FSM_EVENT) {
- case EV_ENTRY: {
- // disables using side switches on bumpers when going up
- enable_switches(false);
- robot.ignore_hokuyo = true;
- /* create the trajectory and go */
- tc = tcSlow;
- tc.maxacc = 0.4;
- robot_trajectory_new_backward(&tc);
- if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
- act_vidle(VIDLE_LOAD_PREPARE, 5);
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
- NO_TURN());
- } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
- FSM_TIMER(3800);
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85));
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
- //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
- NO_TURN());
- }
- break;
- }
- case EV_MOTION_DONE:
- SUBFSM_TRANSITION(load_oranges, NULL);
+ case EV_ENTRY:
+ robot_trajectory_new(&tcFast); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ 1.3,
+ 0.58,
+ NO_TURN());
break;
- case EV_RETURN:
- FSM_TRANSITION(sledge_down);
+ case EV_MOTION_DONE:
+ act_jaws(CATCH);
+ FSM_TIMER(500);
break;
case EV_TIMER:
- act_vidle(VIDLE_LOAD_PREPARE, 15);
+ FSM_TRANSITION(leave_central_buillon);
break;
- case EV_START:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ default:
break;
}
}
-/* subautomaton to load oranges in two stages */
-FSM_STATE_DECL(load_oranges2);
-FSM_STATE_DECL(load_oranges3);
-FSM_STATE(load_oranges)
+FSM_STATE(leave_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- FSM_TIMER(1000);
- act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
- break;
- case EV_VIDLE_DONE:
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(load_oranges2);
+ robot_trajectory_new_backward(&tcFast); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ 0.85,
+ 0.38,
+ NO_TURN());
break;
case EV_MOTION_DONE:
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ break;
+ default:
break;
}
}
-FSM_STATE(load_oranges2)
+FSM_STATE(push_bottle_bw)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- act_vidle(VIDLE_UP, VIDLE_MEDIUM_SPEED);
- FSM_TIMER(1000);
- break;
- case EV_VIDLE_DONE:
- FSM_TRANSITION(load_oranges3);
- //SUBFSM_RET(NULL);
- break;
- case EV_TIMER:
- FSM_TRANSITION(load_oranges3);
- //SUBFSM_RET(NULL);
+ robot.use_back_bumpers = false;
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.7,
+ 0.3);
+ robot_trajectory_add_final_point_trans(
+ 0.64+0.08,
+ ROBOT_AXIS_TO_BACK_M + 0.01,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- act_vidle(VIDLE_UP-1, VIDLE_FAST_SPEED);
+ FSM_TRANSITION(return_home);
+ break;
+ default:
break;
}
}
-FSM_STATE(load_oranges3)
+FSM_STATE(return_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- act_vidle(VIDLE_MIDDLE+50, 0);
- FSM_TIMER(500);
+ robot_trajectory_new(&tcFast); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64 + 0.08,
+ 0.8);
+ robot_trajectory_add_final_point_trans(
+ 0.55,
+ 1.1,
+ ARRIVE_FROM(DEG2RAD(180), 0.01));
break;
- case EV_VIDLE_DONE:
- SUBFSM_RET(NULL);
+ case EV_MOTION_DONE:
+ robot.use_back_bumpers = true;
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
break;
case EV_TIMER:
SUBFSM_RET(NULL);
break;
- case EV_MOTION_DONE:
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ default:
break;
}
}
-FSM_STATE(sledge_down)
-{
- struct TrajectoryConstraints tc;
+FSM_STATE(leave_home)
+{
switch(FSM_EVENT) {
case EV_ENTRY:
- tc = tcFast;
- tc.maxe = 0.5;
- robot_trajectory_new(&tc);
+ //robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_move_by(-0.18, NO_TURN(), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TRANSITION(leave_home_for_totem);
+ break;
+ default:
+ break;
+ }
+}
- if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
- robot_trajectory_add_point_trans(
- x_coord(1.2 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
- robot_trajectory_add_point_trans(
- x_coord(1.0 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.01);
- robot_trajectory_add_point_trans(
- x_coord(0.8 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05);
- robot_trajectory_add_point_trans(
- x_coord(0.6 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
+FSM_STATE(leave_home_for_totem)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcFast); // new trajectory
+ if(up) {
robot_trajectory_add_final_point_trans(
- x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.19,
+ 0.7,
+ 1.3,
NO_TURN());
- } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
- robot_trajectory_add_point_trans(
- x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85));
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
+ }
+ else {
robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.2, slope_approach_style_p->which_side),
- //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
+ 0.7,
+ 0.9,
NO_TURN());
}
break;
case EV_MOTION_DONE:
- /* just for sure, try to close it one more time */
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- SUBFSM_RET(NULL);
- delete(slope_approach_style_p);
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- // enables using side switches on bumpers
- enable_switches(true);
- robot.ignore_hokuyo = false;
- robot.check_turn_safety = true;
-
+ default:
break;
}
}
-/************************************************************************
- * The "unload our oranges" subautomaton
- ************************************************************************/
-
-FSM_STATE(to_cntainer_and_unld)
+FSM_STATE(approach_totem_down)
{
- TrajectoryConstraints tcNotSoFast = trajectoryConstraintsDefault;
- tcNotSoFast.maxv = 0.5; tcNotSoFast.maxacc = 0.3; tcNotSoFast.maxomega = 2;
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcNotSoFast);
+ robot_trajectory_new(&tcFast); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 0.4,
+ TURN_CCW(DEG2RAD(90)));
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000); // FIXME: test this
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ act_lift(UP, 0, 0);
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
break;
case EV_TIMER:
- FSM_TRANSITION(jerk);
+ FSM_TRANSITION(catch_totem_buillon_down);
break;
- case EV_START:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ default:
break;
}
}
-
-FSM_STATE(jerk)
+
+FSM_STATE(catch_totem_buillon_down)
{
- static char move_cnt;
switch(FSM_EVENT) {
case EV_ENTRY:
- move_cnt = 0;
- robot_move_by(-0.05, NO_TURN(), &tcJerk);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+ ARRIVE_FROM(DEG2RAD(90), 0.10));
break;
case EV_MOTION_DONE:
- if (move_cnt == 0) {
- robot_move_by(0.05, NO_TURN(), &tcJerk);
- } else if (move_cnt > 0) {
- FSM_TIMER(1500); // FIXME: test this
- }
- move_cnt++;
+ act_jaws(CATCH);
+ FSM_TIMER(500);
break;
case EV_TIMER:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- SUBFSM_RET(NULL);
- break;
- case EV_START:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ FSM_TRANSITION(leave_totem_down);
+ default:
break;
}
}
-/************************************************************************
- * The "collect corns" subautomaton
- ************************************************************************/
-
-static enum where_to_go {
- CORN,
- TURN_AROUND,
- CONTAINER,
- NO_MORE_CORN
-} where_to_go = CORN;
-
-static struct corn *corn_to_get;
-
-FSM_STATE(rush_corns_decider)
+FSM_STATE(leave_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- if (where_to_go == CORN) {
- FSM_TRANSITION(approach_next_corn);
- } else if (where_to_go == CONTAINER) {
- FSM_TRANSITION(rush_the_corn);
- } else if (where_to_go == TURN_AROUND) {
- FSM_TRANSITION(turn_around);
- } else /* NO_MORE_CORN */ {
- }
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 0.42,
+ NO_TURN());
break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
case EV_MOTION_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ act_jaws(OPEN);
+ act_lift(DOWN, 0, 0);
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(place_buillon_home);
+ break;
+ default:
break;
}
}
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
+FSM_STATE(place_buillon_home)
{
switch(FSM_EVENT) {
- case EV_ENTRY: {
- double x, y, phi;
- robot_get_est_pos(&x, &y, &phi);
- ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
- cnt, x, y, phi);
-
- corn_to_get = choose_next_corn();
- if (corn_to_get) {
- Pos *p = get_corn_approach_position(corn_to_get);
- corn_to_get->was_collected = true;
- robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
- delete(p);
- where_to_go = CONTAINER;
- } else {
- where_to_go = NO_MORE_CORN;
- }
- break;
+ case EV_ENTRY:
+ act_jaws(CATCH);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ if(up) {
+ robot_trajectory_add_point_trans(
+ 0.9,
+ 1.52);
}
+ else {
+ robot_trajectory_add_point_trans(
+ 0.9,
+ 0.48);
+ robot_trajectory_add_point_trans(
+ 0.7,
+ 0.8);
+ }
+ robot_trajectory_add_final_point_trans(
+ 0.55,
+ 1.1,
+ ARRIVE_FROM(DEG2RAD(180),0.01));
+ break;
case EV_MOTION_DONE:
- cnt++;
- FSM_TRANSITION(rush_corns_decider);
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
break;
- case EV_START:
case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(rush_the_corn)
+FSM_STATE(approach_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- double x;
- if (robot.team_color == BLUE) {
- x = corn_to_get->position.x;
- } else {
- x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
- }
- remove_wall_around_corn(x, corn_to_get->position.y);
- robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcSlow);
- where_to_go = TURN_AROUND;
+ robot_trajectory_new(&tcFast); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 1.6,
+ TURN_CW(DEG2RAD(270)));
break;
case EV_MOTION_DONE:
- FSM_TRANSITION(rush_last_cms);
+ act_jaws(OPEN);
+ act_lift(UP, 0, 0);
+ FSM_TIMER(2000);
break;
- case EV_START:
case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ FSM_TRANSITION(catch_totem_buillon_up);
+ break;
+ default:
break;
}
}
-FSM_STATE(rush_last_cms)
+FSM_STATE(catch_totem_buillon_up)
{
- static char move_cnt;
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.ignore_hokuyo = true;
- robot.check_turn_safety = false;
- move_cnt = 0;
- robot_move_by(0.08, NO_TURN(), &tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
- if (move_cnt == 0) {
- robot_move_by(-0.08, NO_TURN(), &tcSlow);
- } else if (move_cnt > 0) {
- FSM_TRANSITION(rush_corns_decider);
- }
- move_cnt++;
+ act_jaws(CATCH);
+ FSM_TIMER(500);
break;
- case EV_START:
case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- robot.ignore_hokuyo = false;
- robot.check_turn_safety = true;
+ FSM_TRANSITION(leave_totem_up);
+ break;
+ default:
break;
}
}
-// used to perform the maneuvre
-FSM_STATE(turn_around)
+FSM_STATE(leave_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 1.6,
+ NO_TURN());
break;
case EV_MOTION_DONE:
- where_to_go = CORN;
- FSM_TRANSITION(rush_corns_decider);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ act_jaws(OPEN);
+ act_lift(DOWN, 0, 0);
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
+
+
+FSM_STATE(push_second_bottle_fw)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcFast); // new trajectory
+// robot_trajectory_add_point_trans(0.7, 0.5);
+ robot_goto_notrans(
+ 1.85,
+ ROBOT_AXIS_TO_FRONT_M,
+ ARRIVE_FROM(DEG2RAD(270), 0.15), &tcFast);
+ break;
+ case EV_MOTION_DONE:
+ SUBFSM_RET(NULL);
+ default:
+ break;
+ }
+}
+
+FSM_STATE(go_back_from_home)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ act_jaws(OPEN);
+ robot_move_by(-0.2, NO_TURN(), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TRANSITION(push_second_bottle_fw);
+ break;
+ default:
+ break;
+ }
+}
\ No newline at end of file
#include "roboevent.h"
#include <fsm.h>
-extern struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
-
-FSM_STATE_DECL(start_six_oranges);
-FSM_STATE_DECL(start_12_oranges);
-FSM_STATE_DECL(start_opp_corn);
-FSM_STATE_DECL(start_opp_oranges);
-
-
-FSM_STATE_DECL(climb_the_slope);
-FSM_STATE_DECL(load_oranges);
-FSM_STATE_DECL(sledge_down);
-FSM_STATE_DECL(to_cntainer_and_unld);
-FSM_STATE_DECL(jerk);
-FSM_STATE_DECL(rush_corns_decider);
-FSM_STATE_DECL(approach_next_corn);
-FSM_STATE_DECL(rush_the_corn);
-FSM_STATE_DECL(rush_last_cms);
-FSM_STATE_DECL(turn_around);
-FSM_STATE_DECL(approach_the_slope);
+extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+extern double totem_x, totem_y;
+extern bool up;
+/* strategy FSM */
+FSM_STATE_DECL(central_buillon_wait_for_start);
+FSM_STATE_DECL(homologation_wait_for_start);
+FSM_STATE_DECL(calibrate);
+
+/* Strategy catch buillon in center */
+FSM_STATE_DECL(approach_central_buillon);
+FSM_STATE_DECL(catch_central_buillon);
+FSM_STATE_DECL(leave_central_buillon);
+FSM_STATE_DECL(push_bottle_bw);
+FSM_STATE_DECL(return_home);
+
+/* Ignore central buillon */
+//FSM_STATE_DECL(push_nearest_buillon);
+//FSM_STATE_DECL(push_bottle_fw);
+
+/* Autocalibration */
+FSM_STATE_DECL(go_back_for_cal);
+
+/* Common states */
+FSM_STATE_DECL(leave_home);
+FSM_STATE_DECL(leave_home_for_totem);
+FSM_STATE_DECL(approach_totem_down);
+FSM_STATE_DECL(catch_totem_buillon_down);
+FSM_STATE_DECL(leave_totem_down);
+FSM_STATE_DECL(place_buillon_home);
+FSM_STATE_DECL(approach_totem_up);
+FSM_STATE_DECL(catch_totem_buillon_up);
+FSM_STATE_DECL(leave_totem_up);
+
+/*FSM_STATE_DECL(place_down_buillon);
+FSM_STATE_DECL(approach_totem_up);
+FSM_STATE_DECL(catch_up_totem_buillon);
+FSM_STATE_DECL(leave_totem_up);
+FSM_STATE_DECL(place_up_buillon);
+
+FSM_STATE_DECL(push_second_bottle);
+*/
+/* HOMOLOGATION states */
+/* movement states - buillon */
+FSM_STATE_DECL(homologation_approach_buillon);
+/* Pushing the bottle */
+FSM_STATE_DECL(homologation_push_bottle);
+FSM_STATE_DECL(push_second_bottle_fw);
+FSM_STATE_DECL(go_back_from_home);
+
void start_entry();
void start_timer();
void start_go();
void start_exit();
+bool read_sharp();
+
+void robot_calibrate_odometry();
+
+
+
#endif
/*
* competition.cc 2010/04/30
- *
+ *
* Robot's control program intended for homologation (approval phase) on Eurobot 2009.
*
* Copyright: (c) 2009 - 2010 CTU Dragons
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
#include "actuators.h"
#include "match-timing.h"
-#include "eb2010misc.h"
#include "common-states.h"
int main()
robot.fsm.motion.debug_states = 1;
//robot.fsm.act.debug_states = 1;
- //robot.fsm.main.state = &fsm_state_main_start_opp_corn;
- //robot.fsm.main.state = &fsm_state_main_start_opp_oranges;
- robot.fsm.main.state = &fsm_state_main_start_six_oranges;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_all_our_figures;
+ robot.fsm.main.state = &fsm_state_main_start_pick_two_our_figures;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_fourth_figure;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_third_figure;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_center_figure;
//robot.fsm.main.transition_callback = trans_callback;
//robot.fsm.motion.transition_callback = move_trans_callback;
- tcJerk = trajectoryConstraintsDefault;
- tcJerk.maxv = 1.5;
- tcJerk.maxacc = 5;
- tcJerk.maxomega = 2;
+ tcVeryFast = trajectoryConstraintsDefault;
+ tcVeryFast.maxv = 1;
+ tcVeryFast.maxacc = 0.6;
+ tcVeryFast.maxomega = 2;
tcFast = trajectoryConstraintsDefault;
- tcFast.maxv = 1;
- tcFast.maxacc = 0.3;
- tcFast.maxomega = 2;
+ tcFast.maxv = 0.6;
+ tcFast.maxacc = 0.2;
+ tcFast.maxomega = 1;
+ tcFast.maxe = 0.02;
tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.3;
- tcSlow.maxacc = 0.1;
+ tcSlow.maxv = 0.4;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 1;
+ tcSlow.maxe = 0.02;
tcVerySlow = trajectoryConstraintsDefault;
- tcVerySlow.maxv = 0.05;
- tcVerySlow.maxacc = 0.05;
+ tcVerySlow.maxv = 0.1;
+ tcVerySlow.maxacc = 0.1;
+ tcVerySlow.maxomega = 0.2;
+ tcVerySlow.maxe = 0.02;
rv = robot_start();
if (rv) error(1, errno, "robot_start() returned %d\n", rv);
--- /dev/null
+ /*
+ * competition.cc 12/02/28
+ *
+ * Robot's control program intended for competition on Eurobot 2012.
+ *
+ * Copyright: (c) 2012 CTU Flamingos
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <trgen.h>
+#include "match-timing.h"
+#include <ul_log.h>
+#include "common-states.h"
+
+UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
+
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
+int main()
+{
+ int rv;
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ robot.obstacle_avoidance_enabled = true;
+
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ //robot.fsm.act.debug_states = 1;
+
+ robot.fsm.main.state = &fsm_state_main_central_buillon_wait_for_start;
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ tcVeryFast = trajectoryConstraintsDefault;
+ tcVeryFast.maxv = 1;
+ tcVeryFast.maxacc = 0.6;
+ tcVeryFast.maxomega = 1;
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 0.6;
+ tcFast.maxacc = 0.4;
+ tcFast.maxomega = 1;
+ tcFast.maxe = 0.02;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.4;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 1;
+ tcSlow.maxe = 0.02;
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.1;
+ tcVerySlow.maxacc = 0.1;
+ tcVerySlow.maxomega = 0.2;
+ tcVerySlow.maxe = 0.02;
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+ robot_destroy();
+
+ return 0;
+}
\ No newline at end of file
{
switch (FSM_EVENT) {
case EV_ENTRY:
- if (robot.localization_works) {
- robot_pos_type p;
- ROBOT_LOCK(est_pos_uzv);
- p = robot.est_pos_uzv;
- ROBOT_UNLOCK(est_pos_uzv);
-
- robot_set_est_pos_notrans(p.x, p.y, p.phi);
- }
break;
case EV_TRAJECTORY_DONE:
// Skip close to target because sometimes it turn the robot to much
}
break;
case EV_OBSTACLE_BEHIND:
- if (robot.moves_backward && robot.use_back_switch)
+ if (robot.moves_backward && robot.use_back_bumpers &&
+ (robot.orte.robot_bumpers.bumper_left_across ||
+ robot.orte.robot_bumpers.bumper_right_across ||
+ robot.orte.robot_bumpers.bumper_rear_left ||
+ robot.orte.robot_bumpers.bumper_rear_right))
FSM_TRANSITION(wait_and_try_again);
break;
case EV_OBSTACLE_SIDE:
- if ((robot.use_left_switch && robot.orte.robot_switches.bumper_left) ||
- (robot.use_right_switch && robot.orte.robot_switches.bumper_right))
+ if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
+ (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
FSM_TRANSITION(wait_and_try_again);
break;
case EV_TRAJECTORY_LOST:
FSM_TRANSITION(wait_for_command);
break;
case EV_OBSTACLE_BEHIND:
- if (robot.moves_backward && robot.use_back_switch)
+ if (robot.moves_backward && robot.use_back_bumpers &&
+ (robot.orte.robot_bumpers.bumper_left_across ||
+ robot.orte.robot_bumpers.bumper_right_across ||
+ robot.orte.robot_bumpers.bumper_rear_left ||
+ robot.orte.robot_bumpers.bumper_rear_right))
FSM_TRANSITION(wait_and_try_again);
break;
case EV_OBSTACLE_SIDE:
- if ((robot.use_left_switch && robot.orte.robot_switches.bumper_left) ||
- (robot.use_right_switch && robot.orte.robot_switches.bumper_right))
+ if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
+ (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
FSM_TRANSITION(wait_and_try_again);
break;
case EV_TRAJECTORY_LOST:
/*
* homologation.cc 08/04/29
- *
+ *
* Robot's control program intended for homologation (approval phase) on Eurobot 2009.
*
* Copyright: (c) 2009 CTU Dragons
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
#include "actuators.h"
#include <trgen.h>
#include "match-timing.h"
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
/* movement states */
-FSM_STATE_DECL(climb_the_slope);
-FSM_STATE_DECL(sledge_down);
-FSM_STATE_DECL(to_container_diag);
-FSM_STATE_DECL(to_container_ortho);
-FSM_STATE_DECL(experiment_decider);
-FSM_STATE_DECL(approach_next_corn);
-FSM_STATE_DECL(rush_the_corn);
-FSM_STATE_DECL(turn_around);
-FSM_STATE_DECL(zvedej_vidle);
+FSM_STATE_DECL(aproach_first_figure);
+FSM_STATE_DECL(load_first_figure);
+FSM_STATE_DECL(go_out_first_figure);
+FSM_STATE_DECL(place_first_figure);
+FSM_STATE_DECL(leave_first_figure);
+
+FSM_STATE_DECL(load_second_figure);
+FSM_STATE_DECL(go_out_second_figure);
+FSM_STATE_DECL(place_second_figure);
+FSM_STATE_DECL(leave_second_figure);
+FSM_STATE_DECL(aproach_third_figure);
+FSM_STATE_DECL(next_state);
+FSM_STATE_DECL(last_state);
+// FSM_STATE_DECL(experiment_decider);
+// FSM_STATE_DECL(approach_next_corn);
+// FSM_STATE_DECL(rush_the_corn);
+// FSM_STATE_DECL(turn_around);
+// FSM_STATE_DECL(zvedej_vidle);
/************************************************************************
* INITIAL AND STARTING STATES
************************************************************************/
-FSM_STATE(init)
+FSM_STATE(init)
{
switch (FSM_EVENT) {
case EV_ENTRY:
- tcFast = trajectoryConstraintsDefault;
- tcFast.maxv = 0.8;
- tcFast.maxacc = 1;
- tcFast.maxomega = 2;
tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.5;
- tcSlow.maxacc = 0.5;
- tcVerySlow = trajectoryConstraintsDefault;
- tcVerySlow.maxv = 0.05;
- tcVerySlow.maxacc = 0.05;
-
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.3;
+ tcSlow.maxomega = 1;
FSM_TRANSITION(wait_for_start);
break;
default:
void set_initial_position()
{
//FIXME:
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
+ 0);
}
void actuators_home()
{
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ act_jaws(CLOSE);
+ // drive lift to home position
+ //act_lift(0, 0, 1);
+ // unset the homing request
+ //act_lift(0, 0, 0);
}
#ifdef COMPETITION
sem_post(&robot.start);
actuators_home();
set_initial_position();
- FSM_TRANSITION(climb_the_slope);
+ FSM_TRANSITION(aproach_first_figure);
break;
case EV_TIMER:
FSM_TIMER(1000);
case EV_RETURN:
case EV_MOTION_ERROR:
case EV_MOTION_DONE:
- case EV_VIDLE_DONE:
+ //case EV_VIDLE_DONE:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
- robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
-
- /*
- //opras na testovani zluteho:
- robot_set_est_pos_trans(PLAYGROUND_WIDTH_M - ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(0));
- robot.team_color = YELLOW;
- */
break;
}
}
-FSM_STATE(zvedej_vidle)
+/************************************************************************
+ * MOVEMENT STATES
+ ************************************************************************/
+
+FSM_STATE(aproach_first_figure)
{
- static int cnt = 0;
switch(FSM_EVENT) {
case EV_ENTRY:
+ // disables using side switches on bumpers when going up
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+
+ robot_trajectory_new(&tcSlow);
+ //robot_move_by(0.5, NO_TURN(), &tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.175,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.2,
+ PLAYGROUND_HEIGHT_M - 0.3);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3,
+ 0.57,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
+// robot_trajectory_add_final_point_trans(
+// 0.45 + 0.2,
+// 0.29+0.28+0.28,
+// TURN_CW(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
- FSM_TIMER(500);
- act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED);
- ul_logdeb("--------------------cnt: %d\n", cnt);
- cnt++;
- if(cnt >= 3) {
- robot_exit();
- //FSM_TRANSITION(sledge_down);
- }
+ FSM_TRANSITION(load_first_figure);
break;
- case EV_START:
case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_DONE:
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-/************************************************************************
- * MOVEMENT STATES
- ************************************************************************/
-FSM_STATE(climb_the_slope)
+// FSM_STATE(load_first_figure)
+// {
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// robot_trajectory_new(&tcSlow);
+// robot_trajectory_add_final_point_trans(
+// 0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+// 0.29+0.28+0.28, NO_TURN());
+// break;
+// case EV_MOTION_DONE:
+// act_jaws(CATCH);
+// FSM_TIMER(2000);
+// break;
+// case EV_TIMER:
+// FSM_TRANSITION(go_out_first_figure);
+// break;
+// case EV_RETURN:
+// case EV_MOTION_ERROR:
+// case EV_EXIT:
+// break;
+// }
+// }
+
+FSM_STATE(load_first_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- // disables using side switches on bumpers when going up
- robot.use_left_switch = false;
- robot.use_right_switch = false;
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
- robot.ignore_hokuyo = true;
- robot_trajectory_new_backward(&tcSlow);
- robot_trajectory_add_point_trans(
- 0.5 - ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
- /* position for collecting oranges*/
+ robot_trajectory_new(&tcSlow);
robot_trajectory_add_final_point_trans(
- SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.05,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
- NO_TURN());
+ ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
+ 0.57, NO_TURN());
break;
case EV_MOTION_DONE:
FSM_TIMER(2000);
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ act_jaws(CATCH);
break;
- case EV_START:
case EV_TIMER:
- FSM_TRANSITION(sledge_down);
+ FSM_TRANSITION(go_out_first_figure);
break;
case EV_RETURN:
- case EV_VIDLE_DONE:
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
+ // enables using side switches on bumpers
+ //robot.use_left_switch = true;
+ //robot.use_right_switch = true;
+ //robot.ignore_hokuyo = false;
break;
}
}
-FSM_STATE(sledge_down)
+FSM_STATE(go_out_first_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(
- 1 -ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
- robot_trajectory_add_point_trans(
- SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
- robot_trajectory_add_final_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6, NO_TURN());
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN());
break;
case EV_MOTION_DONE:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(to_container_diag);
- //FSM_TRANSITION(to_container_ortho);
+ case EV_TIMER:
+ FSM_TRANSITION(place_first_figure);
break;
case EV_START:
- case EV_TIMER:
case EV_RETURN:
- case EV_VIDLE_DONE:
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
- // enables using side switches on bumpers
- robot.use_left_switch = true;
- robot.use_right_switch = true;
- robot.ignore_hokuyo = false;
break;
}
}
-FSM_STATE(to_container_diag)
+FSM_STATE(place_first_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- // face the rim with front of the robot
- //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
- // face the rim with back of the robot
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(6000);
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(0.45 + 0.2, 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M, ARRIVE_FROM(DEG2RAD(-90), 0.20));
break;
+ case EV_START:
case EV_TIMER:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(approach_next_corn);
+ FSM_TRANSITION(leave_first_figure);
break;
- case EV_START:
case EV_RETURN:
- case EV_VIDLE_DONE:
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(2000);
+ break;
case EV_MOTION_ERROR:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
}
}
-FSM_STATE(to_container_ortho)
+
+FSM_STATE(leave_first_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
+ //FSM_TIMER(1000);
+ robot_trajectory_new_backward(&tcSlow);
robot_trajectory_add_point_trans(
- SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
- PLAYGROUND_HEIGHT_M - 0.355);
- robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65);
- robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9);
-
- // face the rim with front of the robot
- //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
- // face the rim with back of the robot
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90)));
+ 0.45 + 0.175,
+ 0.7);
+ robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.35 + 0.35 + 0.175, TURN_CW(DEG2RAD(0)));
+ break;
+ case EV_START:
+ case EV_TIMER:
break;
+ case EV_RETURN:
case EV_MOTION_DONE:
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ FSM_TRANSITION(load_second_figure);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_second_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
+ 0.85, NO_TURN());
break;
case EV_START:
case EV_TIMER:
+ FSM_TRANSITION(go_out_second_figure);
+ break;
case EV_RETURN:
- case EV_VIDLE_DONE:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(CATCH);
+ break;
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-static enum where_to_go {
- CORN,
- TURN_AROUND,
- CONTAINER,
- NO_MORE_CORN
-} where_to_go = CORN;
-
-static struct corn *corn_to_get;
-
-FSM_STATE(experiment_decider)
+FSM_STATE(go_out_second_figure)
{
-
switch(FSM_EVENT) {
case EV_ENTRY:
- if (where_to_go == CORN) {
- FSM_TRANSITION(approach_next_corn);
- } else if (where_to_go == CONTAINER) {
- FSM_TRANSITION(rush_the_corn);
- } else if (where_to_go == TURN_AROUND) {
- FSM_TRANSITION(turn_around);
- } else /* NO_MORE_CORN */ {
- }
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 0.85, NO_TURN());
break;
case EV_START:
case EV_TIMER:
+ break;
case EV_RETURN:
- case EV_VIDLE_DONE:
case EV_MOTION_DONE:
+ FSM_TRANSITION(place_second_figure);
+ break;
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
+FSM_STATE(place_second_figure)
{
switch(FSM_EVENT) {
- case EV_ENTRY: {
- double x, y, phi;
- robot_get_est_pos(&x, &y, &phi);
- ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
- cnt, x, y, phi);
-
- corn_to_get = choose_next_corn();
- if (corn_to_get) {
- Pos *p = get_corn_approach_position(corn_to_get);
- corn_to_get->was_collected = true;
- //robot_trajectory_new(&tcFast);
- //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
- robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
- delete(p);
- where_to_go = CONTAINER;
- } else {
- where_to_go = NO_MORE_CORN;
- }
- break;
- }
- case EV_MOTION_DONE:
- cnt++;
- FSM_TRANSITION(experiment_decider);
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.175,
+ 0.15 + ROBOT_AXIS_TO_FIGURE_CENTER_M, ARRIVE_FROM(DEG2RAD(-90), 0.50));
break;
case EV_START:
case EV_TIMER:
+ FSM_TRANSITION(leave_second_figure);
+ break;
case EV_RETURN:
- case EV_VIDLE_DONE:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(OPEN);
+ break;
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-FSM_STATE(rush_the_corn)
+FSM_STATE(leave_second_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- double x;
- if (robot.team_color == BLUE) {
- x = corn_to_get->position.x;
- } else {
- x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
- }
- ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y);
- remove_wall_around_corn(x, corn_to_get->position.y);
- robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
- where_to_go = TURN_AROUND;
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.175,
+ 0.7, NO_TURN());
break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(aproach_third_figure);
+ break;
+ case EV_RETURN:
case EV_MOTION_DONE:
- FSM_TRANSITION(experiment_decider);
+ FSM_TIMER(2000);
+ act_jaws(CLOSE);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(aproach_third_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.7 + 0.35, 0.7);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.7 + 0.35 + 0.175, 0.7 + 0.175);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.35 + 0.25, 1.4 + 0.1);
+ robot_trajectory_add_final_point_trans(
+ 2, 1.5,
+ ARRIVE_FROM(DEG2RAD(-90), 0.20));
break;
case EV_START:
case EV_TIMER:
+ FSM_TRANSITION(next_state);
+ break;
case EV_RETURN:
- case EV_VIDLE_DONE:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(OPEN);
+ break;
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-// used to perform the maneuvre
-FSM_STATE(turn_around)
+FSM_STATE(next_state)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 1.4 + 0.175, 0.12 + 0.1 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ NO_TURN());
break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
case EV_MOTION_DONE:
- where_to_go = CORN;
- FSM_TRANSITION(experiment_decider);
+ FSM_TRANSITION(last_state);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(last_state)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 1.4 + 0.175, 1,
+ NO_TURN());
break;
case EV_START:
case EV_TIMER:
+ break;
case EV_RETURN:
- case EV_VIDLE_DONE:
+ case EV_MOTION_DONE:
+ break;
case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
}
}
-
/************************************************************************
* MAIN FUNCTION
************************************************************************/
--- /dev/null
+/*
+ * homologation.cc 12/03/15
+ *
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2012.
+ *
+ * Copyright: (c) 2012 CTU Flamingos
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <trgen.h>
+#include "match-timing.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
+
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_start);
+/* movement states - buillon */
+FSM_STATE_DECL(aproach_buillon);
+/* Pushing the bottle */
+FSM_STATE_DECL(push_bottle);
+
+
+FSM_STATE(init)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.3;
+ tcSlow.maxomega = 1;
+ FSM_TRANSITION(wait_for_start);
+ break;
+ default:
+ break;
+ }
+}
+
+void set_initial_position()
+{
+ // TODO define initial position
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+ 0);
+}
+
+void actuators_home()
+{
+ //act_jaws(CLOSE);
+ // drive lift to home position
+ //act_lift(0, 0, 1);
+ // unset the homing request
+ //act_lift(0, 0, 0);
+}
+
+FSM_STATE(wait_for_start)
+{
+ pthread_t thid;
+ #ifdef WAIT_FOR_START
+ ul_logdeb("WAIT_FOR_START mode set\n");
+ #else
+ ul_logdeb("WAIT_FOR_START mode NOT set\n");
+ #endif
+ #ifdef COMPETITION
+ ul_logdeb("COMPETITION mode set\n");
+ #else
+ ul_logdeb("COMPETITION mode NOT set\n");
+ #endif
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ pthread_create(&thid, NULL, timing_thread, NULL);
+#ifdef WAIT_FOR_START
+ FSM_TIMER(1000);
+ break;
+#endif
+ case EV_START:
+ /* start competition timer */
+ sem_post(&robot.start);
+ actuators_home();
+ set_initial_position();
+ FSM_TRANSITION(aproach_buillon);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ // We set initial position periodically in
+ // order for it to be updated on the display
+ // if the team color is changed during waiting
+ // for start.
+ set_initial_position();
+ if (robot.start_state == START_PLUGGED_IN)
+ actuators_home();
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_MOTION_DONE:
+ //case EV_VIDLE_DONE:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(aproach_buillon)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.65,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+ robot_trajectory_add_point_trans(
+ 0.65,
+ 1.3);
+ robot_trajectory_add_final_point_trans(
+ 0.5,
+ 1.1,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ case EV_TIMER:
+ FSM_TRANSITION(push_bottle);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(push_bottle)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 0.7);
+ robot_trajectory_add_final_point_trans(
+ 0.64 + 0.08,
+ ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
+ break;
+ case EV_MOTION_DONE:
+ break;
+ default:
+ break;
+ }
+}
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
+int main()
+{
+ int rv;
+
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ robot.obstacle_avoidance_enabled = true;
+
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ //robot.fsm.act.debug_states = 1;
+
+ robot.fsm.main.state = &fsm_state_main_init;
+ //robot.team_color = BLUE;
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+ robot_destroy();
+
+ return 0;
+}
\ No newline at end of file
#include <robomath.h>
#include <hokuyo.h>
+#include <shape_detect.h>
+
+#include "map_handling.h"
+
/*******************************************************************************
* Parameters of Obstacle detection
*******************************************************************************/
if (!ShmapIsCellInMap(i, j)) continue;
ShmapCell2Point(i, j, &xx, &yy);
if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
- (distance(xx, yy, x, y) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
+ (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
}
}
}
+
}
+
+void figure_detected_at(double x, double y, const bool state)
+{
+ int i,j, xcell, ycell;
+ struct robot_pos_type est_pos;
+ struct map *map = robot.map;
+ double xx, yy;
+ bool valid;
+
+ if (!map)
+ return;
+
+ ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+ /* Ignore obstacles outside playground */
+ if (!valid)
+ return;
+
+ /* Ignore obstacles at marked places */
+ if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+ return;
+
+ if (state) {
+ map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+
+ /* Mark "protected" area around the obstacle */
+ robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+
+ int obst_size_cell = (int)ceil(0.2/MAP_CELL_SIZE_M);
+ for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
+ for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
+ if (!ShmapIsCellInMap(i, j)) continue;
+ ShmapCell2Point(i, j, &xx, &yy);
+ if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
+ (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
+ map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+ }
+ }
+ }
+
+ }
+}
+
/**
* A thread running the trajectory recalc
*
*
* @return
*/
-
void obst_coord(struct robot_pos_type *e, const struct sharp_pos *s, double v, double *x, double *y)
{
double sx, sy, sa;
*y = sy+v*sin(sa);
}
+void get_checkerboard(std::vector<Shape_detect::Point> &team)
+{
+ Shape_detect::Point tmp, newPoint, start;
+ unsigned int i;
+
+ if (robot.team_color) {
+ start.x = 0.625;
+ start.y = 0.525;
+ } else {
+ start.x = 0.975;
+ start.y = 0.525;
+
+ }
+
+ tmp = start;
+
+ for (i = 1; i < 4; i++) {
+ for (int j = 0; j < 3; j++) {
+ newPoint.y = tmp.y + 0.7 * j;
+ newPoint.x = tmp.x;
+ team.push_back(newPoint);
+ }
+ tmp.x = tmp.x + 0.7;
+ }
+
+ if (robot.team_color) {
+ tmp.x = start.x + 0.35;
+ tmp.y = start.y + 0.35;
+ } else {
+ tmp.x = start.x - 0.35;
+ tmp.y = start.y + 0.35;
+
+ }
+
+ for (i = 1; i < 4; i++) {
+ for (int j = 0; j < 2; j++) {
+ newPoint.y = tmp.y + 0.7 * j;
+ newPoint.x = tmp.x;
+ team.push_back(newPoint);
+ }
+ tmp.x = tmp.x + 0.7;
+ }
+
+ if (robot.team_color) {
+ tmp.x = 1.675;
+ tmp.y = 0.175;
+ } else {
+ tmp.x = 1.325;
+ tmp.y = 0.175;
+
+ }
+
+ team.push_back(tmp);
+}
+
+inline float point_distance(Shape_detect::Point a, Shape_detect::Point b)
+{
+ return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+}
+
void update_map_hokuyo(struct hokuyo_scan_type *s)
{
double x, y;
- //Pos p;
struct robot_pos_type e;
- int i;
+ unsigned int i;
struct sharp_pos beam;
u_int16_t *data;
+ static std::vector<Shape_detect::Point> reds;
+ static std::vector<Shape_detect::Point> blues;
+
+ if (reds.size() < 16) {
+ get_checkerboard(blues);
+ get_checkerboard(reds);
+ }
+
robot_get_est_pos(&e.x, &e.y, &e.phi);
-
+
beam.x = HOKUYO_CENTER_OFFSET_M;
beam.y = 0;
data = s->data;
+ Shape_detect shapeDet;
+ std::vector<Shape_detect::Arc> arcs;
+ std::vector<Shape_detect::Point> center_arcs;
+
+ shapeDet.prepare(data);
+ shapeDet.arc_detect(arcs);
+
+ Shape_detect::Point tmpPoint;
+
+ double distance;
+
+// if (arcs.size() > 0) {
+// for (i = 0; i < arcs.size(); i++) {
+// x = arcs[i].center.x / 1000;
+// y = arcs[i].center.y / 1000;
+//
+// tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
+// tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
+//
+// center_arcs.push_back(tmpPoint);
+// }
+//
+// for (i = 0; i < center_arcs.size(); i++) {
+// if (robot.team_color) {
+// for (unsigned int j = 0; j < blues.size(); j++) {
+// distance = point_distance(blues[j], center_arcs[i]);
+// if (distance < 0.05) {
+// figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+// break;
+// }
+// }
+// } else {
+// for (unsigned int j = 0; j < reds.size(); j++) {
+// distance = point_distance(reds[j], center_arcs[i]);
+// if (distance < 0.05) {
+// figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+// break;
+// }
+// }
+// }
+// }
+// }
+
+ bool obstacle = true;
+
for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
beam.ang = HOKUYO_INDEX_TO_RAD(i);
- if((beam.ang<(-70.0/180.0*M_PI))||((beam.ang>(70.0/180.0*M_PI))))
+ if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
continue;
-
- if(data[i] > 19) {
+
+ if(data[i] > 19 && data[i] < 2000) {
obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
- obstacle_detected_at(x, y, true);
- obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
- obstacle_detected_at(x, y, false);
+
+ tmpPoint.x = x;
+ tmpPoint.y = y;
+
+ if (center_arcs.size() > 0) {
+ for (unsigned int j = 0; j < center_arcs.size(); j++) {
+ if (point_distance(tmpPoint, center_arcs[j]) < 0.12) {
+ obstacle = false;
+ break;
+ }
+ }
+ }
+
+ if (obstacle) {
+ obstacle_detected_at(x, y, true);
+ //obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
+ //obstacle_detected_at(x, y, false);
+ }
+ obstacle = true;
}
-
}
}
/**
* Decrease map.detected_obstacle by val with saturation on zero. It
* also clears #MAP_FLAG_DET_OBST if value reaches zero.
- *
+ *
* @param val Value to decrease obstacle life.
* @see map #MAP_NEW_OBSTACLE
* @todo Do faster this process. Use a cell's list to update.
#include <robodim.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
void * thread_obstacle_forgeting(void * arg);
/*void update_map(struct sharps_type *s);*/
void update_map_hokuyo(struct hokuyo_scan_type *s);
+#ifdef __cplusplus
+}
+#endif
+
#endif
UL_LOG_CUST(ulogd_match_timing); // Log domain name = "ulogd_" + the name of the source file
#ifdef COMPETITION
-#define COMPETITION_TIME_DEFAULT 90
-#define TIME_TO_DEPOSITE_DEFAULT 60
+#define COMPETITION_TIME_DEFAULT 87
+#define TIME_TO_DEPOSITE_DEFAULT 65
#else
-#define COMPETITION_TIME_DEFAULT 90
-#define TIME_TO_DEPOSITE_DEFAULT 60
+#define COMPETITION_TIME_DEFAULT 87
+#define TIME_TO_DEPOSITE_DEFAULT 65
#endif
/* competition time in seconds */
// robot.use_back_switch = true;
// printf("Back switch not ignored\n");
-/* WAIT(TIME_TO_DEPOSITE); */
-/* printf("%d seconds timer exceeded!\n", TIME_TO_DEPOSITE); */
-/* robot.short_time_to_end = true; */
+ WAIT(TIME_TO_DEPOSITE);
+ ul_logfatal("%d seconds timer exceeded!\n", TIME_TO_DEPOSITE);
+ robot.short_time_to_end = true;
WAIT(COMPETITION_TIME);
ul_logfatal("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
float robot_current_time()
{
- struct timespec now, diff;
+ struct timespec now, diff, start_local;
+ start_local = start;
clock_gettime(CLOCK_MONOTONIC, &now);
- timespec_subtract(&diff, &now, &start);
+ timespec_subtract(&diff, &now, &start_local);
return diff.tv_sec + diff.tv_nsec/1000000000.0;
}
if (fabs(future_pos.v) < 0.01)
continue;
- x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
- y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
-
+ x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
+ y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
+
ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
if (!valid)
continue;
unsigned l, r;
mul = 1000.0 / ROBOT_WHEEL_RADIUS_MM; // to angular speed
- mul *= 28.0/(2.0*M_PI)/*gear*/*(1<<8); // to pxmc speed
+ mul *= ROBOT_MOTOR_GEARBOX_RATIO/(2.0*M_PI)/*gear*/*(1<<8); // to pxmc speed
// I hope it is not neccessary to lock here
l = (int)(left * mul);
static inline double __trans_ang(double phi)
{
- if (robot.team_color == BLUE) {
+ if (robot.team_color == VIOLET) {
return phi;
} else {
double a;
static inline struct move_target_heading __trans_heading(struct move_target_heading h)
{
- if (robot.team_color == BLUE) {
+ if (robot.team_color == VIOLET) {
return h;
} else {
if (h.operation != TOP_DONT_TURN) {
static inline double __trans_x(double x)
{
- if (robot.team_color == BLUE)
+ if (robot.team_color == VIOLET)
return x;
else
return PLAYGROUND_WIDTH_M - x;
--- /dev/null
+#define FSM_MAIN
+#include "robodata.h"
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <sharp.h>
+#include <trgen.h>
+#include "match-timing.h"
+#include <stdbool.h>
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
+
+#include "common-states.h"
+
+/************************************************************************
+ * Functions used in and called from all the (almost identical)
+ * "wait for start" states in particular strategies.
+ ************************************************************************/
+
+#undef DBG_FSM_STATE
+#define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
+ fsm->debug_name, robot_current_time(), \
+ name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+
+
+void set_initial_position()
+{
+ robot_set_est_pos_trans(ROBOT_START_X_M,
+ ROBOT_START_Y_M,
+ DEG2RAD(ROBOT_START_ANGLE_DEG));
+}
+
+void actuators_home()
+{
+ act_jaws(CLOSE);
+
+ bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
+ // drive lift to home position
+ //act_lift(0, 0, 1);
+ // unset the homing request
+ //act_lift(0, 0, 0);
+}
+
+void start_entry()
+{
+ pthread_t thid;
+ robot.check_turn_safety = false;
+ pthread_create(&thid, NULL, timing_thread, NULL);
+ start_timer();
+}
+
+// We set initial position periodically in order for it to be updated
+// on the display if the team color is changed during waiting for
+// start.
+void start_timer()
+{
+ set_initial_position();
+ if (robot.start_state == START_PLUGGED_IN)
+ actuators_home();
+}
+
+void start_go()
+{
+ sem_post(&robot.start);
+ actuators_home();
+ set_initial_position();
+}
+
+void start_exit()
+{
+
+}
+
+bool read_sharp()
+{
+ int sharp_data = robot.orte.jaws_status.act_pos.left;
+ int sharp_dist = 0;
+ sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
+ printf("sharp data: %dmm\n", sharp_dist);
+ return (sharp_dist <= 150 ? true : false);
+}
+
+void inline enable_bumpers(bool enabled)
+{
+ robot.use_left_bumper = enabled;
+ robot.use_right_bumper = enabled;
+ robot.use_back_bumpers = enabled;
+}
+
+void enable_my_square_walls(bool enabled)
+{
+ for (int i = 0; i < 15; i++) {
+ if (robot.team_color == RED)
+ ShmapSetCircleFlag(red_sq[i].x, red_sq[i].y, 0.2, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
+ else
+ ShmapSetCircleFlag(blue_sq[i].x, blue_sq[i].y, 0.2,enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
+ }
+}
+
+/************************************************************************
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+
+bool tower = false;
+bool bonus_placed = false;
+
+const double load_dist = 0.04; // TODO distance from side when loading green figures
+const double app_dist = 0.04; // TODO distance from green figures when approach
+
+/* fields with numbers of opponent squares, we have five variations of movement style across opp. squares*/
+const int move_around[][8] = {{12, 14, 11, 13, 11, 8, 6, 9},
+ {6, 8, 11, 13, 11, 14, 12, 9},
+ {11, 13, 11, 8, 11, 14, 12, 9},
+ {6, 8, 11, 13, 11, 14, 12, 9},
+ {11, 8, 11, 13, 11, 14, 12, 9}};
+
+/** generate "random" positions on oponent squares and goto this position */
+FSM_STATE(move_around)
+{
+ static int next_sq = 0;
+ static int max_wait = 0;
+ // choose randomly one of five move_around strategies
+ static int strategy = rand() % 5;
+ int goal;
+ double goal_x, goal_y;
+ static bool entry = true;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ max_wait = 0;
+ FSM_TIMER(2000);
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ act_jaws(CLOSE);
+
+ enable_my_square_walls(true);
+
+ srand((int)(robot_current_time()*10000));
+
+ // save next square number where to go to
+ goal = move_around[strategy][next_sq];
+
+ // pick position on opponent squares with goal index
+ if (robot.team_color == RED) {
+ goal_x = blue_sq[goal].x;
+ goal_y = blue_sq[goal].y;
+ } else {
+ goal_x = red_sq[goal].x;
+ goal_y = red_sq[goal].y;
+ }
+
+ robot_goto_notrans(goal_x, goal_y, NO_TURN(), &tcFast);
+ printf("strategy: %d, next_sq: %d, goal: %d\n",strategy, next_sq, goal);
+ next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ // every two seconds check if short time to end
+ FSM_TIMER(2000);
+ if (robot.short_time_to_end == true) {
+ // if short time to end - send stop signal to motion FSM and return to main FSM
+ FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
+ SUBFSM_RET(NULL);
+ printf("return from move around state\n");
+ } else if (robot.fsm.motion.state_name == "wait_and_try_again") {
+ // if goal position unawailable (obstacle) try max. X-times
+ if (++max_wait >= 3) {
+ // if goal not awailable, send stop signal to motion FSM and generate new target trom move_around
+ printf("go to next square!\n");
+ max_wait = 0;
+ next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
+ FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
+ FSM_TRANSITION(move_around);
+ } else {
+ printf("waiting for opponent! %d\n", max_wait);
+ }
+ }
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ if (robot.short_time_to_end == true)
+ SUBFSM_RET(NULL);
+ else
+ FSM_TRANSITION(move_around);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick figure from opponent bonus square */
+FSM_STATE(approach_opp_bonus_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_goto_trans(
+ 0.45 + 2*0.35 + 0.175,
+ 0.35 + 0.175,
+ TURN(DEG2RAD(-50)), &tcFast);
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TRANSITION(load_opp_bonus_figure);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_opp_bonus_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ enable_my_square_walls(false);
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ //robot_trajectory_new(&tcSlow);
+ robot_move_by(0.35, NO_TURN(), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TRANSITION(place_opp_bonus_figure);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_opp_bonus_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 2*0.35 + 0.175,
+ 0.35 + 0.2);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 2*0.35 + 0.175,
+ 2*0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ ARRIVE_FROM(DEG2RAD(90), 0.1));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TRANSITION(leave_opp_bonus_figure);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_opp_bonus_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 2*0.35 + 0.175,
+ 5*0.35 + 0.1,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ SUBFSM_RET();
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** securely bypass firt figure in front of starting area */
+FSM_STATE(bypass_figure_in_front_of_start)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = false;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.3,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3,
+ 4*0.35,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ SUBFSM_RET(NULL);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick second figure from green area */
+FSM_STATE(approach_second_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3 - app_dist,
+ 0.29 + 0.28,
+ TURN(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(load_second_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_second_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
+ 0.29 + 0.28,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ act_jaws(CLOSE);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_second_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(go_out_second_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3,
+ 0.7,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ if (read_sharp()) {
+ FSM_TRANSITION(place_figure_to_bonus_area);
+ } else {
+ FSM_TRANSITION(place_figure_to_protected_block);
+ }
+ break;
+ case EV_TIMER:
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_figure_to_protected_block)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ ARRIVE_FROM(DEG2RAD(-90), 0.20));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_protected_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_protected_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.175,
+ 0.7);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3,
+ 0.29 + 2*0.28,
+ NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick third figure from green area */
+FSM_STATE(approach_third_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3 - app_dist,
+ 0.29 + 2*0.28,
+ TURN(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(load_third_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_third_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
+ 0.29 + 2*0.28,
+ ARRIVE_FROM(DEG2RAD(180), 0.20));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_third_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(go_out_third_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.35,
+ 0.29 + 2*0.28,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ if (bonus_placed)
+ FSM_TRANSITION(place_figure_to_near_area);
+ else
+ FSM_TRANSITION(place_figure_to_bonus_area);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ robot.ignore_hokuyo = false;
+ break;
+ }
+}
+
+FSM_STATE(place_figure_to_near_area)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.35 + 0.175,
+ 0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ TURN(DEG2RAD(-90)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(leave_near_figure);
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_near_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.35 + 0.175,
+ 0.29 + 3*0.28,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ SUBFSM_RET(NULL);
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_figure_to_bonus_area)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.175,
+ 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ ARRIVE_FROM(DEG2RAD(-90), 0.3));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_bonus_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ bonus_placed = true;
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_bonus_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.2,
+ 0.7,
+ NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ act_lift(DOWN, 0, 0);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick fourth green figure from green area */
+FSM_STATE(approach_fourth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3 - app_dist,
+ 0.29 + 3*0.28,
+ TURN(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ tower = read_sharp();
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(load_fourth_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_fourth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
+ 0.29 + 3*0.28,
+ ARRIVE_FROM(DEG2RAD(180), 0.20));
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ act_jaws(CLOSE);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_fourth_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(go_out_fourth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 3*0.35 + 0.175,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(place_figure_to_red_square);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_figure_to_red_square)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 0.7 + 0.15 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ ARRIVE_FROM(DEG2RAD(-90), 0.05));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_red_square_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_red_square_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new_backward(&tcFast);
+// robot_trajectory_add_point_trans(
+// 0.45 + 0.175,
+// 0.7 + 0.7);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 0.7 + 0.7,
+ NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick fifth green figure from green area */
+FSM_STATE(approach_fifth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3 - app_dist,
+ 0.29 + 4*0.28,
+ TURN(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(load_fifth_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_fifth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
+ 0.29 + 4*0.28,
+ ARRIVE_FROM(DEG2RAD(180), 0.20));
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ act_jaws(CLOSE);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_fifth_green_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(go_out_fifth_green_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = true;
+
+ robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 2*0.35 + 0.175,
+ 4*0.35,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ break;
+ case EV_TIMER:
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/** pick center figure */
+FSM_STATE(approach_center_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = false;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcVeryFast);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.35,
+ 5*0.35 + 0.175);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 3*0.35,
+ 4*0.35 + 0.1,
+ TURN(DEG2RAD(-90)));
+ FSM_TIMER(2000);
+ break;
+ case EV_START:
+ case EV_TIMER:
+ robot.use_back_bumpers = true;
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(load_center_figure);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_center_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ act_jaws(OPEN);
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+ robot.ignore_hokuyo = false;
+
+ robot_trajectory_new(&tcFast);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 3*0.35,
+ 3*0.35 + 0.1,
+ ARRIVE_FROM(DEG2RAD(-90), 0.1));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(CLOSE);
+ FSM_TRANSITION(place_figure_to_bonus_area);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#ifndef COMMON_STATES_H
+#define COMMON_STATES_H
+
+#define FSM_MAIN
+
+#include "roboevent.h"
+#include <fsm.h>
+
+extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+extern bool build_tower, tower, bonus_placed;
+
+/* strategy FSM */
+FSM_STATE_DECL(start_pick_all_our_figures);
+FSM_STATE_DECL(start_pick_two_our_figures);
+FSM_STATE_DECL(start_pick_third_figure);
+FSM_STATE_DECL(start_pick_fourth_figure);
+FSM_STATE_DECL(start_pick_center_figure);
+
+/* common FSM states */
+FSM_STATE_DECL(bypass_figure_in_front_of_start);
+FSM_STATE_DECL(move_around);
+FSM_STATE_DECL(place_figure_to_near_area);
+FSM_STATE_DECL(leave_near_figure);
+
+/* pick opponent bonus figure states */
+FSM_STATE_DECL(approach_opp_bonus_figure);
+FSM_STATE_DECL(load_opp_bonus_figure);
+FSM_STATE_DECL(place_opp_bonus_figure);
+FSM_STATE_DECL(leave_opp_bonus_figure);
+
+/* pick second green figure FSM */
+FSM_STATE_DECL(approach_second_green_figure);
+FSM_STATE_DECL(load_second_green_figure);
+FSM_STATE_DECL(go_out_second_green_figure);
+FSM_STATE_DECL(place_figure_to_protected_block);
+FSM_STATE_DECL(leave_protected_figure);
+
+/* pick third green figure FSM */
+FSM_STATE_DECL(approach_third_green_figure);
+FSM_STATE_DECL(load_third_green_figure);
+FSM_STATE_DECL(go_out_third_green_figure);
+FSM_STATE_DECL(place_figure_to_bonus_area);
+FSM_STATE_DECL(leave_bonus_figure);
+
+/* pick fourth green figure FSM */
+FSM_STATE_DECL(approach_fourth_green_figure);
+FSM_STATE_DECL(load_fourth_green_figure);
+FSM_STATE_DECL(go_out_fourth_green_figure);
+FSM_STATE_DECL(place_figure_to_red_square);
+FSM_STATE_DECL(leave_red_square_figure);
+
+/* pick fifth green figure FSM */
+FSM_STATE_DECL(approach_fifth_green_figure);
+FSM_STATE_DECL(load_fifth_green_figure);
+FSM_STATE_DECL(go_out_fifth_green_figure);
+
+/* pick center figure FSM */
+FSM_STATE_DECL(approach_center_figure);
+FSM_STATE_DECL(load_center_figure);
+
+void start_entry();
+void start_timer();
+void start_go();
+void start_exit();
+bool read_sharp();
+
+#endif
--- /dev/null
+/*
+ * competition.cc 2010/04/30
+ *
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
+ *
+ * Copyright: (c) 2009 - 2010 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#ifndef DEBUG
+#define DEBUG
+#endif
+
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
+
+int main()
+{
+ int rv;
+
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ robot.obstacle_avoidance_enabled = true;
+
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ //robot.fsm.act.debug_states = 1;
+
+ //robot.fsm.main.state = &fsm_state_main_start_pick_all_our_figures;
+ robot.fsm.main.state = &fsm_state_main_start_pick_two_our_figures;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_fourth_figure;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_third_figure;
+ //robot.fsm.main.state = &fsm_state_main_start_pick_center_figure;
+
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ tcVeryFast = trajectoryConstraintsDefault;
+ tcVeryFast.maxv = 1;
+ tcVeryFast.maxacc = 0.6;
+ tcVeryFast.maxomega = 2;
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 0.6;
+ tcFast.maxacc = 0.2;
+ tcFast.maxomega = 1;
+ tcFast.maxe = 0.02;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.4;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 1;
+ tcSlow.maxe = 0.02;
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.1;
+ tcVerySlow.maxacc = 0.1;
+ tcVerySlow.maxomega = 0.2;
+ tcVerySlow.maxe = 0.02;
+
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+ robot_destroy();
+
+ return 0;
+}
+
+/************************************************************************
+ * STATE SKELETON
+ ************************************************************************/
+
+/*
+FSM_STATE()
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_ACTION_DONE:
+ case EV_ACTION_ERROR:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+*/
--- /dev/null
+/*
+ * homologation.cc 08/04/29
+ *
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
+ *
+ * Copyright: (c) 2009 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#ifndef DEBUG
+#define DEBUG
+#endif
+
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <trgen.h>
+#include "match-timing.h"
+#include "eb2010misc.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
+
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+/* initial and starting states */
+FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_start);
+/* movement states */
+FSM_STATE_DECL(aproach_first_figure);
+FSM_STATE_DECL(load_first_figure);
+FSM_STATE_DECL(go_out_first_figure);
+FSM_STATE_DECL(place_first_figure);
+FSM_STATE_DECL(leave_first_figure);
+
+FSM_STATE_DECL(load_second_figure);
+FSM_STATE_DECL(go_out_second_figure);
+FSM_STATE_DECL(place_second_figure);
+FSM_STATE_DECL(leave_second_figure);
+FSM_STATE_DECL(aproach_third_figure);
+FSM_STATE_DECL(next_state);
+FSM_STATE_DECL(last_state);
+// FSM_STATE_DECL(experiment_decider);
+// FSM_STATE_DECL(approach_next_corn);
+// FSM_STATE_DECL(rush_the_corn);
+// FSM_STATE_DECL(turn_around);
+// FSM_STATE_DECL(zvedej_vidle);
+
+/************************************************************************
+ * INITIAL AND STARTING STATES
+ ************************************************************************/
+
+FSM_STATE(init)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.3;
+ tcSlow.maxomega = 1;
+ FSM_TRANSITION(wait_for_start);
+ break;
+ default:
+ break;
+ }
+}
+
+void set_initial_position()
+{
+ //FIXME:
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
+ 0);
+}
+
+void actuators_home()
+{
+ act_jaws(CLOSE);
+ // drive lift to home position
+ //act_lift(0, 0, 1);
+ // unset the homing request
+ //act_lift(0, 0, 0);
+}
+
+#ifdef COMPETITION
+#define WAIT_FOR_START
+#else
+#undef WAIT_FOR_START
+#endif
+
+FSM_STATE(wait_for_start)
+{
+ pthread_t thid;
+ #ifdef WAIT_FOR_START
+ ul_logdeb("WAIT_FOR_START mode set\n");
+ #else
+ ul_logdeb("WAIT_FOR_START mode NOT set\n");
+ #endif
+ #ifdef COMPETITION
+ ul_logdeb("COMPETITION mode set\n");
+ #else
+ ul_logdeb("COMPETITION mode NOT set\n");
+ #endif
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ pthread_create(&thid, NULL, timing_thread, NULL);
+#ifdef WAIT_FOR_START
+ FSM_TIMER(1000);
+ break;
+#endif
+ case EV_START:
+ /* start competition timer */
+ sem_post(&robot.start);
+ actuators_home();
+ set_initial_position();
+ FSM_TRANSITION(aproach_first_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ // We set initial position periodically in
+ // order for it to be updated on the display
+ // if the team color is changed during waiting
+ // for start.
+ set_initial_position();
+ if (robot.start_state == START_PLUGGED_IN)
+ actuators_home();
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_MOTION_DONE:
+ //case EV_VIDLE_DONE:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+/************************************************************************
+ * MOVEMENT STATES
+ ************************************************************************/
+
+FSM_STATE(aproach_first_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ // disables using side switches on bumpers when going up
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
+ robot.use_back_bumpers = true;
+
+ robot_trajectory_new(&tcSlow);
+ //robot_move_by(0.5, NO_TURN(), &tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.175,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.2,
+ PLAYGROUND_HEIGHT_M - 0.3);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.3,
+ 0.57,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
+// robot_trajectory_add_final_point_trans(
+// 0.45 + 0.2,
+// 0.29+0.28+0.28,
+// TURN_CW(DEG2RAD(180)));
+ break;
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(load_first_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+
+// FSM_STATE(load_first_figure)
+// {
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// robot_trajectory_new(&tcSlow);
+// robot_trajectory_add_final_point_trans(
+// 0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+// 0.29+0.28+0.28, NO_TURN());
+// break;
+// case EV_MOTION_DONE:
+// act_jaws(CATCH);
+// FSM_TIMER(2000);
+// break;
+// case EV_TIMER:
+// FSM_TRANSITION(go_out_first_figure);
+// break;
+// case EV_RETURN:
+// case EV_MOTION_ERROR:
+// case EV_EXIT:
+// break;
+// }
+// }
+
+FSM_STATE(load_first_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
+ 0.57, NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(CATCH);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_first_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ // enables using side switches on bumpers
+ //robot.use_left_switch = true;
+ //robot.use_right_switch = true;
+ //robot.ignore_hokuyo = false;
+ break;
+ }
+}
+
+FSM_STATE(go_out_first_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ case EV_TIMER:
+ FSM_TRANSITION(place_first_figure);
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_first_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(0.45 + 0.2, 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M, ARRIVE_FROM(DEG2RAD(-90), 0.20));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_first_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ act_jaws(OPEN);
+ FSM_TIMER(2000);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+
+
+FSM_STATE(leave_first_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ //FSM_TIMER(1000);
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.175,
+ 0.7);
+ robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.35 + 0.35 + 0.175, TURN_CW(DEG2RAD(0)));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(load_second_figure);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(load_second_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
+ 0.85, NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(go_out_second_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(CATCH);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(go_out_second_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.175,
+ 0.85, NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(place_second_figure);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(place_second_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.175,
+ 0.15 + ROBOT_AXIS_TO_FIGURE_CENTER_M, ARRIVE_FROM(DEG2RAD(-90), 0.50));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_second_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(OPEN);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(leave_second_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 0.7 + 0.175,
+ 0.7, NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(aproach_third_figure);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(CLOSE);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(aproach_third_figure)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.7 + 0.35, 0.7);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.7 + 0.35 + 0.175, 0.7 + 0.175);
+ robot_trajectory_add_point_trans(
+ 0.45 + 0.35 + 0.25, 1.4 + 0.1);
+ robot_trajectory_add_final_point_trans(
+ 2, 1.5,
+ ARRIVE_FROM(DEG2RAD(-90), 0.20));
+ break;
+ case EV_START:
+ case EV_TIMER:
+ FSM_TRANSITION(next_state);
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ act_jaws(OPEN);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(next_state)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 1.4 + 0.175, 0.12 + 0.1 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
+ NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(last_state);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(last_state)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.45 + 1.4 + 0.175, 1,
+ NO_TURN());
+ break;
+ case EV_START:
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ case EV_MOTION_DONE:
+ break;
+ case EV_MOTION_ERROR:
+ case EV_EXIT:
+ break;
+ }
+}
+
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
+int main()
+{
+ int rv;
+
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ robot.obstacle_avoidance_enabled = true;
+
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ //robot.fsm.act.debug_states = 1;
+
+ robot.fsm.main.state = &fsm_state_main_init;
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+ robot_destroy();
+
+ return 0;
+}
+
+/************************************************************************
+ * STATE SKELETON
+ ************************************************************************/
+
+/*
+FSM_STATE()
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_VIDLE_DONE:
+ case EV_ACTION_ERROR:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+*/
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_all_our_figures); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(bypass_figure);
+
+FSM_STATE(start_pick_all_our_figures)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ FSM_TRANSITION(bypass_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(start_pick_two_our_figures);
+ break;
+ default:;
+ }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_second_green_figure);
+FSM_STATE_DECL(pick_third_green_figure);
+FSM_STATE_DECL(pick_fourth_green_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+
+FSM_STATE(bypass_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_second_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_second_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_second_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_third_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_third_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_third_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_fourth_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_fourth_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_fourth_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(move_around_opp_squares);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(move_around, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ default: break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_center_figure); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(pick_center_figure);
+
+FSM_STATE(start_pick_center_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ FSM_TRANSITION(pick_center_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(start_pick_all_our_figures);
+ break;
+ default:;
+ }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_center_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+FSM_STATE_DECL(pick_opp_bonus_figure);
+
+FSM_STATE(pick_center_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_center_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(move_around_opp_squares);
+ default: break;
+ }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(move_around, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_opp_bonus_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_opp_bonus_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_opp_bonus_figure, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ default: break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_fourth_figure); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(bypass_figure);
+
+FSM_STATE(start_pick_fourth_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ FSM_TRANSITION(bypass_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(start_pick_center_figure);
+ break;
+ default:;
+ }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_fourth_green_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+FSM_STATE_DECL(pick_opp_bonus_figure);
+
+FSM_STATE(bypass_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_fourth_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_fourth_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_fourth_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(move_around_opp_squares);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(move_around, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_opp_bonus_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_opp_bonus_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_opp_bonus_figure, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ default: break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_third_figure); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(bypass_figure);
+
+FSM_STATE(start_pick_third_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ FSM_TRANSITION(bypass_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(start_pick_fourth_figure);
+ break;
+ default:;
+ }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_third_green_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+FSM_STATE_DECL(pick_opp_bonus_figure);
+
+FSM_STATE(bypass_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_third_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_third_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_third_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(move_around_opp_squares);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(move_around, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_opp_bonus_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_opp_bonus_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_opp_bonus_figure, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ default: break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_two_our_figures); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(bypass_figure);
+
+FSM_STATE(start_pick_two_our_figures)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ FSM_TRANSITION(bypass_figure);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ read_sharp();
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(start_pick_third_figure);
+ break;
+ default:;
+ }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_second_green_figure);
+FSM_STATE_DECL(pick_third_green_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+FSM_STATE_DECL(pick_opp_bonus_figure);
+
+FSM_STATE(bypass_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_second_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_second_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_second_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_third_green_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_third_green_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_third_green_figure, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(move_around_opp_squares);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(move_around, NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_opp_bonus_figure);
+ break;
+ default: break;
+ }
+}
+
+FSM_STATE(pick_opp_bonus_figure)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_opp_bonus_figure, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ default: break;
+ }
+}
\ No newline at end of file
"main" : {
"EV_START" : "Changed state of start connector.",
- "EV_VIDLE_DONE" : "",
+ "EV_JAWS_DONE" : "",
+ "EV_LIFT_DONE" : "",
"EV_SWITCH_STRATEGY" : "",
"EV_MOTION_DONE" : "Previously submitted motion is finished",
/*
- * robot_eb2008.c 08/04/20
+ * robot_demo2011.c
*
* Robot's generic initialization and clean up functions.
*
#include "map_handling.h"
#include <string.h>
#include "actuators.h"
-#include "corns_configs.h"
#include <robodim.h>
#include <ul_log.h>
#define MOTION_CONTROL_INIT_ONLY
#include "motion-control.h"
+#include "robot.h"
/* Global definition of robot structure */
struct robot robot;
void fill_in_known_areas_in_map()
{
- /* Do not plan path close to edges */
- //ShmapSetRectangleFlag(0.0, 0.0, 0.26, 2.1, MAP_FLAG_WALL, 0); /* left */
- //ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.26, MAP_FLAG_WALL, 0); /* bottom */
- //ShmapSetRectangleFlag(0.0, 1.84, 3.0, 2.1, MAP_FLAG_WALL, 0); /* top */
- //ShmapSetRectangleFlag(2.74, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); /* right */
-
- /* Container surroundings */
- ShmapSetRectangleFlag(0.0, 0.0, 0.4, 0.2, 0, MAP_FLAG_WALL); /* left container */
- ShmapSetRectangleFlag(3.0, 0.0, 2.6, 0.2, 0, MAP_FLAG_WALL); /* right container */
-
+ /*----- playground width 3.0 m and playground height 2.0 m -----*/
/* Ignore other obstacles at edges */
- ShmapSetRectangleFlag(0.0, 0.0, 0.09, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* left */
+ ShmapSetRectangleFlag(0.0, 0.0, 0.40, 2.0, MAP_FLAG_IGNORE_OBST, 0); /* left */
ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.09, MAP_FLAG_IGNORE_OBST, 0); /* bottom */
- ShmapSetRectangleFlag(0.0, 2.01, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* top */
- ShmapSetRectangleFlag(2.91, 0.0, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* right */
- ShmapSetRectangleFlag(1.0, 1.5, 1.95, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* rised area */
-
- ShmapSetRectangleFlag(0.6, 1.45, 2.45, 1.55, MAP_FLAG_WALL, 0); /* plateau, slopes */
- ShmapSetRectangleFlag(1.46, PLAYGROUND_HEIGHT_M - 0.5, 1.57, PLAYGROUND_HEIGHT_M, MAP_FLAG_WALL, 0); /* plateau, slopes */
+ ShmapSetRectangleFlag(0.0, 1.91, 3.0, 2.0, MAP_FLAG_IGNORE_OBST, 0); /* top */
+ ShmapSetRectangleFlag(2.6, 0.0, 3.0, 2.0, MAP_FLAG_IGNORE_OBST, 0); /* right */
+
+ /* Small walls that cannot be detected by hokuyo */
+ ShmapSetRectangleFlag(0.0, 1.49, 0.39, 1.432, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(3.0, 1.49, 2.61, 1.432, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(0.325, 0.0, 0.38, 0.74, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(2.675, 0.0, 2.62, 0.74, MAP_FLAG_WALL, 0);
- /* corns */
- struct corns_group *corns = get_all_corns(0, 0);
- struct corn * corn;
- for(corn = corns->corns; corn < &corns->corns[corns->corns_count]; corn++) {
- ShmapSetCircleFlag(corn->position.x, corn->position.y, CORN_NEIGHBOURHOOD_RADIUS_M, MAP_FLAG_WALL, 0);
- }
- dispose_corns_group(corns); // robot.corns will be set later, when the corns' configuration is known
-
- ShmapSetRectangleFlag(0.32, 0.25, 0.38, 0.55, 0, MAP_FLAG_WALL); /* destination position near blue container */
- ShmapSetRectangleFlag(2.62, 0.25, 2.68, 0.55, 0, MAP_FLAG_WALL); /* destination position near yellow container */
+ /* Palm tree */
+ //ShmapSetCircleFlag(1.5, 1.0, 0.075, MAP_FLAG_WALL, 0);
+
+ /* Totems and palm tree */
+ ShmapSetRectangleFlag(0.975, 0.875, 2.025, 1.125, MAP_FLAG_WALL, 0);
+ ShmapSetCircleFlag(1.1, 1.0, 0.3, MAP_FLAG_WALL, 0);
+ ShmapSetCircleFlag(2.0, 1.0, 0.3, MAP_FLAG_WALL, 0);
+
+ //ShmapSetRectangleFlag(1.775, 0.875, 2.025, 1.125, MAP_FLAG_WALL, 0);
}
static void trans_callback(struct robo_fsm *fsm)
strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
ORTEPublicationSend(robot.orte.publication_fsm_act);
}
-
+
}
-/**
+/**
* Initializes the robot.
* Setup fields in robot structure, initializes FSMs and ORTE.
- *
+ *
* @return 0
*/
int robot_init()
{
int rv;
pthread_mutexattr_t mattr;
-
rv = pthread_mutexattr_init(&mattr);
#ifdef HAVE_PRIO_INHERIT
rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
#endif
pthread_mutex_init(&robot.lock, &mattr);
pthread_mutex_init(&robot.lock_ref_pos, &mattr);
- pthread_mutex_init(&robot.lock_est_pos_uzv, &mattr);
pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);
pthread_mutex_init(&robot.lock_meas_angles, &mattr);
pthread_mutex_init(&robot.lock_disp, &mattr);
fsm_main_loop_init(&robot.main_loop);
-
+
/* FSM initialization */
- /* fsm_init(&robot.fsm.main, "main", &robot.main_loop);
- fsm_init(&robot.fsm.motion, "motion", &robot.main_loop);
- fsm_init(&robot.fsm.display, "display", &robot.main_loop);
- fsm_init(&robot.fsm.act, "actuators", &robot.main_loop); */
fsm_init(&robot.fsm.main, "MAIN", &robot.main_loop);
fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop);
fsm_init(&robot.fsm.act, "\tACT", &robot.main_loop);
robot.fsm.act.transition_callback = trans_callback;
robot.fsm.motion.transition_callback = trans_callback;
- robot.team_color = BLUE;
+ robot.team_color = VIOLET;
- if (robot.team_color == YELLOW) {
- ul_loginf("We are YELLOW!\n");
+ if (robot.team_color == VIOLET) {
+ ul_loginf("We are VIOLET!\n");
} else {
- ul_loginf("We are BLUE!\n");
+ ul_loginf("We are RED!\n");
}
- robot_set_est_pos_trans(0.16, PLAYGROUND_HEIGHT_M - 0.16, DEG2RAD(-45));
-
+ robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
+
robot.ignore_hokuyo = false;
robot.map = ShmapInit(1);
fill_in_known_areas_in_map();
robot.orte.pwr_ctrl.voltage80 = 1;
robot.orte.camera_control.on = true;
-
+
robot.fsm.motion.state = &fsm_state_motion_init;
/* Only activate display if it is configured */
*/
robot.obstacle_avoidance_enabled = true;
- robot.use_back_switch = true;
- robot.use_left_switch = true;
- robot.use_right_switch = true;
+ robot.use_back_bumpers = true;
+ robot.use_left_bumper = true;
+ robot.use_right_bumper = true;
robot.start_state = POWER_ON;
robot.check_turn_safety = true;
return rv;
}
-/**
+/**
* Starts the robot FSMs and threads.
*
* @return 0
perror("robot_start: pthread_attr_setschedparam()");
goto err;
}
- rv = pthread_create(&thr_obstacle_forgeting,
+ rv = pthread_create(&thr_obstacle_forgeting,
&tattr, thread_obstacle_forgeting, NULL);
if (rv) {
perror("robot_start: pthread_create");
return rv;
}
-/**
+/**
* Signals all the robot threads to finish.
*/
void robot_exit()
fsm_exit(&robot.fsm.act);
}
-/**
+/**
* Stops the robot. All resources alocated by robot_init() or
* robot_start() are dealocated here.
*/
motion_control_done();
// FIXME: set actuators to well defined position (FJ)
-
+
robottype_roboorte_destroy(&robot.orte);
fsm_destroy(&robot.fsm.main);
* Competition parameters
*/
enum team_color {
- BLUE = 0,
- YELLOW
+ VIOLET = 0, /* Coordinate transformation does not apply */
+ RED /* Coordinate transformation applies (in *_trans() functions) */
};
enum robot_start_state {
#define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id)
/**
- * LOCKING MANIPULATION
+ * LOCKING MANIPULATION
*/
#ifdef CONFIG_LOCK_CHECKING
#define __LOCK_CHECK(mutex)
#define __UNLOCK_CHECK(mutex)
#endif
-/**
+/**
* Locks the robot structure.
* @param var Field in the structure you are going to work with.
*/
__LOCK_CHECK(&robot.__robot_lock_##var); \
} while(0)
-/**
+/**
* Unlocks the robot structure.
* @param var Field in the structure, which was locked by ROBOT_LOCK.
*/
/* Mapping of robot structure fields to locks */
//#define __robot_lock_ lock /* ROBOT_LOCK() */
#define __robot_lock_ref_pos lock_ref_pos
-#define __robot_lock_est_pos_uzv lock_est_pos_uzv
#define __robot_lock_est_pos_odo lock_est_pos_odo
#define __robot_lock_est_pos_indep_odo lock_est_pos_indep_odo
#define __robot_lock_joy_data lock_joy_data
#define __robot_lock_hokuyo lock
#define __robot_lock_cmu lock
#define __robot_lock_bumper lock
-#define __robot_lock_drives lock
#define __robot_lock_disp lock_disp
#define __robot_lock_motion_irc lock
#define __robot_lock_odo_data lock
COMPONENT_POWER,
COMPONENT_HOKUYO,
COMPONENT_START,
- COMPONENT_VIDLE,
-
+ COMPONENT_JAWS,
+ COMPONENT_LIFT,
+
ROBOT_COMPONENT_NUMBER
};
struct robot {
pthread_mutex_t lock;
pthread_mutex_t lock_ref_pos;
- pthread_mutex_t lock_est_pos_uzv;
pthread_mutex_t lock_est_pos_odo;
pthread_mutex_t lock_est_pos_indep_odo;
pthread_mutex_t lock_meas_angles;
/** Temporary storage for new trajectory. After the trajectory creation is
* finished, this trajectory is submitted to fsmmove. */
void *new_trajectory;
-
+
unsigned char isTrajectory;
sem_t start;
/* actual position */
struct robot_pos_type ref_pos;
/* estimated position */
- struct robot_pos_type est_pos_uzv;
struct robot_pos_type est_pos_odo;
struct robot_pos_type est_pos_indep_odo;
- /** True if localization data arrives correctly and therfore
- * localization runs */
- bool localization_works;
/** True if est_pos_odo is updated according to reception of motion_irc */
bool odometry_works;
/** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
bool indep_odometry_works;
-
- bool use_back_switch;
- bool use_left_switch;
- bool use_right_switch;
+
+ bool use_back_bumpers;
+ bool use_left_bumper;
+ bool use_right_bumper;
/** True iff at least one wheel rotates backward */
bool moves_backward;
* (taken as sudden zero velocities)
*/
bool motion_irc_received;
-
+
/* orte */
struct robottype_orte_data orte;
/** is set to true in separate thread when there is short time left */
bool short_time_to_end;
bool check_turn_safety;
+
+ float odo_cal_b;
+ float odo_cal_a;
+ float odo_distance_a;
+ float odo_distance_b;
}; /* robot */
extern struct robot robot;
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
int robot_init() __attribute__ ((warn_unused_result));
int robot_start() __attribute__ ((warn_unused_result));
void robot_exit();
void robot_destroy();
-
void robot_get_est_pos_trans(double *x, double *y, double *phi);
void robot_get_est_pos(double *x, double *y, double *phi);
void serial_comm(int status);
-
+
FSM_STATE_FULL_DECL(main, init);
FSM_STATE_FULL_DECL(motion, init);
FSM_STATE_FULL_DECL(disp, init);
#ifdef __cplusplus
}
-#endif
+#endif
/*Thread priorities*/
#define THREAD_PRIO_TRAJ_FOLLOWER 90 /* As high as possible */
#include <math.h>
#include <robomath.h>
#include "map_handling.h"
+#include "match-timing.h"
#include <string.h>
#include <can_msg_def.h>
#include <actuators.h>
{
}
+void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct match_time_type *instance = (struct match_time_type *)vinstance;
+
+ if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
+ instance->time = 90;
+ } else {
+ instance->time = 90 - robot_current_time();
+ }
+}
/* ----------------------------------------------------------------------
* SUBSCRIBER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
break;
}
- dleft = ((robot.odo_data.left - instance->left) >> 8) * c; // TODO >> 8 ?
- dright = ((instance->right - robot.odo_data.right) >> 8) * c;
+ dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a; // TODO >> 8 ?
+ dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
dtang = (dleft + dright) / 2.0;
dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
ROBOT_LOCK(est_pos_indep_odo);
+ robot.odo_distance_a +=dleft;
+ robot.odo_distance_b +=dright;
double a = robot.est_pos_indep_odo.phi;
robot.est_pos_indep_odo.x += dtang*cos(a);
robot.est_pos_indep_odo.y += dtang*sin(a);
double dleft, dright, dtang, dphi;
static bool first_run = true;
/* spocitat prevodovy pomer */
- const double n = (double)(28.0 / 1.0);
+ const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
/* vzdalenost na pulz IRC */
- const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-
+ const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
switch (info->status) {
case NEW_DATA:
if (first_run) {
break;
}
- dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
- dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
+ /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
+ what is the right solution?
+ This was neccessary to view motor odometry correctly in robomon. */
+ dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
+ dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
+
dtang = (dleft + dright) / 2.0;
dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
* SUBSCRIBER CALLBACKS - EB2008
* ---------------------------------------------------------------------- */
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
- static int last_response = 0;
+ struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
+ static int last_response_left = 0;
+ static int last_response_right = 0;
switch (info->status) {
case NEW_DATA:
// new data arrived and requested position equals actual position
- if (instance->flags == 0)
- robot.status[COMPONENT_VIDLE]=STATUS_OK;
+ if (instance->flags.left == 0 &&
+ instance->flags.right == 0)
+ robot.status[COMPONENT_JAWS] = STATUS_OK;
else
- robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
+ robot.status[COMPONENT_JAWS] = STATUS_WARNING;
- if (instance->response != last_response &&
- instance->response == act_vidle_get_last_reqest())
- FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
- last_response = instance->response;
+ if (instance->response.left != last_response_left &&
+ instance->response.right != last_response_right &&
+ instance->response.left == act_jaw_left_get_last_reqest() &&
+ instance->response.left == act_jaw_right_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+
+ last_response_left = instance->response.left;
+ last_response_right = instance->response.right;
break;
case DEADLINE:
- robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
+ robot.status[COMPONENT_JAWS] = STATUS_FAILED;
DBG("ORTE deadline occurred - actuator_status receive\n");
break;
}
}
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct lift_status_type *instance = (struct lift_status_type *)vinstance;
+ static int last_response = 0;
+ switch (info->status) {
+ case NEW_DATA:
+ // new data arrived and requested position equals actual position
+ if (instance->flags == 0)
+ robot.status[COMPONENT_LIFT] = STATUS_OK;
+ else
+ robot.status[COMPONENT_LIFT] = STATUS_WARNING;
+
+ if (instance->response != last_response &&
+ instance->response == act_lift_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+ last_response = instance->response;
+ break;
+ case DEADLINE:
+ robot.status[COMPONENT_LIFT] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - actuator_status receive\n");
+ break;
+ }
+}
+
void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
- static bool last_left, last_right;
+ static bool last_strategy;
switch (info->status) {
case NEW_DATA:
robot.team_color = instance->team_color;
- if (instance->bumper_pressed)
+
+ if (!last_strategy && instance->strategy) {
+ /* strategy switching */
+ FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+ }
+ last_strategy = instance->strategy;
+ break;
+ case DEADLINE:
+ break;
+ }
+}
+
+void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
+ static bool last_left, last_right;
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
- if ((!last_left && instance->bumper_left) ||
- (!last_right && instance->bumper_right)) {
+
+ if (instance->bumper_left || instance->bumper_right) {
FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
- if (robot.start_state == POWER_ON) {
- /* Reuse VIDLE done for strategy switching */
- FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
- }
}
- last_right = instance->bumper_right;
- last_left = instance->bumper_left;
break;
case DEADLINE:
break;
{
int rv = 0;
- robot.orte.strength = 20;
-
rv = robottype_roboorte_init(&robot.orte);
if (rv)
return rv;
robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
+ robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
//???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
// I didn't know what was the difference between the callback function pointer
robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
- robottype_publisher_vidle_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
/* create generic subscribers */
robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
//robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
- robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_cb, &robot.orte);
+ robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
+ robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
- robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
+ robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
+ robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
return rv;
}
--- /dev/null
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+
+UL_LOG_CUST(ulogd_strategy_central_buillon_wait_for_start); /* Log domain name = ulogd + name of the file */
+static FSM_STATE_DECL(pick_central_buillon);
+
+
+FSM_STATE(central_buillon_wait_for_start)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ robot_calibrate_odometry();
+ FSM_TRANSITION(pick_central_buillon);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(homologation_wait_for_start);
+ break;
+ default:;
+ }
+}
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(bottle_bw);
+FSM_STATE_DECL(pick_buillon_from_totem1);
+FSM_STATE_DECL(pick_buillon_from_totem2);
+FSM_STATE_DECL(push_second_bottle);
+
+
+FSM_STATE(pick_central_buillon)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(approach_central_buillon,NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(bottle_bw);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(bottle_bw)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(push_bottle_bw,NULL);
+ up = false;
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(pick_buillon_from_totem1);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(pick_buillon_from_totem1)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ totem_x = 1.1;
+ totem_y = 0.875;
+ SUBFSM_TRANSITION(leave_home,NULL);
+ break;
+ case EV_RETURN:
+ FSM_TRANSITION(push_second_bottle);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(pick_buillon_from_totem2)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ up = true;
+ totem_x = 1.1;
+ totem_y = 1.125;
+ SUBFSM_TRANSITION(leave_home,NULL);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(push_second_bottle)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ SUBFSM_TRANSITION(go_back_from_home, NULL);
+ break;
+ case EV_RETURN:
+ break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#define FSM_MAIN
+#include "robodata.h"
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <sharp.h>
+#include <trgen.h>
+#include "match-timing.h"
+#include <stdbool.h>
+#include <ul_log.h>
+#include "common-states.h"
+
+UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
+
+
+FSM_STATE(homologation_wait_for_start)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ robot_calibrate_odometry();
+ FSM_TRANSITION(homologation_approach_buillon);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(calibrate);
+ break;
+ case EV_RETURN:
+ break;
+ default:;
+ }
+}
+
+FSM_STATE(homologation_approach_buillon)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ //robot.use_back_bumpers = false;
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.65,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+ robot_trajectory_add_point_trans(
+ 0.65,
+ 1.3);
+ robot_trajectory_add_final_point_trans(
+ 0.5,
+ 1.1,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ case EV_TIMER:
+ //robot.use_back_bumpers = true;
+ FSM_TRANSITION(homologation_push_bottle);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(homologation_push_bottle)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 0.7);
+ robot_trajectory_add_final_point_trans(
+ 0.64 + 0.08,
+ ROBOT_AXIS_TO_FRONT_M + 0.08,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
+ break;
+ case EV_MOTION_DONE:
+ break;
+ default:
+ break;
+ }
+}
\ No newline at end of file
--- /dev/null
+#define FSM_MAIN
+#include "robodata.h"
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <sharp.h>
+#include <trgen.h>
+#include "match-timing.h"
+#include <stdbool.h>
+#include <ul_log.h>
+#include "common-states.h"
+
+void robot_calibrate_odometry()
+{
+ robot.odo_distance_a = 0;
+ robot.odo_distance_b = 0;
+ FILE *file;
+ file = fopen ("odometry_cal_data", "r");
+ if (file == NULL)
+ {
+ robot.odo_cal_a = 1;
+ robot.odo_cal_b = 1;
+ fprintf(stderr, "ODO calibration file not found! \n\n");
+ return;
+ }
+ char line [15];
+ float a = 0;
+ float b = 0;
+ int num = 0;
+ while (fgets (line, 15 , file)) {
+ a += atof(strtok(line," "));
+ fgets (line, 15 , file);
+ b += atof(strtok(NULL," "));
+ num ++;
+ printf("a: %i, b: %i, num: %i\n", a, b, num);
+ }
+ fclose (file);
+ if(a == 0 || b == 0) {
+ robot.odo_cal_a = 1;
+ robot.odo_cal_b = 1;
+ return;
+ }
+ robot.odo_cal_a = a / num;
+ robot.odo_cal_b = b / num;
+ printf("ODO calibrated value A: %f\n",robot.odo_cal_a);
+ printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
+}
+
+UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */
+
+
+FSM_STATE(calibrate)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ start_entry();
+//#ifdef COMPETITION
+ ul_logmsg("waiting for start\n");
+ FSM_TIMER(1000);
+ break;
+//#endif
+ case EV_START:
+ start_go();
+ robot.use_back_bumpers = false;
+ robot.odo_cal_a = 1;
+ robot.odo_cal_b = 1;
+ FSM_TRANSITION(go_back_for_cal);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ start_timer();
+ break;
+ case EV_EXIT:
+ start_exit();
+ break;
+ case EV_SWITCH_STRATEGY:
+ FSM_TRANSITION(central_buillon_wait_for_start);
+ break;
+ default:;
+ }
+}
+
+/* State for odometry calibration */
+
+FSM_STATE(go_back_for_cal)
+{
+ double x1 = 0, y1 = 0;
+ char buffer [20];
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+ 0);
+ robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
+ FSM_TIMER(2000);
+ break;
+ case EV_MOTION_DONE:
+ ROBOT_LOCK(est_pos_odo);
+ robot.odo_cal_a = -1.0/robot.odo_distance_a;
+ robot.odo_cal_b = -1.0/robot.odo_distance_b;
+ x1 = robot.odo_distance_a;
+ y1 = robot.odo_distance_b;
+ ROBOT_UNLOCK(est_pos_odo);
+ if(x1 != 0 || y1 != 0) {
+ printf("Distance for calibration: \n");
+ printf("Left%f\n", x1);
+ printf("Right%f\n", y1);
+ FILE * file;
+ file = fopen ("odometry_cal_data","w");
+ sprintf(buffer, "%4.4f", -1/x1);
+ fputs (buffer,file);
+ fputs (" ", file);
+ sprintf(buffer, "%4.4f", -1/y1);
+ fputs (buffer,file);
+ fputs ("\n", file);
+ fclose(file);
+ }
+ FSM_TRANSITION(central_buillon_wait_for_start);
+ break;
+ case EV_TIMER:
+ ROBOT_LOCK(est_pos_odo);
+ if(x1 == robot.odo_distance_a){
+ x1 = robot.odo_distance_a;
+ y1 = robot.odo_distance_b;
+ //robot_stop();
+ FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
+ FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+ } else {
+ FSM_TIMER(10);
+ }
+ ROBOT_UNLOCK(est_pos_odo);
+ break;
+ default:
+ break;
+ }
+}
+
test_PROGRAMS += mcl-laser
mcl-laser_SOURCES = mcl-laser.cc
-test_PROGRAMS += test_vidle
-test_vidle_SOURCES = tune_vidle.cc ../common-states.cc
+#test_PROGRAMS += test_vidle
+#test_vidle_SOURCES = tune_vidle.cc ../common-states.cc
#test_PROGRAMS += homologation
#homologation_SOURCES = homologation.cc
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte \
robottype pthread rt m orte pathplan sharp map fsm \
- rbtree motion actlib cornslib ulut
+ rbtree motion actlib ulut shape_detect
break;
case EV_TIMER:
FSM_TRANSITION(robot_goto_test);
- if (!robot.localization_works) {
- robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
- PLAYGROUND_HEIGHT_M/2,
- DEG2RAD(0));
- }
+ robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
+ PLAYGROUND_HEIGHT_M/2,
+ DEG2RAD(0));
break;
default:;
}
break;
case EV_TIMER:
FSM_TRANSITION(rectangle);
- if (!robot.localization_works) {
- robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
- } else {
- // Set odo position
- robot_set_est_pos_trans(robot.est_pos_uzv.x, robot.est_pos_uzv.y, DEG2RAD(0));
- }
+ robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
break;
default:;
}
switch (FSM_EVENT) {
case EV_ENTRY:
robot_trajectory_new(NULL);
- robot_trajectory_add_final_point_trans(1, 0.5,
+ robot_trajectory_add_final_point_trans(1, 0.5,
TURN_CCW(DEG2RAD(0)));
break;
case EV_MOTION_DONE:
--- /dev/null
+/*
+ * Map.cpp 11/01/31
+ *
+ * Draw a map on the playground.
+ *
+ * Copyright: (c) 2007 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * Authors: Michal Vokac, Michal Sojka
+ * License: GNU GPL v.2
+ */
+
+#include <math.h>
+#include <QPen>
+
+#include "PlaygroundScene.h"
+#include "Map.h"
+#include <map.h>
+#include <robodim.h>
+#include <iostream>
+
+Map::Map() :
+ QObject(), QGraphicsItem()
+{
+ this->pen = pen;
+ this->brush = brush;
+ mapImage = QImage(MAP_WIDTH, MAP_HEIGHT, QImage::Format_ARGB32);
+ mapImage.fill(Qt::transparent);
+ setVisible(false);
+}
+
+Map::~Map()
+{
+}
+
+QRectF Map::boundingRect() const
+{
+ return QRectF(0,0,PLAYGROUND_WIDTH_MM,PLAYGROUND_HEIGHT_MM);
+}
+
+void Map::paint(QPainter *painter,
+ const QStyleOptionGraphicsItem *option, QWidget *widget)
+{
+ Q_UNUSED(option);
+ Q_UNUSED(widget);
+ Q_UNUSED(painter);
+
+ painter->drawImage(QPointF(0, 0), mapImage);
+}
+
+void Map::setPixelColor(int x, int y, QColor color)
+{
+ mapImage.setPixel(x, y, color.rgba());
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Map.h 11/01/31
+ *
+ * Draw a map on the playground.
+ *
+ * Copyright: (c) 2007 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * Authors: Michal Vokac, Michal Sojka
+ * License: GNU GPL v.2
+ */
+
+#ifndef MAP_H
+#define MAP_H
+
+#include <QGraphicsItem>
+#include <QPainter>
+#include <QObject>
+
+class Map : public QObject, public QGraphicsItem
+{
+ Q_OBJECT
+ Q_INTERFACES(QGraphicsItem);
+public:
+ Map();
+ ~Map();
+ QRectF boundingRect() const;
+ void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget);
+ void setPixelColor(int x, int y, QColor color);
+public slots:
+private:
+ QImage mapImage;
+ QPen pen;
+ QBrush brush;
+};
+
+#endif
* Copyright: (c) 2007 CTU Dragons
* CTU FEE - Department of Control Engineering
* Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
- * Edit: Petr Kubiznak
+ * Michal Vokac, Petr Kubiznak
* License: GNU GPL v.2
*/
#include <QGraphicsSceneMouseEvent>
#include <QGraphicsScene>
#include <QGraphicsRectItem>
+#include <QImage>
+#include <QColor>
+#include <QBrush>
+
using namespace Qt;
#include "PlaygroundScene.h"
#include <robodim.h>
#include <math.h>
-#include "../robofsm/corns_configs.h"
//margin around borders (just for better look)
-#define MARGIN 40
+#define MARGIN 40
//playground borders
-#define BORDER_WIDTH 22
-#define BORDER_COLOR black
+#define BORDER_WIDTH 10
+#define BORDER_COLOR black
//playground
-#define PLAYGROUND_WIDTH 3000
-#define PLAYGROUND_HEIGHT 2100
-#define PLAYGROUND_COLOR green
-
-//squares
-#define SQUARE_WIDTH 350
-#define SQUARE_HEIGHT 350
-#define SQUARE_R_COLOR red
-#define SQUARE_B_COLOR blue
-
-//blackline
-#define BLINE_WIDTH 50
-#define BLINE_HEIGHT 2100
-#define BLINE_COLOR black
-
-//blocks
-#define BLOCK_WIDTH 400
-#define BLOCK_HEIGHT 22
-#define BLOCK_COLOR_R red
-#define BLOCK_COLOR_B blue
-
-//starting areas
-#define STARTAREA_WIDTH 400
-#define STARTAREA_HEIGHT 400
-#define STARTAREA_L_COLOR red
-#define STARTAREA_R_COLOR blue
-
-//dispensing areas
-#define DISPENSING_WIDTH 400
-#define DISPENSING_HEIGHT 1678
-#define DISPENSING_COLOR green
-
-//protected area borders
-#define PROTECTEDBORDER_WIDTH 22
-#define PROTECTEDBORDER_HEIGHT 130
-#define PROTECTEDBORDER_COLOR black
-
-//protected area big block
-#define PROTECTEDBLOCK_WIDTH 700
-#define PROTECTEDBLOCK_HEIGHT 120
-#define PROTECTEDBLOCK_COLOR black
-
-//bonus squares
-#define BONUS_WIDTH 100
-#define BONUS_COLOR black
-
-//pawns, kings, queens
-#define FIGURE_WIDTH 200
-#define PAWN_COLOR QBrush(QColor(255, 165, 0))
-#define KING_COLOR QBrush(QColor(50, 20, 250))
-#define QUEEN_COLOR QBrush(QColor(255, 20, 80))
-
-/**
- * Draws all corns using appropriate colors.
- * @param side_configuration is in range of 0 - 8
- * @param center_configuration is in range of 0 - 3
- */
-
-/* draws a bonus */
-void PlaygroundScene::putBonus(QGraphicsEllipseItem *g, int centerX, int centerY) {
- using namespace Qt;
- g = addEllipse(QRect(centerX - BONUS_WIDTH/2, centerY - BONUS_WIDTH/2, BONUS_WIDTH, BONUS_WIDTH), QPen(), QBrush(BONUS_COLOR));
- g->setZValue(3);
-}
-
-/* draws a pawn */
-void PlaygroundScene::putPawn(QGraphicsEllipseItem *g, int centerX, int centerY) {
- using namespace Qt;
- g = addEllipse(QRect(centerX - FIGURE_WIDTH/2, centerY - FIGURE_WIDTH/2, FIGURE_WIDTH, FIGURE_WIDTH), QPen(), QBrush(PAWN_COLOR));
- g->setZValue(3);
-}
-
-/* draws a king */
-void PlaygroundScene::putKing(QGraphicsEllipseItem *g, int centerX, int centerY) {
- using namespace Qt;
- g = addEllipse(QRect(centerX - FIGURE_WIDTH/2, centerY - FIGURE_WIDTH/2, FIGURE_WIDTH, FIGURE_WIDTH), QPen(), QBrush(KING_COLOR));
- g->setZValue(3);
-}
-
-/* draws a queen */
-void PlaygroundScene::putQueen(QGraphicsEllipseItem *g, int centerX, int centerY) {
- using namespace Qt;
- g = addEllipse(QRect(centerX - FIGURE_WIDTH/2, centerY - FIGURE_WIDTH/2, FIGURE_WIDTH, FIGURE_WIDTH), QPen(), QBrush(QUEEN_COLOR));
- g->setZValue(3);
-}
+#define PLAYGROUND_WIDTH PLAYGROUND_WIDTH_MM
+#define PLAYGROUND_HEIGHT PLAYGROUND_HEIGHT_MM
PlaygroundScene::PlaygroundScene(QObject *parent)
: QGraphicsScene(parent)
{
using namespace Qt;
- QGraphicsRectItem *tempRect;
+ QGraphicsRectItem *playgroundRect;
/* All scene units are milimeters */
setSceneRect(QRectF(QPointF(-MARGIN, -MARGIN), QPointF(PLAYGROUND_WIDTH+MARGIN, PLAYGROUND_HEIGHT+MARGIN)));
-
+
/* playground border */
addRect(QRect(0, 0, -BORDER_WIDTH, PLAYGROUND_HEIGHT), QPen(), QBrush(BORDER_COLOR)); //left
addRect(QRect(-BORDER_WIDTH, PLAYGROUND_HEIGHT, PLAYGROUND_WIDTH + 2*BORDER_WIDTH, BORDER_WIDTH), QPen(), QBrush(BORDER_COLOR)); //top
addRect(QRect(-BORDER_WIDTH, -BORDER_WIDTH, PLAYGROUND_WIDTH + 2*BORDER_WIDTH, BORDER_WIDTH), QPen(), QBrush(BORDER_COLOR)); //bottom
/* playground */
- tempRect = addRect(QRect(0, 0, PLAYGROUND_WIDTH, PLAYGROUND_HEIGHT), QPen(), PLAYGROUND_COLOR);
- tempRect->setZValue(0);
-
- /* squares */
- for(int i = 0; i<6; i++){
- for(int j = 0; j<6; j++){
- if(!((i+j)%2)){
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + i*SQUARE_WIDTH, PLAYGROUND_HEIGHT-(j + 1)*SQUARE_HEIGHT, SQUARE_HEIGHT, SQUARE_WIDTH), QPen(), SQUARE_B_COLOR);
- tempRect->setZValue(1);
- } else {
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + i*SQUARE_WIDTH, PLAYGROUND_HEIGHT-(j + 1)*SQUARE_HEIGHT, SQUARE_HEIGHT, SQUARE_WIDTH), QPen(), SQUARE_R_COLOR);
- tempRect->setZValue(1);
- }
- }
- }
-
- /* left protected area */
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH, 0, PROTECTEDBLOCK_WIDTH, PROTECTEDBLOCK_HEIGHT), QPen(), PROTECTEDBLOCK_COLOR);
- tempRect->setZValue(3);
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + 2*SQUARE_WIDTH + PROTECTEDBLOCK_WIDTH, 0, PROTECTEDBLOCK_WIDTH, PROTECTEDBLOCK_HEIGHT), QPen(), PROTECTEDBLOCK_COLOR);
- tempRect->setZValue(3);
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH, PROTECTEDBLOCK_HEIGHT, PROTECTEDBORDER_WIDTH, PROTECTEDBORDER_HEIGHT), QPen(), PROTECTEDBORDER_COLOR);
- tempRect->setZValue(3);
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + PROTECTEDBLOCK_WIDTH - PROTECTEDBORDER_WIDTH, PROTECTEDBLOCK_HEIGHT, PROTECTEDBORDER_WIDTH, PROTECTEDBORDER_HEIGHT), QPen(), PROTECTEDBORDER_COLOR);
- tempRect->setZValue(3);
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + 2*SQUARE_WIDTH + PROTECTEDBLOCK_WIDTH, PROTECTEDBLOCK_HEIGHT, PROTECTEDBORDER_WIDTH, PROTECTEDBORDER_HEIGHT), QPen(), PROTECTEDBORDER_COLOR);
- tempRect->setZValue(3);
- tempRect = addRect(QRect(DISPENSING_WIDTH + BLINE_WIDTH + 2*SQUARE_WIDTH + 2*PROTECTEDBLOCK_WIDTH - PROTECTEDBORDER_WIDTH, PROTECTEDBLOCK_HEIGHT, PROTECTEDBORDER_WIDTH, PROTECTEDBORDER_HEIGHT), QPen(), PROTECTEDBORDER_COLOR);
- tempRect->setZValue(3);
-
- /* left blackline */
- tempRect = addRect(QRect(DISPENSING_WIDTH, 0, BLINE_WIDTH, BLINE_HEIGHT), QPen(), BLINE_COLOR);
- tempRect->setZValue(3);
-
- /* right blackline */
- tempRect = addRect(QRect(PLAYGROUND_WIDTH-DISPENSING_WIDTH-BLINE_WIDTH, 0, BLINE_WIDTH, BLINE_HEIGHT), QPen(), BLINE_COLOR);
- tempRect->setZValue(3);
-
- /* left block */
- tempRect = addRect(QRect(0, DISPENSING_HEIGHT, BLOCK_WIDTH, BLOCK_HEIGHT), QPen(), BLOCK_COLOR_R);
- tempRect->setZValue(4);
-
- /* right block */
- tempRect = addRect(QRect(PLAYGROUND_WIDTH-BLOCK_WIDTH, DISPENSING_HEIGHT, BLOCK_WIDTH, BLOCK_HEIGHT), QPen(), BLOCK_COLOR_B);
- tempRect->setZValue(4);
-
- /* left dispensing area */
- tempRect = addRect(QRect(0, 0, DISPENSING_WIDTH, DISPENSING_HEIGHT), QPen(), DISPENSING_COLOR);
- tempRect->setZValue(3);
-
- /* right dispensing area */
- tempRect = addRect(QRect(PLAYGROUND_WIDTH-DISPENSING_WIDTH, 0, DISPENSING_WIDTH, DISPENSING_HEIGHT), QPen(), DISPENSING_COLOR);
- tempRect->setZValue(3);
-
- /* left starting area */
- tempRect = addRect(QRect(0, PLAYGROUND_HEIGHT-STARTAREA_HEIGHT, STARTAREA_WIDTH, STARTAREA_HEIGHT), QPen(NoPen), QBrush(STARTAREA_L_COLOR));
- tempRect->setZValue(3);
-
- /* right starting area */
- tempRect = addRect(QRect(PLAYGROUND_WIDTH-STARTAREA_WIDTH, PLAYGROUND_HEIGHT-STARTAREA_HEIGHT, STARTAREA_WIDTH, STARTAREA_HEIGHT), QPen(NoPen), QBrush(STARTAREA_R_COLOR));
- tempRect->setZValue(3);
-
- /* bonus squares */
- QGraphicsEllipseItem *tomato = NULL;
- for(int i = 0; i < BONUS_CNT; i++){
- putBonus(tomato, bonus[i].x, bonus[i].y);
- }
-
- /* pawns */
-// QGraphicsEllipseItem *pawn = NULL;
- for(int i = 0; i < PAWN_CNT; i++){
- putPawn(tomato, pawn[i].x, pawn[i].y);
- }
-
- /* kings */
-// QGraphicsEllipseItem *king = NULL;
- for(int i = 0; i < KING_CNT; i++){
- putKing(tomato, king[i].x, king[i].y);
- }
-
- /* queens */
-// QGraphicsEllipseItem *queen = NULL;
- for(int i = 0; i < QUEEN_CNT; i++){
- putQueen(tomato, queen[i].x, queen[i].y);
- }
+ playgroundRect = addRect(
+ QRect(0, 0, PLAYGROUND_WIDTH, PLAYGROUND_HEIGHT),
+ QPen(),
+ QPixmap(":/images/playground_eurobot2012_1024.png").scaled(PLAYGROUND_WIDTH, PLAYGROUND_HEIGHT)
+ );
+ playgroundRect->setZValue(0);
/* obstacles */
obstacle = new QGraphicsEllipseItem(0, 0, SIM_OBST_SIZE_M*1000, SIM_OBST_SIZE_M*1000);
obstacle->setVisible(false);
obstacle->setPos(QPointF(2000, 1000));
this->addItem(obstacle);
-
- initMap();
}
PlaygroundScene::~PlaygroundScene()
QPointF PlaygroundScene::scene2world(QPointF scenePos)
{
return QPointF((scenePos.x()) / 1000.0,
- (scenePos.y()) / 1000.0);
+ (scenePos.y()) / 1000.0);
}
else
obstacle->setVisible(false);
}
-
-void PlaygroundScene::initMap()
-{
-#define MAP_RECT_SCALE 1.0
- this->map = new QGraphicsItemGroup();
- for(int i=0; i < MAP_WIDTH; i++) {
- for(int j=0; j<MAP_HEIGHT; j++) {
- rects[i][j] = new QGraphicsRectItem(
- QRectF(i*MAP_CELL_SIZE_MM + MAP_CELL_SIZE_MM * (1 - MAP_RECT_SCALE)/2,
- (MAP_HEIGHT-1-j)*MAP_CELL_SIZE_MM + MAP_CELL_SIZE_MM * (1 - MAP_RECT_SCALE)/2,
- MAP_CELL_SIZE_MM * MAP_RECT_SCALE ,
- MAP_CELL_SIZE_MM * MAP_RECT_SCALE));
- rects[i][j]->setPen(QPen(Qt::NoPen));
- rects[i][j]->setBrush(QBrush(Qt::lightGray));
- map->addToGroup(rects[i][j]);
- }
- }
- map->setVisible(false);
- map->setZValue(4);
- addItem(map);
-}
-
-void PlaygroundScene::showMap(bool show)
-{
- map->setVisible(show);
-}
-
-void PlaygroundScene::setMapColor(int x, int y, QColor color)
-{
- color.setAlpha(200);
- rects[x][y]->setBrush(QBrush(color));
-}
#define PLAYGROUND_SCENE_H
#include <QGraphicsScene>
+#include <QPainter>
#include <map.h>
#define SIM_OBST_SIZE_M 0.3
~PlaygroundScene();
static QPointF scene2world(QPointF scenePos);
static QPointF world2scene(QPointF worldPos);
- void setMapColor(int x, int y, QColor color);
-
signals:
void obstacleChanged(QPointF pos);
void mouseMoved(QPointF pos);
virtual void mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent);
private:
- void initMap();
- QGraphicsItemGroup *map;
- QGraphicsRectItem *rects[MAP_WIDTH][MAP_HEIGHT]; // This must be slow - convert to pixmap. -- M.S.
QGraphicsEllipseItem *obstacle;
void paintCorns(int side_configuration, int center_configuration);
void putPawn(QGraphicsEllipseItem *g, int centerX, int centerY);
void putKing(QGraphicsEllipseItem *g, int centerX, int centerY);
void putQueen(QGraphicsEllipseItem *g, int centerX, int centerY);
-
+
public slots:
void showObstacle(int val);
- void showMap(bool show);
};
#endif
createOrte();
createRobots();
createActions();
+ createMap();
// connect(vidle, SIGNAL(valueChanged(int)),
// robotEstPosBest, SLOT(setVidle(int)));
colorChoser = new QCheckBox("&Team color");
layout->addWidget(colorChoser);
+
+ strategyButton= new QPushButton(tr("Strategy"));
+ layout->addWidget(strategyButton);
miscGroupBox->setLayout(layout);
}
powerGroupBox->setLayout(layout);
}
-#if 0
-void RobomonAtlantis::createMotorsGroupBox()
-{
- enginesGroupBox = new QGroupBox(tr("Motors"));
- QVBoxLayout *layout = new QVBoxLayout();
- QHBoxLayout *layout1 = new QHBoxLayout();
- QHBoxLayout *layout2 = new QHBoxLayout();
-
- leftMotorSlider = new QSlider(Qt::Vertical);
- rightMotorSlider = new QSlider(Qt::Vertical);
- bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
- stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
-
- leftMotorSlider->setMinimum(-100);
- leftMotorSlider->setMaximum(100);
- leftMotorSlider->setTracking(false);
- leftMotorSlider->setTickPosition(QSlider::TicksLeft);
-
- rightMotorSlider->setMinimum(-100);
- rightMotorSlider->setMaximum(100);
- rightMotorSlider->setTracking(false);
- rightMotorSlider->setTickPosition(QSlider::TicksRight);
-
- stopMotorsPushButton->setMaximumWidth(90);
-
- layout1->addWidget(leftMotorSlider);
- layout1->addWidget(MiscGui::createLabel("0"));
- layout1->addWidget(rightMotorSlider);
-
- layout2->addWidget(bothMotorsCheckBox);
-
- layout->addWidget(MiscGui::createLabel("100"));
- layout->addLayout(layout1);
- layout->addWidget(MiscGui::createLabel("-100"));
- layout->addLayout(layout2);
- layout->addWidget(stopMotorsPushButton);
- enginesGroupBox->setLayout(layout);
-}
-#endif
-
void RobomonAtlantis::createRobots()
{
robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
}
+void RobomonAtlantis::createMap()
+{
+ mapImage = new Map();
+ mapImage->setZValue(5);
+ mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
+
+
+ playgroundScene->addItem(mapImage);
+}
+
/**********************************************************************
* GUI actions
**********************************************************************/
connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
+ connect(strategyButton, SIGNAL(pressed()), this, SLOT(changeStrategy_1()));
+ connect(strategyButton, SIGNAL(released()), this, SLOT(changeStrategy_0()));
/* obstacle simulation */
simulationEnabled = 0;
this, SLOT(setMotorSimulation(int)));
}
+void RobomonAtlantis::changeStrategy_1()
+{
+ orte.robot_switches.strategy = true;
+ ORTEPublicationSend(orte.publication_robot_switches);
+}
+
+void RobomonAtlantis::changeStrategy_0()
+{
+ orte.robot_switches.strategy = false;
+ ORTEPublicationSend(orte.publication_robot_switches);
+}
+
void RobomonAtlantis::setVoltage33(int state)
{
if (state)
disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
}
}
- playgroundScene->showMap(show);
+ mapImage->setVisible(show);
}
void RobomonAtlantis::paintMap()
if (!map) return;
- for(int i=0; i < MAP_WIDTH; i++) {
- for(int j=0; j<MAP_HEIGHT; j++) {
+ for(int i = 0; i < MAP_WIDTH; i++) {
+ for(int j = 0; j < MAP_HEIGHT; j++) {
QColor color;
struct map_cell *cell = &map->cells[j][i];
if (cell->flags & MAP_FLAG_DET_OBST)
color = cyan;
- playgroundScene->setMapColor(i, j, color);
+ color.setAlpha(200);
+ mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
}
}
}
for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
wall_distance = distanceToWallHokuyo(i);
- distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
+
+ distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
if (wall_distance < distance)
distance = wall_distance;
hokuyo[i] = distance*1000;
case QEVENT(QEV_HOKUYO_SCAN):
hokuyoScan->newScan(&orte.hokuyo_scan);
break;
- case QEVENT(QEV_VIDLE_CMD):
- robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
+ case QEVENT(QEV_JAWS_CMD):
+ robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left);
+ robotRefPos->setJaws(orte.jaws_cmd.req_pos.left);
+ robotEstPosIndepOdo->setJaws(orte.jaws_cmd.req_pos.left);
+ robotEstPosOdo->setJaws(orte.jaws_cmd.req_pos.left);
break;
case QEVENT(QEV_REFERENCE_POSITION):
emit actualPositionReceivedSignal();
{
int rv;
- orte.strength = 11;
-
memset(&orte, 0, sizeof(orte));
rv = robottype_roboorte_init(&orte);
if (rv) {
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
robottype_subscriber_hokuyo_scan_create(&orte,
generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
- robottype_subscriber_vidle_cmd_create(&orte,
- generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD));
+ robottype_subscriber_jaws_cmd_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD));
robottype_subscriber_fsm_main_create(&orte,
rcv_fsm_main_cb, this);
robottype_subscriber_fsm_motion_create(&orte,
return min_distance;
}
+double RobomonAtlantis::distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter)
+{
+ struct robot_pos_type e = orte.est_pos_best;
+ double sensor_a;
+ struct sharp_pos s;
+
+ s.x = HOKUYO_CENTER_OFFSET_M;
+ s.y = 0.0;
+ s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+
+ Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
+ e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
+ sensor_a = e.phi + s.ang;
+
+ const double sensorRange = 4.0; /*[meters]*/
+
+ double distance = sensorRange;
+ double angle;
+
+ angle = sensor.angleTo(center) - sensor_a;
+ angle = fmod(angle, 2.0*M_PI);
+ if (angle > +M_PI) angle -= 2.0*M_PI;
+ if (angle < -M_PI) angle += 2.0*M_PI;
+ angle = fabs(angle);
+
+ double k = tan(sensor_a);
+ double r = diameter / 2.0;
+
+ double A = 1 + k*k;
+ double B = 2 * (sensor.y*k - center.x - k*k*sensor.x - center.y*k);
+ double C = center.x*center.x + center.y*center.y +
+ k*k*sensor.x*sensor.x - 2*sensor.y*k*sensor.x +
+ sensor.y*sensor.y + 2*k*sensor.x*center.y -
+ 2*sensor.y*center.y - r*r;
+
+ double D = B*B - 4*A*C;
+
+ if (D > 0) {
+ Point ob1, ob2;
+
+ ob1.x = (-B + sqrt(D)) / (2*A);
+ ob2.x = (-B - sqrt(D)) / (2*A);
+ ob1.y = k * (ob1.x - sensor.x) + sensor.y;
+ ob2.y = k * (ob2.x - sensor.x) + sensor.y;
+
+ double distance1 = sensor.distanceTo(ob1);
+ double distance2 = sensor.distanceTo(ob2);
+ distance = (distance1 < distance2) ? distance1 : distance2;
+ } else if (D == 0) {
+ Point ob;
+ ob.x = -B / (2*A);
+ ob.y = k * (ob.x - sensor.x) + sensor.y;
+ distance = sensor.distanceTo(ob);
+ }
+ distance = distance + (drand48()-0.5)*3.0e-2;
+ if (D < 0 || angle > atan(r / distance))
+ distance = sensorRange;
+ if (distance > sensorRange)
+ distance = sensorRange;
+
+ return distance;
+}
+
/**
* Calculation for Hokuyo simulation. Calculates distance that would
* be returned by Hokuyo sensors, if there is only one obstacle (as
#include "PlaygroundScene.h"
#include "playgroundview.h"
#include "Robot.h"
+#include "Map.h"
#include <roboorte_robottype.h>
#include "trail.h"
#include "hokuyoscan.h"
class QSlider;
class QProgressBar;
class QFont;
+class QImage;
class MotorSimulation : QObject {
Q_OBJECT
void motionStatusReceivedSignal();
void actualPositionReceivedSignal();
void powerVoltageReceivedSignal();
-
+
public slots:
void showMap(bool show);
void useOpenGL(bool use);
void resetTrails();
private slots:
/************************************************************
- * GUI actions
+ * GUI actions
************************************************************/
void setVoltage33(int state);
void setVoltage50(int state);
void changeObstacle(QPointF position);
void sendStart(int plug);
void setTeamColor(int plug);
+ void changeStrategy_1();
+ void changeStrategy_0();
void setMotorSimulation(int state);
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void motionStatusReceived();
void actualPositionReceived();
void createRobots();
void createActions();
+ void createMap();
QVBoxLayout *leftLayout;
QVBoxLayout *rightLayout;
QLabel *fsm_motion_state;
QCheckBox *startPlug;
QCheckBox *colorChoser;
+ QPushButton *strategyButton;
public:
/* robot */
Robot *robotRefPos;
Robot *robotEstPosBest;
Robot *robotEstPosIndepOdo;
Robot *robotEstPosOdo;
+
+ Map *mapImage;
private:
Trail *trailRefPos;
Trail *trailEstPosBest;
void openSharedMemory();
bool sharedMemoryOpened;
QTimer *mapTimer;
-
+
/* obstacle simulation */
double distanceToWallHokuyo(int beamnum);
double distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize);
+ double distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter);
int simulationEnabled;
QTimer *obstacleSimulationTimer;
class MotorSimulation motorSimulation;
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void createOrte();
**********************************************************************/
void RobomonTuning::createOrte()
{
-
- orte.strength = 12;
-
+ memset(&orte, 0, sizeof(orte));
if (robottype_roboorte_init(&orte) != 0) {
perror("robottype_roboorte_init");
exit(1);
this->pen = pen;
this->brush = brush;
setVisible(false);
- setVidle(0);
- moveRobot(1, 1, 0);
+ setJaws(JAW_LEFT_CLOSE);
+ moveRobot(ROBOT_START_X_M, ROBOT_START_Y_M, 0);
}
Robot::~Robot()
QRectF Robot::boundingRect() const
{
- return QRectF(0, -ROBOT_VIDLE_LENGTH_M*1000,
- ROBOT_WIDTH_MM, ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM + ROBOT_VIDLE_LENGTH_M*1000);
+ return QRectF(0, 0,
+ ROBOT_WIDTH_MM, ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM);
}
void Robot::paint(QPainter *painter,
QLineF(xa, yb, xa-xd, yc),
QLineF(xa, yb, xa+xd, yc)};
painter->drawLines(arrow, 3);
- if (vidle != 0) {
- const float vidle_angle = (float)(vidle - VIDLE_DOWN)/(VIDLE_UP-VIDLE_DOWN)*M_PI/2.0;
- const float y = -ROBOT_VIDLE_LENGTH_M*1000*cos(vidle_angle);
- QLineF vidle[] = {
- QLineF(0*ROBOT_WIDTH_MM/3, 0, 0*ROBOT_WIDTH_MM/3, y),
- QLineF(1*ROBOT_WIDTH_MM/3, 0, 1*ROBOT_WIDTH_MM/3, y),
- QLineF(2*ROBOT_WIDTH_MM/3, 0, 2*ROBOT_WIDTH_MM/3, y),
- QLineF(3*ROBOT_WIDTH_MM/3, 0, 3*ROBOT_WIDTH_MM/3, y)};
- painter->drawLines(vidle, 4);
+
+ QLineF jawsLines[2]; /* field witch jaws lines */
+ const int offset = 40; /* offset of point of rotation from robot sides [mm] */
+ const int hold = 40; /* offset from open position [mm] */
+ const int close = 110; /* offset from open positon [mm] */
+ const double y_hold = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(hold, 2));
+ const double y_close = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(close, 2));
+
+ switch (jawsPosition) {
+ case JAW_LEFT_OPEN:
+ jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+ jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+ break;
+
+ case JAW_LEFT_CATCH:
+ jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + hold, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+ jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - hold, ROBOT_HEIGHT_MM + y_hold);
+ break;
+
+ case JAW_LEFT_CLOSE:
+ jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + close, ROBOT_HEIGHT_MM + y_close);
+ jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - close, ROBOT_HEIGHT_MM + y_close);
+ break;
}
-
+
+ QPen penThick(Qt::black, 20, Qt::SolidLine); /* new pen with 20px thickness for jaws drawing */
+ painter->setPen(penThick);
+ painter->drawLines(jawsLines, 2); /* draw jaws */
+ painter->setPen(pen); /* set pen to previous slim pen */
+
painter->setFont(QFont("Arial", 40, 0, 0));
QTransform mirror;
mirror.scale(1,-1);
setVisible(show);
}
-void Robot::setVidle(int value)
+void Robot::setJaws(int value)
{
QRectF r = boundingRect();
//r.setBottom(0);
- vidle = value;
+ jawsPosition = value;
update(r);
}
void moveRobot(double x, double y, double angle);
public slots:
void mySetVisible(bool show);
- void setVidle(int value);
+ void setJaws(int value);
private:
- int vidle;
+ int jawsPosition;
QString text;
QPen pen;
QBrush brush;
{
Shape_detect sd;
- std::vector<int> input(HOKUYO_ARRAY_SIZE);
+ std::vector<Shape_detect::Line> lines;
+ std::vector<Shape_detect::Arc> arcs;
- for (unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
- input[i] = (int) data.data[i];
-
- std::vector<Shape_detect::Line> output;
- sd.shape_detect(input, output);
+ sd.prepare(data.data);
+ sd.line_detect(lines);
+ sd.arc_detect(arcs);
QPen pen(Qt::yellow);
pen.setWidth(20);
+ painter->setBrush(QBrush(Qt::NoBrush));
painter->setPen(pen);
- for (unsigned i = 0; i < output.size(); i++)
- painter->drawLine(output[i].a.x, output[i].a.y,
- output[i].b.x, output[i].b.y);
+ for (unsigned i = 0; i < lines.size(); i++)
+ painter->drawLine(lines[i].a.x, lines[i].a.y,
+ lines[i].b.x, lines[i].b.y);
+
+ QPen pen2(Qt::green);
+ pen2.setWidth(20);
+
+ painter->setPen(pen2);
+
+ for (unsigned i = 0; i < arcs.size(); i++) {
+ Shape_detect::Arc *a = &arcs[i];
+ painter->drawPoint(QPoint(a->center.x, a->center.y));
+ painter->drawEllipse(QRectF(a->center.x - a->radius, a->center.y - a->radius,
+ 2*a->radius, 2*a->radius));
+ }
}
void HokuyoScan::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
{
int d;
+ double ang;
QPointF points[HOKUYO_ARRAY_SIZE + 1];
unsigned point_num = 0;
QColor color;
for (unsigned i=0; i < HOKUYO_ARRAY_SIZE; i++) {
d = data.data[i];
- if (d > 5600)
- d = 5600;
+
+ ang = HOKUYO_INDEX_TO_RAD(i);
+
+ if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI)))) {
+ continue;
+ }
+
+ if (d > 5600)
+ d = 5600;
if (d > 19) {
float x, y;
RobomonTuning.cpp \
PlaygroundScene.cpp \
Robot.cpp \
+ Map.cpp \
Widget.cpp \
GlWidget.cpp \
MiscGui.cpp \
RobomonTuning.h \
PlaygroundScene.h \
Robot.h \
+ Map.h \
Painter.h \
Widget.h \
GlWidget.h \
hokuyoscan.h
LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim -lrobomath -lroboorte \
- -lrobottype -lorte -lsharp -lmap -lactlib -lcornslib -lulut -lshape_detect
+ -lrobottype -lorte -lsharp -lmap -lactlib -lulut -lshape_detect
<!DOCTYPE RCC><RCC version="1.0">
<qresource>
+ <file>images/playground_eurobot2012_1024.png</file>
<file>images/robomon_recycling.png</file>
<file>images/robomon_explorer.png</file>
<file>images/robomon_atlantis.png</file>
QEV_FSM_ACT,
QEV_FSM_MOTION,
QEV_HOKUYO_SCAN,
- QEV_VIDLE_CMD,
+ QEV_JAWS_CMD,
};
class OrteCallbackInfo {
-Subproject commit 0d162fa62da77df8556024aad00392b842db9bef
+Subproject commit 8eace81fd4f1731c2020cd116ddc83d189f9fd8c
foreach $var (@ortegen) {
print OUTFILE << "(END)";
-void ${roboorte_name}_subscriber_$var->{"topic"}_create(struct ${roboorte_name}_orte_data *data, ORTERecvCallBack callback, void *arg);
+ORTESubscription *${roboorte_name}_subscriber_$var->{"topic"}_create(struct ${roboorte_name}_orte_data *data, ORTERecvCallBack callback, void *arg);
(END)
}
*/
#include "$library_name_prefix$roboorte_name.h"
+#include <stdlib.h>
/* ----------------------------------------------------------------------
* CREATE PUBLISHERS
($dmsec, $dsec) = POSIX::modf($var->{"deadline"});
$dmsec = POSIX::floor($dmsec*1000);
print OUTFILE << "(END)";
-void ${roboorte_name}_subscriber_$var->{"topic"}_create(struct ${roboorte_name}_orte_data *data, ORTERecvCallBack callback, void *arg)
+ORTESubscription * ${roboorte_name}_subscriber_$var->{"topic"}_create(struct ${roboorte_name}_orte_data *data, ORTERecvCallBack callback, void *arg)
{
-\tORTESubscription *s;
\tNtpTime deadline, minimumSeparation;
\t
\t$var->{"type"}_type_register($data_arg_name->$orte_domain);
\t
\tNtpTimeAssembFromMs(deadline, $dsec, $dmsec);
\tNtpTimeAssembFromMs(minimumSeparation, $msec, $mmsec);
-\ts = ORTESubscriptionCreate(
+\treturn ORTESubscriptionCreate(
\t\t\t$data_arg_name->$orte_domain, $subscription_mode, $subscription_type,
\t\t\t"$var->{"topic"}", "$var->{"type"}",
\t\t\t&$data_arg_name->$var->{"topic"}, &deadline, &minimumSeparation,
int ${roboorte_name}_roboorte_init(struct ${roboorte_name}_orte_data *data)
{
\tint rv = 0;
+\tconst char *s;
\tORTEDomainProp prop;
\tORTEInit();
-\tif ($data_arg_name->strength < 0)
+\tif ($data_arg_name->strength <= 0)
\t\t$data_arg_name->strength = $publication_strength;
+ if ((s = getenv("ORTE_STRENGTH"))) {
+ char *end;
+ long l = strtol(s, &end, 10);
+ if (s != end)
+ data->strength = l;
+ }
+
\tORTEVerbositySetOptions("ALL.0");
\tORTEDomainPropDefaultGet(&prop);
\tNTPTIME_BUILD(prop.baseProp.refreshPeriod, $refresh_period);
};
struct pwr_alert {
- octet value;
- boolean bat_low;
- boolean bat_critical;
- boolean bat_bye;
- boolean low_33;
- boolean low_50;
- boolean low_80;
+ octet value;
+ boolean bat_full;
+ boolean bat_mean;
+ boolean bat_low;
+ boolean bat_critical;
+ boolean alert_33;
+ boolean alert_50;
+ boolean alert_80;
};
struct robot_cmd {
};
struct robot_switches {
- boolean bumper_pressed;
+ octet team_color;
+ boolean strategy;
+};
+
+struct robot_bumpers {
boolean bumper_left;
boolean bumper_right;
- octet team_color;
+ boolean bumper_right_across;
+ boolean bumper_left_across;
+ boolean bumper_rear_left;
+ boolean bumper_rear_right;
};
struct can_msg {
unsigned short data[681];
};
-/** Request for hokuyo pitch (angle) */
-struct hokuyo_pitch {
- /** Hokuyo angle (0..down, 255..up)*/
- octet angle;
+struct pos_short {
+ unsigned short left;
+ unsigned short right;
+};
+
+struct pos_octet {
+ octet left;
+ octet right;
};
// Actuators
-struct vidle_cmd {
+struct jaws_cmd {
+ pos_short req_pos;
+ pos_octet speed;
+};
+
+struct lift_cmd {
unsigned short req_pos;
octet speed;
+ octet homing;
};
+
/** Status sent from actuators */
-struct vidle_status {
+struct jaws_status {
+ pos_short act_pos;
+ pos_short response;
+ pos_octet flags;
+};
+
+struct lift_status {
unsigned short act_pos;
unsigned short response; // Equals to req_pos when the move is done
- octet flags; // Zero when everything OK, otherwise see CAN_VIDLE_FLAG_*
+ octet flags; // Zero when everything OK, otherwise see CAN_LIFT_FLAG_*
+};
+
+struct match_time {
+ double time;
};
// FIXME: What's this??? M.S.
/* Definition of ORTE variables - input for roboortegen.pl */
-type=vidle_status topic=vidle_status deadline=0.5
-type=vidle_cmd topic=vidle_cmd
+type=jaws_status topic=jaws_status deadline=0.5
+type=jaws_cmd topic=jaws_cmd
+type=lift_status topic=lift_status deadline=0.5
+type=lift_cmd topic=lift_cmd
+type=match_time topic=match_time deadline=0.5
type=binary_data topic=binary_data
type=can_msg topic=can_msg
type=robot_pos topic=est_pos_odo
type=robot_pos topic=est_pos_indep_odo
type=robot_pos topic=est_pos_best
-type=hokuyo_pitch topic=hokuyo_pitch
type=hokuyo_scan topic=hokuyo_scan deadline=1
type=odo_data topic=odo_data deadline=0.3
type=motion_irc topic=motion_irc deadline=0.3
type=robot_pos topic=ref_pos
type=robot_cmd topic=robot_cmd deadline=1
type=robot_switches topic=robot_switches deadline=1
+type=robot_bumpers topic=robot_bumpers deadline=1
type=camera_result topic=camera_result
type=camera_control topic=camera_control pubdelay=1
type=fsm_state topic=fsm_main pubdelay=1
--- /dev/null
+Subproject commit 81d502f9fb6ceb6827c63493ea996c30331e4fef
projects / eurobot / public.git / commitdiff
