[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
-SUBDIRS=linux ppc lpceurobot # spejblarm h8eurobot h8mirosot
+SUBDIRS=host ppc lpceurobot # spejblarm h8eurobot h8mirosot
all := all $(filter-out all Makefile,$(MAKECMDGOALS))
# (C) Copyright 2006, 2007, 2008, 2009, 2010 by Michal Sojka - Czech Technical University, FEE, DCE
#
# Homepage: http://rtime.felk.cvut.cz/omk/
-# Version: 0.2-15-g5530d5e
+# Version: 0.2-37-gdf68588
#
# 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
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
#=========================
# 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))
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)/ | sed -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'` ; \
if [ x$$GOOD_MAKE_VERSION != xy ] ; then \
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):
+ mkdir -p $(1).wvtest
+ cd $(1).wvtest && $(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
$(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
$(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)
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
+CONFIG_PXMC_VARIANT=eurobot
+++ /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/eb_demo
\ 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
# 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
# -*- makefile -*-
-SUBDIRS = rozkuk ijgjpeg barcam
+SUBDIRS = barcol barcam ijgjpeg
#define WITH_GUI
#endif
+#ifdef PPC
+#undef WITH_GUI
+#endif
+
/******************************************************************************/
#include <inttypes.h>
#include <semaphore.h>
#include <getopt.h>
#include <unistd.h>
-#include <fftw3.h>
#include <string>
#include <opencv/cv.h>
#include <opencv/highgui.h>
+#include "barcam.h"
// gnuplot fifo plot
-#include <c2gnuplot.h>
+//#include <c2gnuplot.h>
extern "C" {
#include <roboorte_robottype.h>
-//#include <robot.h>
}
-/******************************************************************************/
-/***************************** macro definitions ******************************/
-/******************************************************************************/
-
-//uncomment next line to "log" the output frames - save them as pnm to the working directory
-//#define PPC_DEBUG
-
-// modes definitions
-#define MODE_QUIT 0x01
-#define MODE_VIDEO 0x02
-#define MODE_RECOGNIZE 0x04
-#define MODE_WAIT 0x08
-#define MODE_IMAGE 0x10
-
-// highgui windows names
-#define WINDOW_ORIG "BARCAM original scene"
-#define WINDOW_PROD "BARCAM recognition product"
-
-// size of graphical windows
-#define WINDOW_WIDTH 640
-#define WINDOW_HEIGHT 480
-
-//TODO tune size of region of interest (crop camera view)
-#define ROI_X 0
-#define ROI_Y 0
-#define ROI_WIDTH WINDOW_WIDTH
-#define ROI_HEIGHT WINDOW_HEIGHT
-
-// values of keys
-#define KEY_ESC 0x1B
-#define KEY_SPACE 0x20
-#define KEY_O 0x4f
-#define KEY_P 0x50
-#define KEY_R 0x52
-#define KEY_S 0x53
-#define KEY_V 0x56
-#define KEY_W 0x57
-#define KEY_o 0x6f
-#define KEY_p 0x70
-#define KEY_r 0x72
-#define KEY_s 0x73
-#define KEY_v 0x76
-#define KEY_w 0x77
-#define KEY_PLUS 0x2B
-#define KEY_MINUS 0x2D
-#define KEY_1 0x31
-#define KEY_2 0x32
-#define KEY_3 0x33
-#define KEY_4 0x34
-#define KEY_5 0x35
-#define KEY_6 0x36
-#define KEY_7 0x37
-#define KEY_8 0x38
-
-
-// filename pattern of snapshots (PPC does not support png)
-#ifdef WITH_GUI
-#define SNAPSHOTFILENAME "snapshot%02d.png"
-#else /* PPC */
-#define SNAPSHOTFILENAME "snapshot%02d.pnm"
-#endif /* WITH_GUI */
-
-
-/******************************************************************************/
-/************************** function declarations *****************************/
-/******************************************************************************/
-
-/********************** multimode graphical functions *************************/
-int saveFrame(const CvArr *img);
-void selectROI(CvCapture* capture, CvRect *roi);
-
-/******************** multimode computational functions ***********************/
-int countThreshold(const IplImage *frame);
-int recognize(const CvMat *frame);
-
-/********************************** MODE_IMAGE ********************************/
-int modeImage(char *imageFilename);
-
-/********************************* MODE_VIDEO *********************************/
-int modeVideo(CvCapture* capture, CvRect *roi);
-
-/****************************** MODE_RECOGNIZE ********************************/
-int recognizeMode_processKeys(IplImage *frame);
-void displayFrames(IplImage *frame);
-int modeRecognize(CvCapture* capture, CvRect *roi);
-
-/********************************* MODE_WAIT **********************************/
-int waitMode_processKeys(int previousMode);
-int modeWait(int previousMode);
-
-/******************************** mode manager ********************************/
-void setAnalyticWindowsVisible(bool visible);
-void destroyGUI(void);
-int modeManager(int defaultMode, char *paramC = NULL);
-
-/*********************************** orte *************************************/
-bool getCameraControlOn(void);
-void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance, void *recvCallBackParam);
-void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
-void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
-
-/********************************* application ********************************/
-int getParamI(int argc, char *argv[], char *imgFilename);
-int main(int argc, char *argv[]);
-
-
/******************************************************************************/
/**************************** variable declarations ***************************/
/******************************************************************************/
* @return Value returned by cvSaveImage. */
int saveFrame(const CvArr *img)
{
- struct stat stFileInfo;
- char filename[30];
- int i = 0;
+ struct stat stFileInfo;
+ char filename[30];
+ int i = 0;
- //find a non-existent filename (e.g. "snapshot13.png")
- do {
- sprintf(filename, SNAPSHOTFILENAME, i++);
- } while (!stat(filename, &stFileInfo));
+ //find a non-existent filename (e.g. "snapshot13.png")
+ do {
+ sprintf(filename, SNAPSHOTFILENAME, i++);
+ } while (!stat(filename, &stFileInfo));
- fprintf(stdout, "Saving frame to %s...\n", filename);
- return cvSaveImage(filename, img);
+ fprintf(stdout, "Saving frame to %s...\n", filename);
+ return cvSaveImage(filename, img);
}
/******************************************************************************/
#ifdef WITH_GUI
void selectROI(CvCapture* capture, CvRect *roi)
{
- IplImage* frame = NULL;
- int step = 10;
-
- while (1) {
- // wait 10ms for an event
- switch(cvWaitKey(10) & 0xFF) {
- // exit ROI setting
- case KEY_ESC:
- return;
-
- case KEY_PLUS:
- // increase ROI step
- step++;
- break;
-
- case KEY_MINUS:
- // decrease ROI step
- step--;
- break;
-
- case KEY_R:
- case KEY_r:
- // reset ROI to full frame
- roi->x = 0;
- roi->y = 0;
- roi->width = WINDOW_WIDTH;
- roi->height = WINDOW_HEIGHT;
- break;
-
- case KEY_1:
- // increase left border
- roi->x -= step;
- roi->width += step;
- break;
-
- case KEY_2:
- // decrease left border
- roi->x += step;
- roi->width -= step;
- break;
-
- case KEY_3:
- // increase upper border
- roi->y -= step;
- roi->height += step;
- break;
-
- case KEY_4:
- // decrease upper border
- roi->y += step;
- roi->height -= step;
- break;
-
- case KEY_5:
- // decrease lower border
- roi->height -= step;
- break;
-
- case KEY_6:
- // increase lower border
- roi->height += step;
- break;
-
- case KEY_7:
- // decrease right border
- roi->width -= step;
- break;
-
- case KEY_8:
- // increase right border
- roi->width += step;
- break;
- }
-
- step = (step < 0 ? 0 : step);
-
- fprintf(stderr, "ROI step: %d x: %d y: %d width: %d height: %d\n",
- step, roi->x, roi->y, roi->width, roi->height);
-
- frame = cvQueryFrame(capture);
-
- cvPutText(frame, "Select ROI", cvPoint(frame->width/2 -60,
- frame->height - 50),
- &fontLarge, cvScalar(0,0,255));
-
- cvRectangle(frame, cvPoint(roi->x, roi->y), cvPoint(roi->width + roi->x,
- roi->height + roi->y),
- cvScalar(0,0,255), 2, 8, 0);
- cvShowImage(WINDOW_ORIG, frame);
-
- }
+ IplImage* frame = NULL;
+ int step = 10;
+
+ while (1) {
+ // wait 10ms for an event
+ switch(cvWaitKey(10) & 0xFF) {
+ // exit ROI setting
+ case KEY_ESC:
+ return;
+
+ case KEY_PLUS:
+ // increase ROI step
+ step++;
+ break;
+
+ case KEY_MINUS:
+ // decrease ROI step
+ step--;
+ break;
+
+ case KEY_R:
+ case KEY_r:
+ // reset ROI to full frame
+ roi->x = 0;
+ roi->y = 0;
+ roi->width = WINDOW_WIDTH;
+ roi->height = WINDOW_HEIGHT;
+ break;
+
+ case KEY_1:
+ // increase left border
+ roi->x -= step;
+ roi->width += step;
+ break;
+
+ case KEY_2:
+ // decrease left border
+ roi->x += step;
+ roi->width -= step;
+ break;
+
+ case KEY_3:
+ // increase upper border
+ roi->y -= step;
+ roi->height += step;
+ break;
+
+ case KEY_4:
+ // decrease upper border
+ roi->y += step;
+ roi->height -= step;
+ break;
+
+ case KEY_5:
+ // decrease lower border
+ roi->height -= step;
+ break;
+
+ case KEY_6:
+ // increase lower border
+ roi->height += step;
+ break;
+
+ case KEY_7:
+ // decrease right border
+ roi->width -= step;
+ break;
+
+ case KEY_8:
+ // increase right border
+ roi->width += step;
+ break;
+ }
+
+ step = (step < 0 ? 0 : step);
+
+ fprintf(stderr, "ROI step: %d x: %d y: %d width: %d height: %d\n",
+ step, roi->x, roi->y, roi->width, roi->height);
+
+ frame = cvQueryFrame(capture);
+
+ cvPutText(frame,
+ "Select ROI",
+ cvPoint(frame->width/2 -60,
+ frame->height - 50),
+ &fontLarge, cvScalar(0,0,255));
+
+ cvRectangle(frame,
+ cvPoint(roi->x, roi->y),
+ cvPoint(roi->width + roi->x, roi->height + roi->y),
+ cvScalar(0,0,255), 2, 8, 0);
+
+ cvShowImage(WINDOW_ORIG, frame);
+ }
}
#else /* PPC */
-
void selectROI(CvCapture *capture, CvRect *roi) {}
#endif /* WITH_GUI */
/******************************************************************************/
/******************** multimode computational functions ***********************/
/******************************************************************************/
-
-/** Returns a threshold computed from the frame.
- * @param frame Frame to compute a threshold from.
- * @return Mean of all pixels of the frame. */
-int countThreshold(const IplImage *frame)
+int myPow(int number)
{
- return cvAvg(frame).val[0];
+ return (number*number);
}
-/******************************************************************************/
-
-/** Returns an ordinary number of recognized configuration.
+/** Recognizes a strip code in the image
* @param frame Frame to be processed.
- * @return */
-int recognize(IplImage* gray_frame) {
-
- // create gnuplot window
- static gnuplot_window window;
-
- int i = 0;
- int n = gray_frame->height;
- int m = 0;
- double absval = 0;
- double cc = 0;
- double correction = (double)255 / (double)n;
-
- double *in, *out;
- fftw_plan rplan;
-
- in = (double*) malloc(sizeof(double) * n);
- out = (double*) malloc(sizeof(double) * n);
-
- rplan = fftw_plan_r2r_1d(n, in, out, FFTW_R2HC, FFTW_ESTIMATE);
-
- // we have no imaginary data, so clear idata
- // memset((void *)out, 0, n * sizeof(double));
-
- // fill rdata with actual data
- for (i = 0; i < n; i++) {
- in[i] = ((uchar*)(gray_frame->imageData + i*gray_frame->widthStep))[(gray_frame->width/2)*3];
-
- // draw black line in the midle of the image where DFT is computed
- ((uchar*)(gray_frame->imageData + i*gray_frame->widthStep))[(gray_frame->width/2)*3] = 0;
- }
-
- fftw_execute(rplan);
-
- // set plot parameters
- window.plot_data("u 1:2 w p ps 2");
-
- // post-process FFT data: make absolute values, and calculate
- // real frequency of each power line in the spectrum
- for (i = 1; i < (n/2 - 1); i++) {
- absval = sqrt((out[i] * out[i]) + (out[n - i] * out[n - i])) / n;
- cc = (double)m * correction;
- m++;
-
- // plot fft data, do not plot DC value
- window.data("%f, %f",cc,absval);
- }
-
- // plot values from FIFO
- window.flush();
-
- //usleep(50000);
-
- fftw_destroy_plan(rplan);
- free(in);
- free(out);
-
- return 0;
+ * @return number of detected codes, zero if none
+ */
+int recognize(IplImage* gray_frame)
+{
+ int8_t i8angle = -1;
+
+ int RESOLUTION_X = 640; //Camera resolution setting is not working!
+ int RESOLUTION_Y = 480;
+
+ int detectedLinesPoints[3][ARRAY_SIZE];
+ int detectedCodes[2][2][100];
+ int codesCount = 0;
+
+ int i, j, k, l, m;
+ uint8_t lineBuffer[MAX_RES_Y + 10];
+ CvPoint startPoint;
+ CvPoint endPoint;
+ CvPoint codeEndPoint, codeStartPoint;
+ CvPoint a, b;
+ codeEndPoint.x = -1;
+ codeEndPoint.y = -1;
+ codeStartPoint.x = -1;
+ codeStartPoint.y = -1;
+
+ unsigned int space = 0;
+ unsigned int detectedTransitions = 0;
+ unsigned int linesPerSample = 0; //all lines in one vertical sample
+ unsigned int totalLines = 0; //total count of detected lines in the image
+
+ unsigned int singleLinesPerBlock = 0; //only 1 line per vertical sample
+
+ unsigned int darkest = 255, lightest = 0;
+ int lineBegining = -1;
+
+ int colourSum = -1;
+ int colourSamplesCount = 0;
+
+ int longestCodeLength = 0, longestCodePosition = 0;
+ double angle;
+
+ IplImage *gimg = gray_frame;
+ IplImage *lineImg = 0; //(only vertical lines)
+
+ lineImg = cvCreateImage(cvSize(gimg->width,gimg->height),IPL_DEPTH_8U,1);
+
+ RESOLUTION_X = gimg->width;
+ RESOLUTION_Y = gimg->height;
+
+ cvZero(lineImg);
+ linesPerSample = 0;
+ totalLines = 0;
+ singleLinesPerBlock = 0;
+
+ /******* Detect vertical lines of required parameters. *******/
+ for (j = LINES_BY_DISTANCE; j < RESOLUTION_X; j += LINES_BY_DISTANCE) {
+ startPoint.x = j; startPoint.y = 0;
+ endPoint.x = j; endPoint.y = RESOLUTION_Y;
+ cvSampleLine(gimg, startPoint, endPoint, lineBuffer, 8); //retrieve 1 line from the grayscale image
+ space = 0; detectedTransitions = 0;
+ cvSetReal2D(lineImg, 0, j, lineBuffer[0]); //first point of the line to the line image
+
+ for (i = 1; i < RESOLUTION_Y; i++) {
+ if ((lineBuffer[i] < (lineBuffer[i-1] - COLOUR_DIFF)) ||
+ (lineBuffer[i] > (lineBuffer[i-1]) + COLOUR_DIFF)) {
+ if (lineBegining == -1) lineBegining = i;
+ if (codeStartPoint.x == -1) {
+ codeStartPoint.x = j;
+ codeStartPoint.y = i;
+ }
+ detectedTransitions++;
+ if (detectedTransitions == TRANSITIONS){
+ darkest = 255;
+ lightest = 0;
+ colourSamplesCount = 0;
+ colourSum = 0;
+
+ for (k = lineBegining; k <= i; k++) {
+ if (lineBuffer[k] > lightest) lightest = lineBuffer[k];
+ if (lineBuffer[k] < darkest) darkest = lineBuffer[k];
+ colourSum += lineBuffer[k];
+ colourSamplesCount++;
+ }
+ if ((darkest <= CONTAINS_BLACK) &&
+ (lightest >= CONTAINS_WHITE) &&
+ ((colourSum / colourSamplesCount) < AVERAGE_COLOUR_MAX) &&
+ ((colourSum / colourSamplesCount) > AVERAGE_COLOUR_MIN)) {
+ detectedLinesPoints[0][totalLines] = j; //X
+ detectedLinesPoints[1][totalLines] = lineBegining; //Y start
+ detectedLinesPoints[2][totalLines] = i; //Y end
+ totalLines++;
+ } else {
+ detectedTransitions = 0;
+ lineBegining = -1;
+ codeStartPoint.x = -1;
+ codeStartPoint.y = -1;
+ }
+
+ space = 0;
+
+ } else if (detectedTransitions > TRANSITIONS) {
+ darkest = 255;
+ lightest = 0;
+ colourSamplesCount = 0;
+ colourSum = 0;
+
+ for (k = lineBegining; k <= i; k++) {
+ if (lineBuffer[k] > lightest) lightest = lineBuffer[k];
+ if (lineBuffer[k] < darkest) darkest = lineBuffer[k];
+ colourSum += lineBuffer[k];
+ colourSamplesCount++;
+ }
+ if ((darkest <= CONTAINS_BLACK) &&
+ (lightest >= CONTAINS_WHITE) &&
+ ((colourSum / colourSamplesCount) < AVERAGE_COLOUR_MAX) &&
+ ((colourSum / colourSamplesCount) > AVERAGE_COLOUR_MIN)) {
+ detectedLinesPoints[2][totalLines-1] = i; //Y end
+ }
+ space = 0;
+ }
+ space = 0;
+ } else if (space <= MAX_GAP) {
+ space++;
+ } else {
+ codeStartPoint.x = -1;
+ codeStartPoint.y = -1;
+ lineBegining = -1;
+ space = 0;
+ detectedTransitions = 0;
+ }
+ cvSetReal2D(lineImg, i, j, lineBuffer[i]); //1 point of the line to the line image
+ }
+ }
+ angle = -1;
+ /******* Detect blocks of lines. They should be a part of a bar code. *******/
+ if (totalLines >= VERTICAL_LINES) {
+ codesCount = 0;
+ singleLinesPerBlock = 1;
+ codeStartPoint.x = -1;
+ codeStartPoint.y = -1;
+ for (i = 0; i < (totalLines - VERTICAL_LINES + 1); i++) {
+ codeStartPoint.x = -1;
+ codeStartPoint.y = -1;
+ singleLinesPerBlock = 1;
+ k = i + 1;
+ l = LINES_BY_DISTANCE;
+ while ((detectedLinesPoints[0][k] < (detectedLinesPoints[0][i] + l)) &&
+ k < (totalLines - VERTICAL_LINES + 1)) {
+ k++;
+ }
+
+ while ((detectedLinesPoints[0][k] == detectedLinesPoints[0][i] + l) &&
+ k < (totalLines - VERTICAL_LINES + 1)) {
+ if (detectedLinesPoints[1][k] < detectedLinesPoints[1][i] + VERTICAL_TOLERANCE &&
+ detectedLinesPoints[1][k] > detectedLinesPoints[1][i] - VERTICAL_TOLERANCE) {
+ singleLinesPerBlock++;
+
+ if (singleLinesPerBlock == VERTICAL_LINES) {
+ codeStartPoint.x = detectedLinesPoints[0][i];
+ codeStartPoint.y = detectedLinesPoints[1][i];
+ codeEndPoint.x = detectedLinesPoints[0][k];
+ codeEndPoint.y = detectedLinesPoints[2][k];
+
+ for(j = i; j <= k; j++) {
+ a.x = detectedLinesPoints[0][j];
+ a.y = detectedLinesPoints[1][j];
+ b.x = detectedLinesPoints[0][j];
+ b.y = detectedLinesPoints[2][j];
+ cvLine(gimg, a, b, CV_RGB(255, 255, 255), 2, 8, 0);
+ }
+ }
+ if(singleLinesPerBlock > VERTICAL_LINES){
+ codeEndPoint.x = detectedLinesPoints[0][k];
+ codeEndPoint.y = detectedLinesPoints[2][k];
+ a.x = detectedLinesPoints[0][k];
+ a.y = detectedLinesPoints[1][k];
+ b.x = detectedLinesPoints[0][k];
+ b.y = detectedLinesPoints[2][k];
+ cvLine(gimg, a, b, CV_RGB(255, 255, 255), 2, 8, 0);
+ }
+ l += LINES_BY_DISTANCE;
+
+ while ((detectedLinesPoints[0][k] < (detectedLinesPoints[0][i] + l)) &&
+ k < (totalLines - VERTICAL_LINES + 1)) {
+ k++;
+ }
+ continue;
+ }
+ k++;
+ }
+ /******* Merge similar blocks into one. *******/
+ if (codeStartPoint.x != -1 && codesCount < 100) {
+ if (codesCount > 0) {
+ for(m = 0; m < codesCount; m++) {
+ if (detectedCodes[1][0][m] == codeEndPoint.x &&
+ detectedCodes[1][1][m] == codeEndPoint.y) {
+ codeStartPoint.x = -1;
+ break;
+ }
+ }
+ if (codeStartPoint.x != -1) {
+ codesCount++;
+ detectedCodes[0][0][codesCount-1] = codeStartPoint.x;
+ detectedCodes[0][1][codesCount-1] = codeStartPoint.y;
+ detectedCodes[1][0][codesCount-1] = codeEndPoint.x;
+ detectedCodes[1][1][codesCount-1] = codeEndPoint.y;
+ cvLine(gimg, codeStartPoint, codeEndPoint, CV_RGB(255, 255, 255), 2, 8, 0);
+ //printf("Detected code: %i %i; %i %i\n",codeStartPoint.x,codeStartPoint.y,codeEndPoint.x,codeEndPoint.y);
+ }
+ } else {
+ codesCount++;
+ detectedCodes[0][0][0] = codeStartPoint.x;
+ detectedCodes[0][1][0] = codeStartPoint.y;
+ detectedCodes[1][0][0] = codeEndPoint.x;
+ detectedCodes[1][1][0] = codeEndPoint.y;
+ cvLine(gimg, codeStartPoint, codeEndPoint, CV_RGB(255, 255, 255), 2, 8, 0);
+ //printf("Detected code: %i %i; %i %i\n",codeStartPoint.x,codeStartPoint.y,codeEndPoint.x,codeEndPoint.y);
+ }
+ }
+
+ }
+ /******* Determine the largest bar code and the middle of it. *******/
+ if (codesCount > 0) {
+ longestCodeLength = sqrt(myPow (detectedCodes[1][0][0] - detectedCodes[0][0][0]) +
+ myPow(detectedCodes[1][1][0] - detectedCodes[0][1][0]));
+ longestCodePosition = 0;
+
+ for (m = 0; m < codesCount; m++) {
+ i = sqrt(myPow(detectedCodes[1][0][m] - detectedCodes[0][0][m]) +
+ myPow(detectedCodes[1][1][m] - detectedCodes[0][1][m]));
+ if (i > longestCodeLength) {
+ longestCodeLength = i;
+ longestCodePosition = m;
+ }
+ }
+ codeStartPoint.x = detectedCodes[0][0][longestCodePosition];
+ codeStartPoint.y = detectedCodes[0][1][longestCodePosition];
+ codeEndPoint.x = detectedCodes[1][0][longestCodePosition];
+ codeEndPoint.y = detectedCodes[1][1][longestCodePosition];
+ cvLine(gimg, codeStartPoint, codeEndPoint, CV_RGB(0, 0, 0), 2, 8, 0);
+ a.x = (codeStartPoint.x + codeEndPoint.x) / 2;
+ a.y = (codeStartPoint.y + codeEndPoint.y) / 2;
+ cvCircle(gimg, a, 3, CV_RGB(0, 0, 0), -1, 16, 0);
+ angle = atan((double)a.x / 824);
+ angle = (180 / M_PI) * angle;
+ //printf("Detected code: x=%i y=%i - angle %i\n", a.x, a.y, (int)angle);
+ } else {
+ angle = -1;
+ }
+ i8angle = (int8_t)angle;
+ }
+ printf("detected codes: %d\n", codesCount);
+ cvReleaseImage(&lineImg);
+ return codesCount;
}
#ifdef WITH_GUI
int modeImage(char *imageFilename)
{
- IplImage *frame = NULL;
-
- frame = cvLoadImage(imageFilename);
-
- if (!frame) {
- fprintf(stderr, "File %s cannot be loaded as image.\n", imageFilename);
- return MODE_QUIT;
- }
-
- while (1) {
-
-
- // wait infinitely for a keyboard event
- switch((char)(cvWaitKey(0) & 0xFF)) {
- // quit
- case KEY_ESC:
- goto _modeImage_end;
- break;
- }
- }
-
-_modeImage_end:
- cvReleaseImage(&frame);
- return MODE_QUIT;
+ IplImage *frame = NULL;
+
+ frame = cvLoadImage(imageFilename);
+
+ if (!frame) {
+ fprintf(stderr, "File %s cannot be loaded as image.\n", imageFilename);
+ return MODE_QUIT;
+ }
+
+ while (1) {
+ // wait infinitely for a keyboard event
+ switch ((char)(cvWaitKey(0) & 0xFF)) {
+ // quit
+ case KEY_ESC:
+ goto _modeImage_end;
+ break;
+ }
+ }
+
+ _modeImage_end:
+ cvReleaseImage(&frame);
+ return MODE_QUIT;
}
#else /* PPC */
-int modeImage(char *imageFilename) {
- fprintf(stderr, "Image mode is unsupported on PPC.\nTerminating.\n");
- return MODE_QUIT;
+int modeImage(char *imageFilename)
+{
+ fprintf(stderr, "Image mode is unsupported on PPC.\nTerminating.\n");
+ return MODE_QUIT;
}
#endif /* WITH_GUI */
#ifdef WITH_GUI
int modeVideo(CvCapture* capture, CvRect *roi)
{
- IplImage *frame = NULL;
-
- while (1) {
- // wait 10ms for an event
- switch(cvWaitKey(10) & 0xFF) {
- // stop capturing
- case KEY_ESC:
+ IplImage *frame = NULL;
+
+ while (1) {
+ // wait 10ms for an event
+ switch(cvWaitKey(10) & 0xFF) {
+ // stop capturing
+ case KEY_ESC:
+ return MODE_QUIT;
+
+ // switch to recognize mode
+ case KEY_R:
+ case KEY_r:
+ return MODE_RECOGNIZE;
+
+ // pause - switch to wait mode
+ case KEY_P:
+ case KEY_p:
+ return MODE_WAIT;
+
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
+
+ // display ROI select window
+ case KEY_O:
+ case KEY_o:
+ cvResetImageROI(frame);
+ selectROI(capture, roi);
+ cvSetImageROI(frame, *roi);
+ break;
+ }
+
+ // get one frame from camera (do not release it!)
+ frame = cvQueryFrame(capture);
+
+ if (!frame) {
+ fprintf(stderr, "NULL frame\n");
+ continue;
+ }
+ cvShowImage(WINDOW_ORIG, frame);
+ }
return MODE_QUIT;
-
- // switch to recognize mode
- case KEY_R:
- case KEY_r:
- return MODE_RECOGNIZE;
-
- // pause - switch to wait mode
- case KEY_P:
- case KEY_p:
- return MODE_WAIT;
-
- // save frame to file
- case KEY_S:
- case KEY_s:
- saveFrame(frame);
- break;
-
- // display ROI select window
- case KEY_O:
- case KEY_o:
- cvResetImageROI(frame);
- selectROI(capture, roi);
- cvSetImageROI(frame, *roi);
- break;
-
- }
-
- // get one frame from camera (do not release it!)
- frame = cvQueryFrame(capture);
-
- if (!frame) {
- fprintf(stderr, "NULL frame\n");
- continue;
- }
-
- cvShowImage(WINDOW_ORIG, frame);
- }
-
- return MODE_QUIT;
}
#else /* PPC */
-int modeVideo(CvCapture* capture, CvRect *roi) {
- fprintf(stderr, "Video mode is unsupported on PPC.\nTerminating.\n");
- return MODE_QUIT;
+int modeVideo(CvCapture* capture, CvRect *roi)
+{
+ fprintf(stderr, "Video mode is unsupported on PPC.\nTerminating.\n");
+ return MODE_QUIT;
}
#endif /* WITH_GUI */
#ifdef WITH_GUI
int recognizeMode_processKeys(IplImage *frame)
{
- // wait 10ms for an event
- switch (cvWaitKey(10) & 0xFF) {
- // stop capturing
- case KEY_ESC:
- return MODE_QUIT;
-
- // video mode
- case KEY_V:
- case KEY_v:
- return MODE_VIDEO;
-
- // pause - wait mode
- case KEY_P:
- case KEY_p:
- camera_control_on = 0;
- return MODE_WAIT;
-
- // save frame to file
- case KEY_S:
- case KEY_s:
- saveFrame(frame);
- break;
- }
- return MODE_RECOGNIZE;
+ // wait 10ms for an event
+ switch (cvWaitKey(10) & 0xFF) {
+ // stop capturing
+ case KEY_ESC:
+ return MODE_QUIT;
+
+ // video mode
+ case KEY_V:
+ case KEY_v:
+ return MODE_VIDEO;
+
+ // pause - wait mode
+ case KEY_P:
+ case KEY_p:
+ camera_control_on = 0;
+ return MODE_WAIT;
+
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
+ }
+ return MODE_RECOGNIZE;
}
#else /* PPC */
int recognizeMode_processKeys(IplImage *frame)
{
- return MODE_RECOGNIZE;
+ return MODE_RECOGNIZE;
}
#endif /* WITH_GUI */
* @param window_name Name of the window to show frame in.
*/
#ifdef WITH_GUI
-void displayFrames(const char* window_name, IplImage *frame) {
- cvShowImage(window_name, frame);
-
+void displayFrames(const char* window_name, IplImage *frame)
+{
+ cvShowImage(window_name, frame);
}
#else /* PPC */
* @return Code of mode to switch to. */
int modeRecognize(CvCapture* capture, CvRect *roi)
{
- int ret;
- IplImage *frame = NULL;
- IplImage *roi_frame = cvCreateImage(cvSize(roi->width, roi->height), 8, 1);
-
- while (camera_control_on) {
-
- if ((ret = recognizeMode_processKeys(roi_frame)) != MODE_RECOGNIZE)
- return ret;
-
- //get one frame from camera (do not release!)
- frame = cvQueryFrame(capture);
-
- if (!frame) {
- fprintf(stderr, "NULL frame\n");
- usleep(100*1000);
- continue;
- } //else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
- // orte.camera_result.error &= ~camera_ERR_NO_FRAME;
- //ORTEPublicationSend(orte.publication_camera_result);
- // }
-
- // set region of interest on captured image
- cvSetImageROI(frame, *roi);
-
- // copy ROI part of captured image to roi_frame and convert to grayscale
- cvCvtColor(frame, roi_frame, CV_BGR2GRAY);
-
- //TODO call image processing (do something useful with the image, FFT atc.)
- ret = recognize(roi_frame);
-
- displayFrames(WINDOW_ORIG, frame);
- displayFrames(WINDOW_PROD, roi_frame);
-
-// publish results
-// orte.camera_result.side = sideConfig;
-// orte.camera_result.center = centerConfig;
-// orte.camera_result.sideDist = sideDist;
-// orte.camera_result.centerDist = centerDist;
-// ORTEPublicationSend(orte.publication_camera_result);
-
- }
- return MODE_WAIT;
+ int ret;
+ IplImage *frame = NULL;
+ IplImage *roi_frame = cvCreateImage(cvSize(roi->width, roi->height), 8, 1);
+
+ while (camera_control_on) {
+
+ if ((ret = recognizeMode_processKeys(roi_frame)) != MODE_RECOGNIZE)
+ return ret;
+
+ ret = 0;
+
+ for (int i = 0; i < 5; i++) {
+ //get one frame from camera (do not release!)
+ frame = cvQueryFrame(capture);
+
+ if (!frame) {
+ fprintf(stderr, "NULL frame\n");
+ usleep(100*1000);
+ continue;
+ } else if (orte.camera_result.error & camera_ERR_NO_FRAME) {
+ orte.camera_result.error &= ~camera_ERR_NO_FRAME;
+ ORTEPublicationSend(orte.publication_camera_result);
+ }
+
+ // set region of interest on captured image
+ cvSetImageROI(frame, *roi);
+
+ // copy ROI part of captured image to roi_frame and convert to grayscale
+ cvCvtColor(frame, roi_frame, CV_BGR2GRAY);
+
+ //TODO call image processing (do something useful with the image, FFT atc.)
+ ret += recognize(roi_frame);
+
+ displayFrames(WINDOW_ORIG, frame);
+ displayFrames(WINDOW_PROD, roi_frame);
+ }
+
+ // publish results
+ printf("return: %d\n", ret);
+ orte.camera_result.target_valid = ret;
+ orte.camera_result.data_valid = true;
+ ORTEPublicationSend(orte.publication_camera_result);
+ }
+ orte.camera_result.data_valid = false;
+ ORTEPublicationSend(orte.publication_camera_result);
+
+ cvReleaseImage(&roi_frame);
+ return MODE_WAIT;
}
#ifdef WITH_GUI
int waitMode_processKeys(int previousMode)
{
- // wait 10ms for an event
- switch(cvWaitKey(10) & 0xFF) {
- // exit program
- case KEY_ESC:
- return MODE_QUIT;
-
- // stop waiting
- case KEY_P:
- case KEY_p:
- camera_control_on = 1;
- return previousMode;
-
- // continue waiting
- default:
- return MODE_WAIT;
- }
+ // wait 10ms for an event
+ switch(cvWaitKey(10) & 0xFF) {
+ // exit program
+ case KEY_ESC:
+ return MODE_QUIT;
+
+ // stop waiting
+ case KEY_P:
+ case KEY_p:
+ camera_control_on = 1;
+ return previousMode;
+
+ // continue waiting
+ default:
+ return MODE_WAIT;
+ }
}
#else /* PPC */
-int waitMode_processKeys(int previousMode) {
- return (getCameraControlOn() ? previousMode : MODE_WAIT);
+int waitMode_processKeys(int previousMode)
+{
+ return (getCameraControlOn() ? previousMode : MODE_WAIT);
}
#endif /* WITH_GUI */
* @return Code of mode to switch to. */
int modeWait(int previousMode)
{
- int mode;
-
- while((mode = waitMode_processKeys(previousMode)) == MODE_WAIT) {
- fprintf(stderr, "waiting...\n");
- sleep(1);
- }
-
- return mode;
+ int mode;
+
+ while ((mode = waitMode_processKeys(previousMode)) == MODE_WAIT) {
+ fprintf(stderr, "waiting...\n");
+ sleep(1);
+ }
+
+ return mode;
}
/** Initializes GUI, displays static video window. */
#ifdef WITH_GUI
void initGUI(void) {
- cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);
- cvInitFont(&fontLarge, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2);
+ cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);
+ cvInitFont(&fontLarge, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2);
}
#else /* PPC */
void initGUI(void) {}
#ifdef WITH_GUI
void destroyGUI(void)
{
- cvDestroyWindow(WINDOW_ORIG);
- cvDestroyWindow(WINDOW_PROD);
+ cvDestroyWindow(WINDOW_ORIG);
+ cvDestroyWindow(WINDOW_PROD);
}
#else /* PPC */
void destroyGUI(void) {}
* @return Zero on success, error number on fail. */
int modeManager(int defaultMode, char *paramC)
{
- int mode = defaultMode;
- int lastMode = MODE_RECOGNIZE;
- CvCapture* capture = NULL;
- CvRect roi;
-
- roi.x = ROI_X;
- roi.y = ROI_Y;
- roi.width = ROI_WIDTH;
- roi.height = ROI_HEIGHT;
-
- if (defaultMode == MODE_IMAGE) {
- fprintf(stderr, "barcam started in image mode\n");
- } else {
- // connect to a camera
- while (!(capture = cvCaptureFromCAM(-1))) {
- fprintf(stderr, "NULL capture (is camera connected?)\n");
- //orte.camera_result.error |= camera_ERR_NO_VIDEO;
- //ORTEPublicationSend(orte.publication_camera_result);
- usleep(500*1000);
- }
-
- //orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
- //ORTEPublicationSend(orte.publication_camera_result);
- fprintf(stderr, "barcam started, camera connected successfully\n");
- }
-
- initGUI();
-
- while(!(mode & MODE_QUIT)) {
- switch(mode) {
-
- case MODE_VIDEO:
- mode = modeVideo(capture, &roi);
- lastMode = MODE_VIDEO;
- break;
-
- case MODE_RECOGNIZE:
- mode = modeRecognize(capture, &roi);
- lastMode = MODE_RECOGNIZE;
- break;
-
- case MODE_WAIT:
- mode = modeWait(lastMode);
- break;
-
- case MODE_IMAGE:
- mode = modeImage(paramC);
- lastMode = MODE_IMAGE;
- break;
-
- // jump out of the loop
- default:
- goto _modeManager_end;
- }
- }
+ int mode = defaultMode;
+ int lastMode = MODE_RECOGNIZE;
+ CvCapture* capture = NULL;
+ CvRect roi;
+
+ roi.x = ROI_X;
+ roi.y = ROI_Y;
+ roi.width = ROI_WIDTH;
+ roi.height = ROI_HEIGHT;
+
+ if (defaultMode == MODE_IMAGE) {
+ fprintf(stderr, "barcam started in image mode\n");
+ } else {
+ // connect to a camera
+ while (!(capture = cvCaptureFromCAM(-1))) {
+ fprintf(stderr, "NULL capture (is camera connected?)\n");
+ orte.camera_result.error |= camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
+ usleep(500*1000);
+ }
+
+ orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
+ fprintf(stderr, "barcam started, camera connected successfully\n");
+ }
+
+ initGUI();
+
+ while (!(mode & MODE_QUIT)) {
+ switch(mode) {
+ case MODE_VIDEO:
+ mode = modeVideo(capture, &roi);
+ lastMode = MODE_VIDEO;
+ break;
+
+ case MODE_RECOGNIZE:
+ mode = modeRecognize(capture, &roi);
+ lastMode = MODE_RECOGNIZE;
+ break;
+
+ case MODE_WAIT:
+ mode = modeWait(lastMode);
+ break;
+
+ case MODE_IMAGE:
+ mode = modeImage(paramC);
+ lastMode = MODE_IMAGE;
+ break;
+
+ // jump out of the loop
+ default:
+ goto _modeManager_end;
+ }
+ }
_modeManager_end:
- cvReleaseCapture(&capture);
- //destroyGUI();
- return 0;
+ cvReleaseCapture(&capture);
+ //destroyGUI();
+ return 0;
}
/** Returns actual state of orte.camera_control.on.
* If value changed from previous call, informative output is printed. */
#ifdef WITH_GUI
-inline bool getCameraControlOn(void) {
- return camera_control_on;
+inline bool getCameraControlOn(void)
+{
+ return camera_control_on;
}
#else /* PPC */
inline bool getCameraControlOn(void) {
- if(orte.camera_control.on!=camera_control_on) {
- camera_control_on = orte.camera_control.on;
- fprintf(stderr, "orte: camera_control changed: ctrl %d\n",
- camera_control_on);
- }
- return camera_control_on;
+ if (orte.camera_control.on!=camera_control_on) {
+ camera_control_on = orte.camera_control.on;
+ fprintf(stderr, "orte: camera_control changed: ctrl %d\n",
+ camera_control_on);
+ }
+ return camera_control_on;
}
#endif /* WITH_GUI */
/** Orte camera result callback function. Does nothing. */
void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance,
- void *recvCallBackParam) { /* nothing */ }
+ void *recvCallBackParam)
+{
+ /* nothing */
+}
/******************************************************************************/
/** Orte camera control callback function.
* Sets actual value of orte.camera_control.on to camera_control_on. */
-void rcv_cmr_ctrl_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:
- fprintf(stderr, "orte: New camera data: ctrl %d\n",
- orte_data->camera_control.on);
- getCameraControlOn();
- break;
- case DEADLINE:
- fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
- break;
- }
+void rcv_cmr_ctrl_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:
+ fprintf(stderr, "orte: New camera data: ctrl %d\n",
+ orte_data->camera_control.on);
+ getCameraControlOn();
+ break;
+ case DEADLINE:
+ fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
+ break;
+ }
}
/** If "-i filename" is in argv, filename is stored to imgFilename.
* @return Mode to switch to. */
#ifdef WITH_GUI
-int getStartMode(int argc, char *argv[], char *imgFilename) {
- char opt;
-
- // scan for program arguments
- while((opt = getopt(argc, argv, "i:")) != -1) {
- switch (opt) {
- //"-i filename"
- case 'i':
- if(optarg) sprintf(imgFilename, "%s", optarg);
- else {
- fprintf(stderr, "Specify image filename to process: barcam -i filename\n");
- return MODE_QUIT;
- }
- return MODE_IMAGE;
- break;
- }
- }
-
- camera_control_on = true;
- return MODE_VIDEO;
+int getStartMode(int argc, char *argv[], char *imgFilename)
+{
+ char opt;
+
+ // scan for program arguments
+ while ((opt = getopt(argc, argv, "i:")) != -1) {
+ switch (opt) {
+ //"-i filename"
+ case 'i':
+ if (optarg) {
+ sprintf(imgFilename, "%s", optarg);
+ } else {
+ fprintf(stderr, "Specify image filename to process: barcam -i filename\n");
+ return MODE_QUIT;
+ }
+ return MODE_IMAGE;
+ break;
+ }
+ }
+
+ camera_control_on = true;
+ return MODE_VIDEO;
}
#else /* PPC */
-int getStartMode(int argc, char *argv[], char *imgFilename) {
- return MODE_WAIT;
+int getStartMode(int argc, char *argv[], char *imgFilename)
+{
+ return MODE_WAIT;
}
#endif /* WITH_GUI */
* modes are available - recognize and wait. Orte's camera_control_on variable
* is used to switch between them. */
int main(int argc, char *argv[])
-{
- char imgFilename[100];
- int ret;
-
- //ret = robottype_roboorte_init(&orte);
-
-// if(ret < 0) {
-// fprintf(stderr, "robottype_roboorte_init failed\n");
-// return ret;
-// }
-//
-
-// robottype_publisher_camera_result_create(&orte, send_cmr_res_cb, &orte);
-// robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
-// robottype_subscriber_robot_switches_create(&orte, rcv_robot_switches_cb, &orte);
-
- ret = getStartMode(argc, argv, imgFilename);
- modeManager(ret, imgFilename);
-
- //ret = robottype_roboorte_destroy(&orte);
- return ret;
-
+{
+ char imgFilename[100];
+ int ret;
+
+ ret = robottype_roboorte_init(&orte);
+
+ if (ret < 0) {
+ fprintf(stderr, "barcam: robottype_roboorte_init failed\n");
+ return ret;
+ }
+
+ robottype_publisher_camera_result_create(&orte, send_cmr_res_cb, &orte);
+ robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
+
+ ret = getStartMode(argc, argv, imgFilename);
+ modeManager(ret, imgFilename);
+
+ ret = robottype_roboorte_destroy(&orte);
+ return ret;
}
--- /dev/null
+#ifndef BARCAMH
+#define BARCAMH
+
+//uncomment next line to "log" the output frames - save them as pnm to the working directory
+//#define PPC_DEBUG
+
+// modes definitions
+#define MODE_QUIT 0x01
+#define MODE_VIDEO 0x02
+#define MODE_RECOGNIZE 0x04
+#define MODE_WAIT 0x08
+#define MODE_IMAGE 0x10
+
+// highgui windows names
+#define WINDOW_ORIG "BARCAM original scene"
+#define WINDOW_PROD "BARCAM recognition product"
+
+// size of graphical windows
+#define WINDOW_WIDTH 640
+#define WINDOW_HEIGHT 480
+
+//TODO tune size of region of interest (crop camera view)
+#define ROI_X 0
+#define ROI_Y 0
+#define ROI_WIDTH WINDOW_WIDTH
+#define ROI_HEIGHT WINDOW_HEIGHT
+
+// values of keys
+#define KEY_ESC 0x1B
+#define KEY_SPACE 0x20
+#define KEY_O 0x4f
+#define KEY_P 0x50
+#define KEY_R 0x52
+#define KEY_S 0x53
+#define KEY_V 0x56
+#define KEY_W 0x57
+#define KEY_o 0x6f
+#define KEY_p 0x70
+#define KEY_r 0x72
+#define KEY_s 0x73
+#define KEY_v 0x76
+#define KEY_w 0x77
+#define KEY_PLUS 0x2B
+#define KEY_MINUS 0x2D
+#define KEY_1 0x31
+#define KEY_2 0x32
+#define KEY_3 0x33
+#define KEY_4 0x34
+#define KEY_5 0x35
+#define KEY_6 0x36
+#define KEY_7 0x37
+#define KEY_8 0x38
+
+
+// filename pattern of snapshots (PPC does not support png)
+#ifdef WITH_GUI
+#define SNAPSHOTFILENAME "snapshot%02d.png"
+#else /* PPC */
+#define SNAPSHOTFILENAME "snapshot%02d.pnm"
+#endif /* WITH_GUI */
+
+//maximum possible resolution:
+#define MAX_RES_Y 640
+
+//Barcode detection criteria:
+#define LINES_BY_DISTANCE 5
+#define COLOUR_DIFF 20
+#define VERTICAL_LINES 4
+#define TRANSITIONS 10
+#define MAX_GAP 15
+#define CONTAINS_WHITE 200
+#define CONTAINS_BLACK 100
+#define AVERAGE_COLOUR_MAX 200 //average colour of a barcode is cca 95
+#define AVERAGE_COLOUR_MIN 40
+#define VERTICAL_TOLERANCE 10
+#define ARRAY_SIZE (((MAX_RES_Y) / (TRANSITIONS)) * (VERTICAL_LINES))
+
+
+/******************************************************************************/
+/************************** function declarations *****************************/
+/******************************************************************************/
+
+/********************** multimode graphical functions *************************/
+int saveFrame(const CvArr *img);
+void selectROI(CvCapture* capture, CvRect *roi);
+void displayFrames(IplImage *frame);
+
+/******************** multimode computational functions ***********************/
+int countThreshold(const IplImage *frame);
+int recognize(const CvMat *frame);
+
+/********************************** MODES ********************************/
+int modeImage(char *imageFilename);
+int modeVideo(CvCapture* capture, CvRect *roi);
+int modeRecognize(CvCapture* capture, CvRect *roi);
+int modeWait(int previousMode);
+
+/********************************** Key processing ********************************/
+int recognizeMode_processKeys(IplImage *frame);
+int waitMode_processKeys(int previousMode);
+
+/******************************** mode manager ********************************/
+void setAnalyticWindowsVisible(bool visible);
+void destroyGUI(void);
+int modeManager(int defaultMode, char *paramC = NULL);
+
+/*********************************** orte *************************************/
+bool getCameraControlOn(void);
+
+/********************************* application ********************************/
+int getParamI(int argc, char *argv[], char *imgFilename);
+
+#endif /* BARCAMH */
--- /dev/null
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
--- /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 -*-
+
+default_CONFIG = CONFIG_BARCOL=y
+
+ifeq ($(CONFIG_BARCOL),y)
+
+# If OpenCV is installed in /usr/local
+INCLUDES += -I /usr/local/include
+LDFLAGS += -L /usr/local/lib
+
+bin_PROGRAMS = barcol
+
+barcol_SOURCES = barcol.cxx
+barcol_LIBS = robodim pthread roboorte robottype orte cv highgui cxcore rt z jpeg
+
+endif
--- /dev/null
+/**
+ ============================================================================
+ Name : BarCoL.c
+ Author : Ondřej Holešovský
+ Description : Bar Code Locator
+ ============================================================================
+ */
+
+/**
+ * Copyright 2011 Ondřej Holešovský
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+
+int8_t i8angle=-1;
+
+//maximum possible resolution, please:
+#define MAX_RES_Y 640
+
+int RESOLUTION_X=640; //Camera resolution setting is not working!
+int RESOLUTION_Y=480;
+
+//Barcode detection criteria:
+#define LINES_BY_DISTANCE 5
+#define COLOUR_DIFF 20
+#define VERTICAL_LINES 7
+#define TRANSITIONS 20
+#define MAX_GAP 2
+#define CONTAINS_WHITE 200
+#define CONTAINS_BLACK 100
+#define AVERAGE_COLOUR_MAX 200 //average colour of a barcode is cca 95
+#define AVERAGE_COLOUR_MIN 40
+#define VERTICAL_TOLERANCE 10
+#define ARRAY_SIZE (((MAX_RES_Y)/(TRANSITIONS))*(VERTICAL_LINES))
+
+int detectedLinesPoints[3][ARRAY_SIZE];
+int detectedCodes[2][2][100];
+int codesCount=0;
+
+int myPow(int number)
+{
+ return (number*number);
+}
+
+int main(void)
+{
+ unsigned char key=0;
+
+ int i,j,k,l,m;
+ uint8_t lineBuffer[MAX_RES_Y+10];
+ CvPoint startPoint;
+ CvPoint endPoint;
+ CvPoint codeEndPoint, codeStartPoint;
+ CvPoint a, b;
+ codeEndPoint.x=-1;
+ codeEndPoint.y=-1;
+ codeStartPoint.x=-1;
+ codeStartPoint.y=-1;
+
+ unsigned int space=0;
+ unsigned int detectedTransitions=0;
+ unsigned int linesPerSample=0; //all lines in one vertical sample
+ unsigned int totalLines=0; //total count of detected lines in the image
+
+ unsigned int singleLinesPerBlock=0; //only 1 line per vertical sample
+
+ unsigned int darkest=255, lightest=0;
+ int lineBegining=-1;
+
+ int colourSum=-1;
+ int colourSamplesCount=0;
+
+ int longestCodeLength=0, longestCodePosition=0;
+ double angle;
+
+ IplImage *img = 0; //colour image
+ IplImage *gimg = 0; //gray
+ IplImage *lineImg = 0; //(only vertical lines)
+
+ CvCapture* capture = cvCaptureFromCAM(0); // capture from video device #0
+ cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, RESOLUTION_X);
+ cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, RESOLUTION_Y);
+
+ // create a window
+ cvNamedWindow("Camera", CV_WINDOW_AUTOSIZE);
+ cvMoveWindow("Camera", 100, 100);
+
+ if(!cvGrabFrame(capture)){ // capture a frame
+ printf("Could not grab a frame\n\7");
+ exit(0);
+ }
+
+ img=cvRetrieveFrame(capture,0); // retrieve the captured frame
+
+ // img=cvLoadImage("/home/eurobot/BarCoL/image.jpg",CV_LOAD_IMAGE_COLOR); //!!!!!!
+ gimg=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
+ lineImg=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
+ puts("Barcol (Barcode Locator) is running.");
+ printf("OpenCV version: %s\n",CV_VERSION);
+ RESOLUTION_X=gimg->width;
+ RESOLUTION_Y=gimg->height;
+ printf("RESX=%i; RESY=%i\n",RESOLUTION_X,RESOLUTION_Y);
+
+ while(key!=27){ //ESC key
+
+ if(!cvGrabFrame(capture)){ // capture a frame
+ printf("Could not grab a frame\n\7");
+ exit(0);
+ }
+
+ img=cvRetrieveFrame(capture,0); // retrieve the captured frame
+ cvCvtColor(img,gimg,CV_BGR2GRAY);
+ cvZero(lineImg);
+ linesPerSample=0;
+ totalLines=0;
+ singleLinesPerBlock=0;
+
+ /******* Detect vertical lines of required parameters. *******/
+ for(j=LINES_BY_DISTANCE;j<RESOLUTION_X;j+=LINES_BY_DISTANCE){
+ startPoint.x=j; startPoint.y=0;
+ endPoint.x=j; endPoint.y=RESOLUTION_Y;
+ cvSampleLine(gimg, startPoint, endPoint, lineBuffer,8); //retrieve 1 line from the grayscale image
+ space=0; detectedTransitions=0;
+ cvSetReal2D(lineImg,0,j,lineBuffer[0]); //first point of the line to the line image
+ for(i=1;i<RESOLUTION_Y;i++){
+ if((lineBuffer[i]<(lineBuffer[i-1]-COLOUR_DIFF)) || (lineBuffer[i]>(lineBuffer[i-1])+COLOUR_DIFF)){
+ if(lineBegining==-1)lineBegining=i;
+ if(codeStartPoint.x==-1){
+ codeStartPoint.x=j;
+ codeStartPoint.y=i;
+ }
+ detectedTransitions++;
+ if(detectedTransitions==TRANSITIONS){
+ darkest=255;
+ lightest=0;
+ colourSamplesCount=0;
+ colourSum=0;
+ for(k=lineBegining;k<=i;k++){
+ if(lineBuffer[k]>lightest)lightest=lineBuffer[k];
+ if(lineBuffer[k]<darkest)darkest=lineBuffer[k];
+ colourSum+=lineBuffer[k];
+ colourSamplesCount++;
+ }
+ if(darkest<=CONTAINS_BLACK && lightest>=CONTAINS_WHITE &&
+ ((colourSum/colourSamplesCount)<AVERAGE_COLOUR_MAX) && ((colourSum/colourSamplesCount)>AVERAGE_COLOUR_MIN)){
+ detectedLinesPoints[0][totalLines]=j; //X
+ detectedLinesPoints[1][totalLines]=lineBegining; //Y start
+ detectedLinesPoints[2][totalLines]=i; //Y end
+ totalLines++;
+ }else{
+ detectedTransitions=0;
+ lineBegining=-1;
+ codeStartPoint.x=-1;
+ codeStartPoint.y=-1;
+ }
+
+ space=0;
+
+ }else if(detectedTransitions>TRANSITIONS){
+ darkest=255;
+ lightest=0;
+ colourSamplesCount=0;
+ colourSum=0;
+ for(k=lineBegining;k<=i;k++){
+ if(lineBuffer[k]>lightest)lightest=lineBuffer[k];
+ if(lineBuffer[k]<darkest)darkest=lineBuffer[k];
+ colourSum+=lineBuffer[k];
+ colourSamplesCount++;
+ }
+ if(darkest<=CONTAINS_BLACK && lightest>=CONTAINS_WHITE &&
+ ((colourSum/colourSamplesCount)<AVERAGE_COLOUR_MAX) && ((colourSum/colourSamplesCount)>AVERAGE_COLOUR_MIN)){
+ detectedLinesPoints[2][totalLines-1]=i; //Y end
+ }
+ space=0;
+ }
+ space=0;
+ }else if(space<=MAX_GAP){
+ space++;
+ }else{
+ codeStartPoint.x=-1;
+ codeStartPoint.y=-1;
+ lineBegining=-1;
+ space=0;
+ detectedTransitions=0;
+ }
+
+ cvSetReal2D(lineImg,i,j,lineBuffer[i]); //1 point of the line to the line image
+ }
+
+
+ }
+ angle=-1;
+ /******* Detect blocks of lines. They should be a part of a bar code. *******/
+ if(totalLines>=VERTICAL_LINES){
+ codesCount=0;
+ singleLinesPerBlock=1;
+ codeStartPoint.x=-1;
+ codeStartPoint.y=-1;
+ for(i=0;i<(totalLines-VERTICAL_LINES+1);i++){
+ codeStartPoint.x=-1;
+ codeStartPoint.y=-1;
+ singleLinesPerBlock=1;
+ k=i+1;
+ l=LINES_BY_DISTANCE;
+ while((detectedLinesPoints[0][k]<(detectedLinesPoints[0][i]+l))&&k<(totalLines-VERTICAL_LINES+1))k++;
+
+ while((detectedLinesPoints[0][k]==detectedLinesPoints[0][i]+l) && k<(totalLines-VERTICAL_LINES+1)){
+ if(detectedLinesPoints[1][k]<detectedLinesPoints[1][i]+VERTICAL_TOLERANCE && detectedLinesPoints[1][k]>detectedLinesPoints[1][i]-VERTICAL_TOLERANCE){
+ singleLinesPerBlock++;
+ if(singleLinesPerBlock==VERTICAL_LINES){
+ codeStartPoint.x=detectedLinesPoints[0][i];
+ codeStartPoint.y=detectedLinesPoints[1][i];
+ codeEndPoint.x=detectedLinesPoints[0][k];
+ codeEndPoint.y=detectedLinesPoints[2][k];
+ for(j=i;j<=k;j++){
+ a.x=detectedLinesPoints[0][j];
+ a.y=detectedLinesPoints[1][j];
+ b.x=detectedLinesPoints[0][j];
+ b.y=detectedLinesPoints[2][j];
+ cvLine(gimg,a,b,CV_RGB(255,255,255),2,8,0);
+ }
+ }
+ if(singleLinesPerBlock>VERTICAL_LINES){
+ codeEndPoint.x=detectedLinesPoints[0][k];
+ codeEndPoint.y=detectedLinesPoints[2][k];
+ a.x=detectedLinesPoints[0][k];
+ a.y=detectedLinesPoints[1][k];
+ b.x=detectedLinesPoints[0][k];
+ b.y=detectedLinesPoints[2][k];
+ cvLine(gimg,a,b,CV_RGB(255,255,255),2,8,0);
+ }
+ l+=LINES_BY_DISTANCE;
+ while((detectedLinesPoints[0][k]<(detectedLinesPoints[0][i]+l))&&k<(totalLines-VERTICAL_LINES+1))k++;
+ continue;
+ }
+ k++;
+ }
+ /******* Merge similar blocks into one. *******/
+ if(codeStartPoint.x!=-1 && codesCount<100){
+ if(codesCount>0){
+ for(m=0;m<codesCount;m++){
+ if(detectedCodes[1][0][m]==codeEndPoint.x && detectedCodes[1][1][m]==codeEndPoint.y){
+ codeStartPoint.x=-1;
+ break;
+ }
+ }
+ if(codeStartPoint.x!=-1){
+ codesCount++;
+ detectedCodes[0][0][codesCount-1]=codeStartPoint.x;
+ detectedCodes[0][1][codesCount-1]=codeStartPoint.y;
+ detectedCodes[1][0][codesCount-1]=codeEndPoint.x;
+ detectedCodes[1][1][codesCount-1]=codeEndPoint.y;
+ cvLine(gimg,codeStartPoint,codeEndPoint,CV_RGB(255,255,255),2,8,0);
+ //printf("Detected code: %i %i; %i %i\n",codeStartPoint.x,codeStartPoint.y,codeEndPoint.x,codeEndPoint.y);
+ }
+ }else{
+ codesCount++;
+ detectedCodes[0][0][0]=codeStartPoint.x;
+ detectedCodes[0][1][0]=codeStartPoint.y;
+ detectedCodes[1][0][0]=codeEndPoint.x;
+ detectedCodes[1][1][0]=codeEndPoint.y;
+ cvLine(gimg,codeStartPoint,codeEndPoint,CV_RGB(255,255,255),2,8,0);
+ //printf("Detected code: %i %i; %i %i\n",codeStartPoint.x,codeStartPoint.y,codeEndPoint.x,codeEndPoint.y);
+ }
+ }
+
+ }
+ /******* Determine the largest bar code and the middle of it. *******/
+ if(codesCount>0){
+ longestCodeLength=sqrt(myPow(detectedCodes[1][0][0]-detectedCodes[0][0][0])+myPow(detectedCodes[1][1][0]-detectedCodes[0][1][0]));
+ longestCodePosition=0;
+ for(m=0;m<codesCount;m++){
+ i=sqrt(myPow(detectedCodes[1][0][m]-detectedCodes[0][0][m])+myPow(detectedCodes[1][1][m]-detectedCodes[0][1][m]));
+ if(i>longestCodeLength){
+ longestCodeLength=i;
+ longestCodePosition=m;
+ }
+ }
+ codeStartPoint.x=detectedCodes[0][0][longestCodePosition];
+ codeStartPoint.y=detectedCodes[0][1][longestCodePosition];
+ codeEndPoint.x=detectedCodes[1][0][longestCodePosition];
+ codeEndPoint.y=detectedCodes[1][1][longestCodePosition];
+ cvLine(gimg,codeStartPoint,codeEndPoint,CV_RGB(0,0,0),2,8,0);
+ a.x=(codeStartPoint.x+codeEndPoint.x)/2;
+ a.y=(codeStartPoint.y+codeEndPoint.y)/2;
+ cvCircle(gimg,a,3,CV_RGB(0,0,0),-1,16,0);
+ angle=atan((double)a.x/824);
+ angle=(180/3.14159)*angle;
+ printf("Detected code: x=%i y=%i - angle %i\n",a.x,a.y, (int)angle);
+ }else{
+ angle=-1;
+ }
+ i8angle=(int8_t)angle;
+ }
+ //cvCircle(gimg,codeEndPoint,3,CV_RGB(255,255,255),-1,8,0);
+ //printf("Detected code: %i,%i\n",codeEndPoint.x,codeEndPoint.y);
+
+
+ cvShowImage("Camera", gimg);
+ // wait for a key
+ key=cvWaitKey(5);
+ }
+ // release the image
+ cvReleaseImage(&img );
+ cvReleaseImage(&gimg );
+ cvReleaseImage(&lineImg );
+ cvReleaseCapture(&capture);
+ cvDestroyWindow("Camera");
+ return 0;
+}
/**
* @file cand.cc
* @date 08/04/08
- *
+ *
* @brief CAN-ORTE bridge
- *
+ *
* Copyright: (c) 2008 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
}
can_addr.can_ifindex = can_ifreq.ifr_ifindex;
-
+
if (!can_addr.can_ifindex) {
perror("cand: invalid socket interface");
return -3;
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);
{
unsigned char data[4];
- data[0] = orte_data->motion_speed.right >> 8;
- data[1] = orte_data->motion_speed.right & 0xff;
- data[2] = orte_data->motion_speed.left >> 8;
- data[3] = orte_data->motion_speed.left & 0xff;
+ data[0] = orte_data->motion_speed.left >> 8;
+ data[1] = orte_data->motion_speed.left & 0xff;
+ data[2] = orte_data->motion_speed.right >> 8;
+ data[3] = orte_data->motion_speed.right & 0xff;
can_send(CAN_MOTION_CMD, 4, data);
return 0;
}
-int set_vidle_cmd(uint16_t req_pos, char speed)
+int set_crane_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data[2];
+ unsigned char data[3];
- data[0] = req_pos >> 8;
- data[1] = req_pos & 0xff;
- data[2] = speed;
- can_send(CAN_VIDLE_CMD, 3, data);
+ data[0] = orte_data->crane_cmd.req_pos >> 8;
+ data[1] = orte_data->crane_cmd.req_pos & 0xff;
+ data[2] = orte_data->crane_cmd.speed;
+ can_send(CAN_CRANE_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_magnet_cmd(unsigned char on)
{
- unsigned char data = orte_data->hokuyo_pitch.angle;
+ unsigned char data;
+
+ data = on;
+ can_send(CAN_MAGNET_CMD, 1, &data);
- can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
return 0;
}
-
int can_send(canid_t id, unsigned char length, unsigned char *data)
{
struct can_frame frame;
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_CRANE_STATUS:
+ orte->crane_status.act_pos = (frame.data[0] << 8 | frame.data[1]);
+ orte->crane_status.response = (frame.data[2] << 8 | frame.data[3]);
+ orte->crane_status.flags = frame.data[4];
+ ORTEPublicationSend(orte->publication_crane_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;
- ORTEPublicationSend(orte->publication_robot_switches);
+ case CAN_ROBOT_BUMPERS:
+ orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
+ orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
+ orte->robot_bumpers.bumper_rear = (frame.data[0] & CAN_BUMPER_REAR) ? 0 : 1;
+ ORTEPublicationSend(orte->publication_robot_bumpers);
break;
+ case CAN_ROBOT_SWITCHES:
+ orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY) ? 1 : 0;
+ ORTEPublicationSend(orte->publication_robot_switches);
+ break;
- /* positioning by odometry */
+ /* positioning by independent odometry */
case CAN_ODO_DATA:
- orte->odo_data.left =
+ orte->odo_data.left =
((frame.data[0]<<24)|
(frame.data[1]<<16)|
(frame.data[2]<<8));
- orte->odo_data.right =
+ orte->odo_data.right =
((frame.data[3]<<24)|
(frame.data[4]<<16)|
(frame.data[5]<<8));
ORTEPublicationSend(orte->publication_odo_data);
break;
- /* positioning by odometry */
+ /* positioning by odometry from motors*/
case CAN_MOTION_ODOMETRY_SIMPLE:
- orte->motion_irc.right =
- ((frame.data[0]<<24)|
- (frame.data[1]<<16)|
- (frame.data[2]<<8));
- orte->motion_irc.left =
- ((frame.data[3]<<24)|
- (frame.data[4]<<16)|
- (frame.data[5]<<8));
- orte->motion_irc.seq = frame.data[6];
- ORTEPublicationSend(orte->publication_motion_irc);
- break;
+ orte->motion_irc.left =
+ ((frame.data[0]<<24)|
+ (frame.data[1]<<16)|
+ (frame.data[2]<<8));
+ orte->motion_irc.right =
+ ((frame.data[3]<<24)|
+ (frame.data[4]<<16)|
+ (frame.data[5]<<8));
+ orte->motion_irc.seq = frame.data[6];
+ ORTEPublicationSend(orte->publication_motion_irc);
+ break;
/* 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 =
- (frame.data[2]<<8)|(frame.data[3]);
+ 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) | \
orte->pwr_voltage.voltage50 = volt50;
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);
}
}
-void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
}
}
-void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
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 rcv_motion_speed_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:
/* 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;
case DEADLINE:
- //printf("motor cmd deadline occurred, stopping motors\n");
- orte_data->motion_speed.left = 0;
- orte_data->motion_speed.right = 0;
- set_motor_speed(orte_data);
- break;
+ /* stopping motors, release motion control feedback */
+ /* motion speed value greater then 0xFF00 causes feedback release */
+ orte_data->motion_speed.left = 0xFFFF;
+ orte_data->motion_speed.right = 0xFFFF;
+ set_motor_speed(orte_data);
+ break;
}
}
-void rcv_vidle_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_crane_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_crane_cmd(orte_data);
break;
case DEADLINE:
break;
}
}
-void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_magnet_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+ struct magnet_cmd_type *magnet_cmd = (struct magnet_cmd_type *)vinstance;
switch (info->status) {
case NEW_DATA:
- set_hokuyo_pitch(orte_data);
+ set_magnet_cmd(magnet_cmd->on);
break;
case DEADLINE:
-// printf("ORTE deadline occurred - hokuyo pitch receive\n");
break;
}
}
-
int main(int argc, char *argv[])
{
int ret;
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_crane_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_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
+ robottype_subscriber_crane_cmd_create(&orte, rcv_crane_cmd_cb, &orte);
+ robottype_subscriber_magnet_cmd_create(&orte, rcv_magnet_cmd_cb, &orte);
robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
-
+
printf("subscribers OK\n");
* @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_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 */
+/* ids of can-peripherials */
+#define CAN_ROBOT_BUMPERS to_boa(0x30)
+#define CAN_ROBOT_SWITCHES to_boa(0x31)
+#define CAN_SHARP_DATA to_boa(0x32)
+#define CAN_CRANE_STATUS to_boa(0x33)
+#define CAN_CRANE_CMD to_per(0x34)
+#define CAN_MAGNET_CMD to_per(0x35)
-
-#define CAN_ROBOT_SWITCHES to_boa(0x30)
-
-// 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 to_per(0x40) /* BOA->PER */
+ /* spodni 3 bity: zapnout. dalsi 3 b vypnout */
#define CAN_PWR_ADC1 to_boa(0x41) /* PER->BOA */
#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_VIDLE_STATUS to_boa(0x48)
-#define CAN_VIDLE_CMD to_per(0x49)
-
// #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_CRANE_STATUS message */
+#define CAN_CRANE_INITIALIZING 0x01
+#define CAN_CRANE_TIMEOUT 0x02
+#define CAN_CRANE_OUT_OF_BOUNDS 0x04
-#define CAN_SWITCH_BUMPER 0x01
-#define CAN_SWITCH_COLOR 0x02
-#define CAN_SWITCH_LEFT 0x04
-#define CAN_SWITCH_RIGHT 0x08
+/* switches */
+#define CAN_SWITCH_STRATEGY 0x01
+
+/* bumpers */
+#define CAN_BUMPER_REAR 0x01
+#define CAN_BUMPER_LEFT 0x02
+#define CAN_BUMPER_RIGHT 0x04
+
+/* 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
+
+enum robot_start_state {
+ POWER_ON = 0,
+ START_PLUGGED_IN = 1,
+ COMPETITION_STARTED = 2,
+};
+
+#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
PWR 3
HOK 4
APP 5
- VID 6
+ MAN 6
STA 7
#END
names[3] := "PWR";
names[4] := "HOK";
names[5] := "APP";
- names[6] := "VID";
+ names[6] := "MAN";
names[7] := "STA";
idx := 0;
//*********************************************************************************************
func paint_corn()
- gfx_Set(PEN_SIZE, 1);//table
+ gfx_Set(PEN_SIZE, 1);//table
gfx_Rectangle(180, 126, 239, 222, WHITE);//table
- gfx_Rectangle(180, 126, 239, 144, WHITE);//table
+ //gfx_Rectangle(180, 126, 239, 144, WHITE);//table
gfx_Rectangle(179, 126, 239, 222, WHITE);//table
- txt_MoveCursor(8,15);
- txt_Set(TEXT_COLOUR, GRAY);
- print("Corn:")
+ //txt_MoveCursor(8,15);
+ //txt_Set(TEXT_COLOUR, GRAY);
+ //print("Corn:")
endfunc
//*********************************************************************************************
if(id == CORNS && index == 2)
corns[0] := RX_buffer[0];
corns[1] := RX_buffer[1];
- redraw_corns();//draw_status();
+ //redraw_corns();//draw_status();
id:=0;
goto out;//return;
endif
if(RX_buffer[0] == 0)
dress_color := BLUE;
endif
- redraw_corns();
+ //redraw_corns();
id:=0;
goto out;//return;
endif
until(cnt++ == 8)
paint_position();
redraw_position();
- redraw_corns();
+ //redraw_corns();
redraw_main_state();
redraw_move_state();
redraw_act_state();
/**
* @file displayd.c
* @date 10/04/19
- *
+ *
* @brief Display-ORTE bridge
- *
+ *
* Copyright: (c) 2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
#include <stdlib.h>
#include <signal.h>
#include <stdbool.h>
+#include <can_msg_def.h>
struct sercom_data* sercom;
int interrupt = 0;
/**
* 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.
+ * Return 1 if the difference is negative, otherwise 0.
*/
int timespec_subtract (struct timespec *result,
struct timespec *x,
status = STATUS_OK;
break;
case DEADLINE:
+ uoled_display_voltage(0,0,0,0);
status = STATUS_FAILED;
break;
}
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;
+ switch (instance->start_condition) {
+ case POWER_ON:
+ case COMPETITION_STARTED:
+ status = STATUS_WARNING;
+ break;
+ case START_PLUGGED_IN:
+ status = STATUS_OK;
+ break;
+ }
break;
case DEADLINE:
status = STATUS_FAILED;
break;
}
if (status != last_status ||
- miliseconds_elapsed_since(&last_sent, 500)) {
+ miliseconds_elapsed_since(&last_sent, 1000)) {
uoled_display_status(STA, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
switch (info->status) {
case NEW_DATA:
- if (instance->team_color != last_color ||
- miliseconds_elapsed_since(&last_sent, 1000)) {
- uoled_display_color(instance->team_color);
- clock_gettime(CLOCK_MONOTONIC, &last_sent);
- }
- last_color = instance->team_color;
+// if (instance->team_color != last_color ||
+// miliseconds_elapsed_since(&last_sent, 1000)) {
+// uoled_display_color(instance->team_color);
+// clock_gettime(CLOCK_MONOTONIC, &last_sent);
+// }
+// last_color = instance->team_color;
case DEADLINE:
status = STATUS_FAILED;
break;
last_status = status;
}
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_status_type *m = (struct vidle_status_type *)vinstance;
+ struct crane_status_type *m = (struct crane_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 == 0)
- status = STATUS_OK;
- else
- status = STATUS_WARNING;
+ status = STATUS_OK;
break;
case DEADLINE:
status = STATUS_FAILED;
/* Neni potreba aktualizovat stav na displeji 25x za sekundu */
if (status != last_status ||
miliseconds_elapsed_since(&last_sent, 1000)) {
- uoled_display_status(VID, status);
+ uoled_display_status(MAN, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
static UDE_hw_status_t last_status;
static struct timespec last_sent;
struct camera_result_type *instance = (struct camera_result_type *)vinstance;
- char s, c;
+ char valid;
switch (info->status) {
case NEW_DATA:
if (instance->error == 0)
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
+}
+
+void rcv_system_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ system_status *instance = (system_status *)vinstance;
- if (status == STATUS_OK) {
- s = (instance->side >= 0 && instance->side < 10) ?
- '0'+instance->side : 'E';
- c = (instance->center >= 0 && instance->center < 10) ?
- '0'+instance->center : 'E';
- } else
- s = c = '-';
- uoled_display_corns(s, c);
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->system_condition)
+ status = STATUS_WARNING;
+ else
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(APP, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
}
void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- status = STATUS_OK;
+ status = STATUS_OK;
break;
case DEADLINE:
status = STATUS_FAILED;
}
if (status != last_status ||
miliseconds_elapsed_since(&last_sent, 100)) {
- uoled_display_status(APP, status);
if (status == STATUS_OK)
uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
signal(SIGTERM, sig_handler);
//struct can_frame frame;
- tty = getenv("DISPLAY_TTY");
+ tty = getenv("DISPLAY_TTY");
if (!tty){
tty="/dev/ttyUSB0";
printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
/* creating subscribers */
robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
- robottype_subscriber_vidle_status_create(&orte, rcv_vidle_status_cb, NULL);
+ robottype_subscriber_crane_status_create(&orte, rcv_crane_status_cb, NULL);
robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
+ robottype_subscriber_system_status_create(&orte, rcv_system_status_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_can_msg_create(&orte, rcv_can_msg_cb, &orte);
4. PWR - napajeci zdroje
5. HOK - Hokuyo
6. APP - ridici aplikace
- 7. VID - ovladani vidli a dvirek na vysypavani
+ 7. MAN - ovladani manipulatoru
8. STA - startovaci tlacitko
Format zpravy pro displej:
* 4. PWR - napajeci zdroje
* 5. HOK - Hokuyo
* 6. APP - ridici aplikace
- * 7. VID - ovladani vidli a dvirek na vysypavani
+ * 7. MAN - ovladani manipulatoru
* 8. STA - startovaci tlacitko
*/
typedef enum {
PWR = 4,
HOK = 5,
APP = 6,
- VID = 7,
+ MAN = 7,
STA = 8
} UDE_component_t;
- \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.
--- /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_demo
+
+eb_demo_SOURCES = main.c fsm.c fsm_crane.c uar.c
+eb_demo_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+#include <expansion.h>
+
+#define CAN_SPEED 1000000 /* CAN bus speed */
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+#define START_PIN EXPPORT_8 /* start pin */
+#define SWITCH_STRATEGY_PIN EXPPORT_4 /* strategy pin */
+#define BUMPER_REAR_PIN EXPPORT_5 /* bumper pin */
+#define BUMPER_LEFT_PIN EXPPORT_6 /* left bumper */
+#define BUMPER_RIGHT_PIN EXPPORT_7 /* right bumper */
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+enum {
+ LONG_PRESS,
+ SHORT_PRESS
+} button_states;
+
+enum {
+ PRESSED = 1,
+ RELEASED = 0
+} button;
+
+#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
+ uint32_t act_pos; // actual position
+ uint32_t req_pos; // requested position
+ volatile uint32_t can_req_position; // next requested position
+ uint32_t req_target;
+ uint32_t req_spd;
+ uint32_t can_req_spd;
+ char req_magnet;
+ char act_magnet;
+ volatile char can_req_magnet;
+ uint32_t sharp_L;
+ uint32_t sharp_R;
+ uint32_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;
+};
+
+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 "fsm.h"
+#include <uar.h>
+#include <servo.h>
+#include <engine.h>
+#include <stdbool.h>
+#include "def.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_crane_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->can_req_position = fsm->act_pos;
+ fsm->req_target = fsm->act_pos;
+ fsm->req_pos = fsm->act_pos;
+ fsm->start_pos = fsm->act_pos;
+ fsm->req_spd = 0;
+ fsm->flags |= CAN_CRANE_INITIALIZING;
+ break;
+ case EVENT_DO:
+ /* TODO: Homing */
+ fsm->flags &= ~CAN_CRANE_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void magnet_on()
+{
+ //deb_led_on(LEDY);
+ engine_B_pwm(100);
+ engine_B_en(ENGINE_EN_ON);
+}
+
+static void magnet_off()
+{
+ //deb_led_off(LEDY);
+ engine_B_pwm(0);
+ engine_B_en(ENGINE_EN_OFF);
+}
+
+static void stop() {
+ engine_A_pwm(0);
+ engine_A_en(ENGINE_EN_OFF);
+}
+
+#define DEAD_ZONE 10
+
+static bool do_control(struct fsm *fsm)
+{
+ bool finished;
+ int action;
+ int e = fsm->req_pos - fsm->act_pos;
+
+ if (fsm->req_magnet)
+ magnet_on();
+ else
+ magnet_off();
+
+ action = (e);
+ engine_A_dir(action < 0);
+
+ action = action > 0 ? action : -action;
+ 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;
+ static char last_can_req_magnet = 0;
+
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ do_control(fsm);
+
+ if (fsm->can_req_magnet != last_can_req_magnet) {
+ last_can_req_magnet = fsm->can_req_magnet;
+ fsm->req_magnet = fsm->can_req_magnet;
+ }
+
+ 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->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_CRANE_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 ? 3000 : 5000)) {
+ fsm->flags |= CAN_CRANE_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+ finished = do_control(fsm);
+ 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 Michal Vokac <vokacmic@fel.cvut.cz>
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup demo
+ */
+
+
+/**
+ * @defgroup Demo Demo application
+ */
+/**
+ * @ingroup demo
+ * @{
+ */
+
+#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 <servo.h>
+#include <uar.h>
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+
+struct fsm fsm_crane;
+
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void fsm_crane_init(struct fsm *fsm, enum event event);
+void timer0_irq() __attribute__((interrupt));
+
+/*********************************************************/
+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 B
+ engine_B_dir(ENGINE_DIR_FW); //set direction
+ engine_B_pwm(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;
+}
+
+/*********************************************************/
+/** timer0 & ISR **/
+
+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)
+{
+ can_msg_t rx_msg;
+ int req_pos = 0;
+ int req_spd = 0;
+ char magnet = 0;
+
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id) {
+ case CAN_CRANE_CMD:
+ deb_led_on(LEDB);
+ /* save crane req. position and speed */
+ req_pos = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ req_spd = rx_msg.data[2];
+
+ if (req_pos >= 0x80 && req_pos <= 0x240) {
+ fsm_crane.flags &= ~CAN_CRANE_OUT_OF_BOUNDS;
+ fsm_crane.can_req_position = req_pos;// save new req position of lift
+ fsm_crane.can_req_spd = req_pos;// save new req spd of lift
+ } else
+ fsm_crane.flags |= CAN_CRANE_OUT_OF_BOUNDS;
+ break;
+ case CAN_MAGNET_CMD:
+ /* save magnet state */
+ magnet = rx_msg.data[0];
+
+ /* set electromagnet state */
+ fsm_crane.can_req_magnet = magnet;
+ 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_CRANE_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_crane.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_crane.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_crane.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_crane.can_response & 0xFF;
+ tx_msg.data[4] = fsm_crane.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 blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500) {
+ led_time = timer_msec;
+ deb_led_change(LEDG);
+// send_rs_str("pos: ");
+// send_rs_int(fsm_crane.act_pos);
+// send_rs_str(" L: ");
+// send_rs_int(fsm_crane.sharp_L);
+// send_rs_str(" R: ");
+// send_rs_int(fsm_crane.sharp_R);
+// send_rs_str("\n");
+ }
+}
+
+/*
+ This function handles homing/starting/stopping of the robot using one push button.
+ When power supply is applied, board is in POWER_UP state => start plug not inserted.
+ Short start button press causes transition to READY state => start plug insertition, actuators homing
+ Long start button press causes transition to STARTED state => start plug removed.
+*/
+void start_handling(void)
+{
+ can_msg_t msg;
+ static uint32_t next_start_send = 0;
+ static uint32_t pressed_time = 0;
+ static uint32_t handle_button_time = 0;
+
+ char start_button;
+
+ /* it is necassary initialize this to value 1
+ If the initial value is zero - SHORT_PRESS signal is generated when power supply is applied
+ => but we do not want this after power on */
+ static bool last_start_button = 0;
+
+ static bool start_condition = 0;
+
+ int hold_time = 0;
+
+ static char start_state = POWER_ON;
+
+ start_button = (IO0PIN & (1<<START_PIN)) == 0 ? PRESSED : RELEASED;
+
+ if (timer_msec >= handle_button_time + 10) {
+ if (start_button != last_start_button && start_button == PRESSED) {
+ // button presed
+ last_start_button = start_button;
+ pressed_time = timer_msec; // save time when button is presed
+ send_rs_str("button pressed\n");
+ } else if (start_button != last_start_button && start_button == RELEASED) {
+ // button released
+ last_start_button = start_button;
+ pressed_time = 0;
+ send_rs_str("button released\n");
+ } else if (start_button == PRESSED) {
+ // button hold
+ send_rs_str("button hold\n");
+ last_start_button = start_button;
+
+ hold_time = timer_msec - pressed_time;
+
+ // check how long the button was pressed
+ if (hold_time >= 100) {
+ start_condition = SHORT_PRESS;
+ }
+ if (hold_time >= 1000){
+ start_condition = LONG_PRESS;
+ }
+ }
+ handle_button_time = timer_msec;
+ }
+
+ if (timer_msec >= next_start_send + 200) {
+
+ char start_signal = 0;
+
+ switch (start_state) {
+ case POWER_ON:
+ /* state after power supply applied */
+ if (start_condition == SHORT_PRESS) {
+ start_state = START_PLUGGED_IN;
+ }
+ deb_led_off(LEDY);
+ start_signal = POWER_ON;
+ send_rs_str("POWER_ON\n");
+ break;
+ case START_PLUGGED_IN:
+ /* ready state */
+ if (start_condition == LONG_PRESS) {
+ start_state = COMPETITION_STARTED;
+ }
+ deb_led_on(LEDY);
+ start_signal = START_PLUGGED_IN;
+ send_rs_str("READY\n");
+ break;
+ case COMPETITION_STARTED:
+ /* demo started */
+ if (start_condition == SHORT_PRESS) {
+ start_state = START_PLUGGED_IN;
+ }
+ deb_led_change(LEDY);
+ start_signal = COMPETITION_STARTED;
+ send_rs_str("STARTED\n");
+ break;
+ }
+
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_signal;
+ /*while*/ (can_tx_msg(&msg));
+
+ next_start_send = timer_msec;
+ }
+}
+
+void handle_switches()
+{
+ static uint32_t switch_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= switch_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ switch_time = timer_msec;
+
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
+ sw &= ~CAN_SWITCH_STRATEGY;
+ else
+ sw |= CAN_SWITCH_STRATEGY;
+
+ if (sw & CAN_SWITCH_STRATEGY){
+ //deb_led_on(LEDY);
+ }
+ else
+ //deb_led_off(LEDY)
+ ;
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+void handle_bumpers()
+{
+ 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_PIN))
+ sw &= ~CAN_BUMPER_REAR;
+ else
+ sw |= CAN_BUMPER_REAR;
+
+ if (IO0PIN & (1<<BUMPER_LEFT_PIN))
+ sw &= ~CAN_BUMPER_LEFT;
+ else
+ sw |= CAN_BUMPER_LEFT;
+
+ if (IO0PIN & (1<<BUMPER_RIGHT_PIN))
+ sw &= ~CAN_BUMPER_RIGHT;
+ else
+ sw |= CAN_BUMPER_RIGHT;
+
+ if (sw & (CAN_BUMPER_REAR|CAN_BUMPER_LEFT|CAN_BUMPER_RIGHT))
+ deb_led_on(LEDR);
+ else
+ deb_led_off(LEDR);
+
+ tx_msg.id = CAN_ROBOT_BUMPERS;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+/*********************************************************/
+void can_send_sharp()
+{
+ static uint32_t sharp_time = 0;
+
+ if (timer_msec >= sharp_time + 100) {
+ can_msg_t tx_msg;
+ sharp_time = timer_msec;
+
+ tx_msg.id = CAN_SHARP_DATA;
+ tx_msg.dlc = 4;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_crane.sharp_L >> 8) & 0xff;
+ tx_msg.data[1] = fsm_crane.sharp_L & 0xff;
+ tx_msg.data[2] = (fsm_crane.sharp_R >> 8) & 0xff;
+ tx_msg.data[3] = fsm_crane.sharp_R & 0xff;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+ }
+}
+
+
+/*********************************************************/
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+
+ init_periphery();
+
+ SET_PIN(PINSEL1, (START_PIN - 16), PINSEL_0);
+ SET_PIN(PINSEL0, SWITCH_STRATEGY_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, (BUMPER_LEFT_PIN - 16), PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_RIGHT_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, (BUMPER_REAR_PIN - 16), PINSEL_0);
+
+ IO0DIR &= ~(1<<START_PIN);
+ IO0DIR &= ~(1<<SWITCH_STRATEGY_PIN);
+ IO0DIR &= ~((1<<BUMPER_REAR_PIN) | (1<<BUMPER_LEFT_PIN) | (1<<BUMPER_RIGHT_PIN));
+
+ send_rs_str("Demo started\n");
+ // The above send_rs_str is important - we wait for the first AD conversion to be finished
+ fsm_crane.act_pos = adc_val[2];
+ init_fsm(&fsm_crane, &fsm_crane_init);
+
+ while (1) {
+ if (timer_usec >= main_time + 1000) {
+ main_time = timer_usec;
+
+ fsm_crane.sharp_L = adc_val[0];
+ fsm_crane.sharp_R = adc_val[1];
+ fsm_crane.act_pos = adc_val[2];
+ run_fsm(&fsm_crane);
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_crane.trigger_can_send) { //or when something important happen
+ fsm_crane.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ can_send_status();
+ }
+
+ start_handling();
+ handle_bumpers();
+ handle_switches();
+ blink_led();
+ //can_send_sharp();
+ }
+}
+
+/** @} */
--- /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
+//-------------------------------------------------\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
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);
+ pwr_33(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
# -*- 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
# -*- 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_crane(CRANE_UP);
} else {
;//act OFF
}
break;
case BT2:
if(state) {
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ act_crane(CRANE_DOWN);
} else {
;//act OFF
}
/* creating publishers */
robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte);
- robottype_publisher_vidle_cmd_create(&orte, send_dummy_cb, &orte);
-
+ robottype_publisher_crane_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
spd_left = (msg_rcv.data[0] << 8) | msg_rcv.data[1];
spd_right= (msg_rcv.data[2] << 8) | msg_rcv.data[3];
- pxmc_spd(&mcs_left, -spd_left, pxmc_sfikhz*1000 /*timeout in sampling periods = 0.5ms*/);
- pxmc_spd(&mcs_right,+spd_right, pxmc_sfikhz*1000);
+ if (spd_left < 0xFF00 && spd_right < 0xFF00) {
+ pxmc_spd(&mcs_left, -spd_left, pxmc_sfikhz*1000 /*timeout in sampling periods = 0.5ms*/);
+ pxmc_spd(&mcs_right,+spd_right, pxmc_sfikhz*1000);
+ } else {
+ /* release motors feedback */
+ pxmc_set_const_out(&mcs_left, 0);
+ pxmc_set_const_out(&mcs_right, 0);
+ }
/* printf("Left motor speed command: %08x\n",spd_left); */
/* printf("Right motor speed command: %08x\n",spd_right); */
led_can_rec(50);
{
static unsigned last_send_time=0;
Message m;
+ uint16_t status;
if (pxmc_msec_counter - last_send_time >= 500) {
last_send_time = pxmc_msec_counter;
memset(&m, 0, sizeof(m));
m.cob_id.w = CAN_MOTION_STATUS;
m.len = 4;
- m.data[0] = mcs_left.pxms_flg & PXMS_ERR_m ? mcs_left.pxms_errno >> 8 : 0;
- m.data[1] = mcs_left.pxms_flg & PXMS_ERR_m ? mcs_left.pxms_errno & 0xff : 0;
- m.data[2] = mcs_right.pxms_flg & PXMS_ERR_m ? mcs_right.pxms_errno >> 8 : 0;
- m.data[3] = mcs_right.pxms_flg & PXMS_ERR_m ? mcs_right.pxms_errno & 0xff : 0;
+
+ if (mcs_left.pxms_flg & PXMS_ERR_m) {
+ status = mcs_left.pxms_errno;
+ } else if ((mcs_left.pxms_flg & PXMS_PHA_m) == 0) {
+ status = 0x200;
+ } else
+ status = 0;
+
+ m.data[0] = status >> 8;
+ m.data[1] = status & 0xff;
+
+ if (mcs_right.pxms_flg & PXMS_ERR_m) {
+ status = mcs_right.pxms_errno;
+ } else if ((mcs_right.pxms_flg & PXMS_PHA_m) == 0) {
+ status = 0x200;
+ } else
+ status = 0;
+
+ m.data[2] = status >> 8;
+ m.data[3] = status & 0xff;
+
canMsgTransmit(0, m);
}
}
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");
+ pxmc_set_const_out(&mcs_left,0);
+ pxmc_set_const_out(&mcs_right,0);
+
int32_t receive_id[] = { CAN_CORR_TRIG, 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");
/**
* @file map.c
- * @brief Useful functions related map
+ * @brief Useful functions related map
* @author Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
- *
+ *
* This file contains functions to manage map.
* The map should be accessed only by this library.
*
/** @addtogroup maplib */
/**
* @{
- *
+ *
*/
* @name Shared Memory Map related functions
* @{
*/
-/**
+/**
* @brief Init Shared Map memory
*
* @param init_flag 1 to init the map memory and set all cells with
const int shmap_size = sizeof(struct map);
if (map == NULL){
// Allocate memory to shared map
- shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | S_IRUSR | S_IWUSR); // Removed flag IPC_EXCL
-
+ shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | S_IRUSR | S_IWUSR); // Removed flag IPC_EXCL
+
if (shmap_id == -1) {
perror("shmget");
exit(1);
}
-
+
/* Attach the shared memory segment. */
map = (struct map *) shmat (shmap_id, 0, 0);
if ((int)map == -1) {
perror("shmat");
exit(1);
}
-
+
/* Initialize Map Memory */
if (init_flag) ShmapAllFreeSpace();
-
+
//printf("Map initialized\n");
}
return map;
}
-/**
+/**
* @brief Free Shared Map memory
*/
void ShmapFree(void){
shmctl (shmap_id, IPC_RMID, 0);
}
-/**
+/**
* @brief Deatach Shared Map memory
*/
void ShmapDt(void){
shmdt (map);
}
-/**
+/**
* @brief Check if Shared Map memory is init
* @return Pointer to the map or NULL if not initialized.
*/
int ShmapIsFreeCell(int x, int y)
{
struct map_cell *cell;
- if(map && ShmapIsCellInMap(x,y)) {
- bool free;
+
+ if (map && ShmapIsCellInMap(x,y)) {
+ bool free;
cell = &(map->cells[y][x]);
- free = (cell->detected_obstacle == 0) &&
- (((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST|MAP_FLAG_PLAN_MARGIN)) == 0) ||
- ((cell->flags & MAP_FLAG_WALL) && (cell->flags & MAP_FLAG_INVALIDATE_WALL)));
- return free;
+ free = ((cell->detected_obstacle == 0) &&
+ ((cell->flags & (MAP_FLAG_WALL | MAP_FLAG_DET_OBST | MAP_FLAG_PLAN_MARGIN)) == 0));
+ return free;
}
else return -1;
}
* @brief Creates an obstacle in the map with a square shape
* @param xs Coordonate X (in m) of the central point
* @param ys Coordonate Y (in m) of the central point
- * @param r Radius (in m)
+ * @param r Radius (in m)
* @param cell Type of obstacle.
- *
+ *
*/
int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
{
// define a circumscribe square - just to shorten cyclic pass through the playground
//ShmapPoint2Cell(xs-r, ys-r, &i1, &j1, &valid);
//ShmapPoint2Cell(xs+r, ys+r, &i2, &j2, &valid);
-
+
i1 = 0;
i2 = MAP_WIDTH;
j1 = 0;
* @param x2 Coordonate X (in m) of the second point
* @param y2 Coordonate Y (in m) of the second point
* @param cell Type of obstacle.
- *
+ *
*/
int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
{
i2 = i1;
i1 = i;
}
-
+
if (j1 > j2) {
j = j2;
j2 = j1;
j1 = j;
}
-
+
//DBG("Drawing a rectangle between points (%d, %d) and (%d,%d)\n", init_i,init_j,limit_i,limit_j);
for(i=i1; i<=i2; i++) {
for(j=j1; j<=j2; j++) {
int xx, yy;
xx = (int)floor(x/MAP_CELL_SIZE_M);
yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
-
+
if (valid != NULL) {
*valid = ( ( xx < MAP_WIDTH ) && (yy < MAP_HEIGHT) && ( xx >= 0 ) && ( yy >= 0) );
}
*/
int ShmapCell2Point(int ix, int iy, double *x, double *y)
{
- if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
+ if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
if (x) *x = (ix+0.5)*MAP_CELL_SIZE_M;
}
else return -1;
#define __MAP_H
#include <stdbool.h>
+#include <robodim.h>
/**
* @defgroup maplib Library to manage the map
interprocess comunication methods.
* - Discrete Space (@ref discrete_map): the real space is divided
into cells of variable size. Each cell has its own properties.
- *
+ *
* @section shmap Shared Memory Map
* There are two important methods to access the shared map. Before
* @warning If the program exit without a ShmapFree(), the memory will
* stay in Linux SHM. You can verify it with command "ipcs -m". For
* further details, read shm documentation.
- *
+ *
* @section the_map The map
* @subsection real_map The real space
- *
+ *
* The map size is (#MAP_PLAYGROUND_WIDTH_MM
* x #MAP_PLAYGROUND_HEIGHT_MM). This space discretization is
* explained in section @ref discrete_map .
- *
+ *
* @code
* 0 MAP_PLAYGROUND_WIDTH_MM
* MAP_PLAYGROUND_HEIGHT_MM +---------------------------------------+
* There are only a coordonates difference between points and
* cells. Cell which cordonates [0,0] represents all the points
* contained in a square sited up left corner.
- *
+ *
* @code
* X 0 1 2 3 4 5 6 7 8 MAP_WIDTH-1
* Y +---+---+---+---+---+---+---+---+---+---+
* +---+---+---+---+---+---+---+---+---+---+
* MAP_HEIGHT-1
* @endcode
- *
+ *
* @note Matrix convention is usally (row, column). In this program
* the convention will be (column, row) in order to keep real
* coordonates relation. C language stockes matrix in rows, so a cell
* coordonate (x,y) will be in a C matrix [y][x].
- *
+ *
* @section map_content Map Content
* Each cell/point of the map (::MapCell) contains two types of information:
* - Value (::MapCellValue) : It determins the type of cell. It is the
information used by A* algorithm.
* - Flag (::MapCellFlag) : It is extra information.
- *
+ *
* @subsection cell_values Map Values
- *
+ *
* This information is of the type ::MapCellValue.The value that the
* map contains can be:
* - #MAP_WALL: Wall of the map. A fixed obstacle.
sensors found an obstacle. It can be view as a moving obstacle.
* - #MAP_NEW_OBSTACLE_CSPACE: Configuration Space of the obstacle .
* - #MAP_FREE: Free space.
- *
+ *
* One special type is #MAP_NOT_IN_MAP. It is a error code return when
* we try to acces to a space wich is not in the map. (i.e. the
* request cell/point exceeds map dimensions.)
* @note The configuration space is a special obstacle type that
* depends on the geometry of the robot. For more information, read
* a book about robotics.
- *
+ *
* @subsection cell_flags Cell Flags
- * The possible cell flags are:
- * - #MAP_FLAG_NO_FLAG
- * - #MAP_FLAG_WALL
- * - #MAP_FLAG_PATH
- * - #MAP_FLAG_START
- * - #MAP_FLAG_GOAL
+ * The possible cell flags are:
+ * - #MAP_FLAG_NO_FLAG
+ * - #MAP_FLAG_WALL
+ * - #MAP_FLAG_PATH
+ * - #MAP_FLAG_START
+ * - #MAP_FLAG_GOAL
* One special type is #MAP_FLAG_ERROR. It is a error code return when
* we try to acces to a space wich is not in the map. (i.e. the
* request cell/point exceeds map dimensions.)
* @subsection map_func Accessing to map information
- *
+ *
* FIXME: Obsolete: ShmapGetPointValue(), ShmapSetPointValue(),
* ShmapSetPointValue(), ShmapSetCellValue() do not exist
*
* To read and write cell values, use functions ShmapGetCellValue()
* and ShmapSetCellValue(). If you want to acces directly to points,
* you can also use ShmapGetPointValue() and ShmapSetPointValue().
- *
+ *
* There are similar functions to get and set flags:
* ShmapGetCellFlag() and ShmapSetCellFlag(), ShmapGetPointValue() and
* ShmapSetPointFlag().
- *
+ *
* It can be possible also to edit more than a point at the same time
* with function ShmapSetRectangleType().
- *
+ *
* If you want to know if a cell is free see ShmapIsFreeCell() and
* ShmapIsFreePoint().
- *
+ *
* ShmapUpdateTmpObstacles() is a specific function created for
* Eurobot Project. Please, see an example in testmap.c file
- *
+ *
* @example testmap.c
* Example how to use Shmap library.
*/
/** @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 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_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)
/**@}*/
/**
* @name Cell Flags
* @{
*/
-#define MAP_FLAG_WALL 1 /**< Known wall */
-#define MAP_FLAG_PATH 2
-#define MAP_FLAG_START 4
-#define MAP_FLAG_GOAL 8
-#define MAP_FLAG_DET_OBST 16 /**< Set when an obstacle is detected, cleard on every "map forget cycle" */
-#define MAP_FLAG_SIMULATED_WALL 32 /**< Used by robomon to simulate obstacles */
-#define MAP_FLAG_IGNORE_OBST 64 /**< If obstacle is detected here, ignore it */
-
-/** "Safety margin" around obstacle - used only during A* planning and
- * not during runtime obstacle avoidance. */
-#define MAP_FLAG_PLAN_MARGIN 128
-#define MAP_FLAG_INVALIDATE_WALL 256 /**< Area, where the wall should be forgotten */
+#define MAP_FLAG_WALL 1 /**< Known wall */
+#define MAP_FLAG_PATH 2
+#define MAP_FLAG_START 4
+#define MAP_FLAG_GOAL 8
+#define MAP_FLAG_DET_OBST 16 /**< Set when an obstacle is detected, cleard on every "map forget cycle" */
+#define MAP_FLAG_SIMULATED_WALL 32 /**< Used by robomon to simulate obstacles */
+#define MAP_FLAG_IGNORE_OBST 64 /**< If obstacle is detected here, ignore it */
+#define MAP_FLAG_PLAN_MARGIN 128 /**< Safety margin around obstacle,
+ * do not plan path here,
+ * motion controler is not allowed to enter this area */
/** @}*/
/** @name Shared Memory macros */
/**@{*/
-#define SHM_MAP_KEY 555 /**< Key use to share the memory SHM*/
+#define SHM_MAP_KEY 555 /**< Key use to share the memory SHM*/
/**@}*/
/**@}*/
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
typedef unsigned int map_cell_detobst_t;
typedef unsigned int map_cell_flag_t;
map_cell_detobst_t detected_obstacle; /**< 0 = no obstacle, 255 = new obstacle */
} MapCell;
-#define MAP_NEW_OBSTACLE 255
-#define MAP_NO_OBSTACLE 0
+#define MAP_NEW_OBSTACLE 255
+#define MAP_NO_OBSTACLE 0
struct map {
MapCell cells[MAP_HEIGHT][MAP_WIDTH];
-
+
}; /**< The main structure.*/
/** See ShmapCellAtPoint(). */
extern struct map_cell ShmapNoCell;
-
+
void ShmapAllFreeSpace(void);
struct map *ShmapInit(int init_flag);
void ShmapFree(void);
/**
* Gives information about a cell
- *
+ *
* @param x Coordinate of a cell
* @param y Coordinate of a cell
* @return 1 if cell is in map, 0 otherwise
#ifdef __cplusplus
}
-#endif
+#endif
#endif //__MAP_H_
struct map *map = ShmapIsMapInit();
const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
{-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
-
+
for (yy=0;yy<MAP_HEIGHT;yy++) {
for (xx=0;xx<MAP_WIDTH;xx++) {
// check obstacles around this cell to determine safety zone flag
map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN; // delete margin status
for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
{
- if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y)
- && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST)
+ if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y)
+ && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST)
|| (map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).detected_obstacle > 0 ))
{
// creates safety status
}
-/**
+/**
* Returns the number of points of the path on success or an error code.
* @param xstart_real X coordinate in meters
* @param ystart_real Y coordinate in meters
* It takes parameters of start and goal points. It returns the number
* of points of the trajectory and the caller must specify pointers to
* path and angle.
- *
+ *
* The result of the algorithm is stored in a list of ::PathPoint,
* terminated by NULL, containing the points of the path. This memory
* is allocated dynamicaly and its address will be returned as
* argument. After use the path, memory should be free with
* freePathMemory(). A pointer of the final angle must be also passed
* as argument.
- *
+ *
* The map memory must be init with ShmapInit() before calling
* path_planner(). Do not forget to free this map memory with
* ShmapFree().
PathPoint *original_path;
PathPoint * tmp;
static pthread_mutex_t plan_mutex = PTHREAD_MUTEX_INITIALIZER;
-
+
// Init the map
if (! ShmapIsMapInit()) {
DBG("ERROR: shared map memory is not init.\n");
}
pthread_mutex_lock(&plan_mutex);
-
+
// Call to A* algorithm
nbpoints = aAlgorithm(xstart_real,ystart_real, xgoal_real, ygoal_real, &astar_path);
if (nbpoints < 0) {
tmp--;
tmp->x = xgoal_real;
tmp->y = ygoal_real;
-
-
+
+
// DBG("Simplifing the path\n");
- // Calc a simplest path
+ // Calc a simplest path
(*simple_path) = (PathPoint *) malloc(sizeof(PathPoint));
count = path_simplifier(original_path, nbpoints, *simple_path, angle);
// Do not send the first point
(*simple_path)= (*simple_path)->next;
free(tmp);
free(original_path);
-
+
// All is OK
ret = 1;
out:
-Subproject commit dcb49770b8aaa099920fd387b96203811788b8d7
+Subproject commit bdc269f8d6f25e6dfde0700623f640279fbb1363
* (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},
- {975, 875},
- {1325, 175},
- {1675, 175},
- {2025, 1575},
- {2025, 875},
-};
-
-const struct pawn_pos pawn[PAWN_CNT] = {
- {800, 1400}, // center
- {800, 350},
- {1150, 1750},
- {1150, 350},
- {1500, 1050},
- {1850, 1750},
- {1850, 350},
- {2200, 1400},
- {2200, 350},
- {200, 250}, // left dispensing
- {200, 550},
- {200, 850},
- {2800, 250}, // right dispensing
- {2800, 550},
- {2800, 850},
-};
-
-const struct queen_pos queen[QUEEN_CNT] = {
- {200, 1450},
- {2800, 1450},
-};
-
-const struct king_pos king[KING_CNT] = {
- {200, 1150},
- {2800, 1150},
-};
+// 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},
+// {975, 875},
+// {1325, 175},
+// {1675, 175},
+// {2025, 1575},
+// {2025, 875},
+// };
+//
+// const struct pawn_pos pawn[PAWN_CNT] = {
+// {800, 1400}, // center
+// {800, 350},
+// {1150, 1750},
+// {1150, 350},
+// {1500, 1050},
+// {1850, 1750},
+// {1850, 350},
+// {2200, 1400},
+// {2200, 350},
+// {200, 250}, // left dispensing
+// {200, 550},
+// {200, 850},
+// {2800, 250}, // right dispensing
+// {2800, 550},
+// {2800, 850},
+// };
+//
+// const struct queen_pos queen[QUEEN_CNT] = {
+// {200, 1450},
+// {2800, 1450},
+// };
+//
+// const struct king_pos king[KING_CNT] = {
+// {200, 1150},
+// {2800, 1150},
+// };
/*
/*
- * robodim_eb2010.h
+ * robodim.h
*
* Dimensions for the robot, playground and other stuff.
- * Eurobot 2010
+ * Demo application
*
* Copyright: (c) 2008 - 2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
-#ifndef ROBODIM_EB2010_H
-#define ROBODIM_EB2010_H
+#ifndef ROBODIMH
+#define ROBODIMH
/**
- * FIXME: update robot's dimensions !!!
- * and update the pitcture
- *
- * ROBOT DIMENSIONS
+ * ROBOT DIMENSIONS
*
* ^ +--------------------------+
* | | | | |
* v +--------------------------+
* <-------> <-->
* V WR
- *
- * Sx - Sharp sensor
+ *
*/
-/* FIXME update robot's dimensions!!! */
#define ROBOT_WIDTH_MM 280 /* W*/
#define ROBOT_WIDTH_M (ROBOT_WIDTH_MM/1000.0)
#define ROBOT_ROTATION_RADIUS_MM ((188)/2) /* 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 /* 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 /* 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 200 /* 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 HOKUYO_CENTER_OFFSET_MM 170 //
#define HOKUYO_CENTER_OFFSET_M (HOKUYO_CENTER_OFFSET_MM/1000.0)
+#define HOKUYO_RANGE_ANGLE_LEFT 70 /* view angle in degrees from center axis */
+#define HOKUYO_RANGE_ANGLE_RIGHT 70
#define ODOMETRY_WHEEL_RADIUS_MM 30
#define ODOMETRY_WHEEL_RADIUS_M (ODOMETRY_WHEEL_RADIUS_MM/1000.0)
#define ODOMETRY_ROTATION_RADIUS_MM (246/2)
#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_CRANE_LENGTH_M 0.2 /* V */ /* FIXME: Measure the right value */
+#define ROBOT_DIAGONAL_RADIUS_MM sqrt((ROBOT_WIDTH_MM*ROBOT_WIDTH_MM/4) + (ROBOT_AXIS_TO_FRONT_MM*ROBOT_AXIS_TO_FRONT_MM))
+#define ROBOT_DIAGONAL_RADIUS_M (ROBOT_DIAGONAL_RADIUS_MM/1000.0)
/**
* PLAYGROUND DIMENSIONS
- *
- * S2 R3
- * +---------------------------------+---------------------------------+
- * ^ | | [W,H]|
- * | | SL_T_R | |
- * | |<------------->| |
- * | | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * H | R1| |S1
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * v |[0,0] |
- * +---------------------------------+---------------------------------+
- * S3 R2
- * <----------------------------------------------------------------->
- * W
*/
-#define PLAYGROUND_WIDTH_MM 3000
+
+#define PLAYGROUND_WIDTH_MM 5000
#define PLAYGROUND_WIDTH_M (PLAYGROUND_WIDTH_MM/1000.0)
-#define PLAYGROUND_HEIGHT_MM 2100
+#define PLAYGROUND_HEIGHT_MM 5000
#define PLAYGROUND_HEIGHT_M (PLAYGROUND_HEIGHT_MM/1000.0)
-#define SLOPE_TO_RIM_MM 740
-#define SLOPE_TO_RIM_M (SLOPE_TO_RIM_MM/1000.0)
-#define SLOPE_LENGTH_MM 519.23
-#define SLOPE_LENGTH_M (SLOPE_LENGTH_MM/1000.0)
-
-#define SQUARE_WIDTH_MM 350
-#define SQUARE_HEIGHT_MM 350
-
-#define BLINE_WIDTH_MM 50
-#define BLINE_HEIGHT_MM 2100
-
-#define BLOCK_WIDTH_MM 400
-#define BLOCK_HEIGHT_MM 22
-
-#define STARTAREA_WIDTH_MM 400
-#define STARTAREA_HEIGHT_MM 400
-
-#define DISPENSING_WIDTH_MM 400
-#define DISPENSING_HEIGHT_MM 1678
-
-#define PROTECTEDBORDER_WIDTH_MM 22
-#define PROTECTEDBORDER_HEIGHT_MM 130
-
-#define PROTECTEDBLOCK_WIDTH_MM 700
-#define PROTECTEDBLOCK_HEIGHT_MM 120
-
-#define FIGURE_WIDTH_MM 200
-
-#define CORN_DIAMETER_MM 50
-#define CORN_DIAMETER_M (CORN_DIAMETER_MM/1000.0)
-#define CORN_RADIUS_MM (50/2)
-#define CORN_RADIUS_M (CORN_RADIUS_MM/1000.0)
-
-#define CORN_NEIGHBOURHOOD_RADIUS_MM 220
-#define CORN_NEIGHBOURHOOD_RADIUS_M (CORN_NEIGHBOURHOOD_RADIUS_MM/1000.0)
-/*
- * 3 ultrasonic beacons
- */
-#define BEACON_WIDTH 0.08
-#define BEACON_HEIGHT BEACON_WIDTH
-
-#define BEACON_GREEN 0
-#define BEACON_RED 1
-#define BEACON_CNT 3
-
-/* bonus squares */
-#define BONUS_CNT 6
-
-/* pawns */
-#define PAWN_CNT 15
-
-/* king */
-#define KING_CNT 2
-
-/* queen */
-#define QUEEN_CNT 2
-
-struct beacon_pos {
- float x, y;
-};
-
-struct bonus_pos {
- int x, y;
-};
-
-struct pawn_pos {
- int x, y;
-};
-
-struct king_pos {
- int x, y;
-};
-
-struct queen_pos {
- int 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 pawn_pos pawn[PAWN_CNT];
-extern const struct king_pos king[KING_CNT];
-extern const struct queen_pos queen[QUEEN_CNT];
-
-/*
- * Position of Shapr sensors on the robot with respect to the robot center
- * (not used in EB2009)
+/**
+ * TARGET DIMENSIONS
*/
-struct sharp_pos {
- float x, y, ang;
-};
-#define SHARP_COUNT 0 /* no sharp sensors in EB2009, I hope */
+#define TARGET_RADIUS_MM 100
+#define TARGET_RADIUS_M (TARGET_RADIUS_MM/1000.0)
-#endif /* ROBODIM_EB2008_H */
+#endif /* ROBODIMH */
config_include_HEADERS = robot_config.h
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
-
-bin_PROGRAMS += homologation
-homologation_SOURCES = homologation.cc
+bin_PROGRAMS += demo
+demo_SOURCES = demo.cc common-states.cc sub-states.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
+ 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
orte = ortedata;
}
-// FIXME: obsolete (?)
-void act_hokuyo(unsigned char angle)
-{
- orte->hokuyo_pitch.angle = angle;
- ORTEPublicationSend(orte->publication_hokuyo_pitch);
-}
-
void act_camera_on(void)
{
orte->camera_control.on = 1;
ORTEPublicationSend(orte->publication_camera_control);
}
-static uint16_t vidle_last_request;
+void act_magnet(unsigned char on)
+{
+ orte->magnet_cmd.on = on;
+ ORTEPublicationSend(orte->publication_magnet_cmd);
+}
+
+static int crane_last_request;
-void act_vidle(uint16_t position, char speed)
+void act_crane(int position)
{
- orte->vidle_cmd.req_pos = position;
- orte->vidle_cmd.speed = speed;
+ orte->crane_cmd.req_pos = position;
/* 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);
+ * response in rcv_crane_status_cb() */
+ crane_last_request = position;
+ ORTEPublicationSend(orte->publication_crane_cmd);
}
-uint16_t act_vidle_get_last_reqest(void)
+int act_crane_get_last_reqest(void)
{
- return vidle_last_request;
+ return crane_last_request;
}
/**
\defgroup actlib Actuators control library
-Actuators library serves as a single control point for all the actuators
+Actuators library serves as a single control point for all the actuators
of the robot.
*/
#ifndef ACTUATORS_H
#define ACTUATORS_H
-/* Hokuyo pitch angle limits */ // FIXME: obsolete: delete or update
-#define HOKUYO_PITCH_MAX 0xF8
-#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
-
-#define VIDLE_FAST_SPEED 0x00
-#define VIDLE_MEDIUM_SPEED 0x0a
+#define CRANE_UP 0x90
+#define CRANE_DOWN 0x190
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
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_crane(int position);
+void act_magnet(unsigned char on);
+int act_crane_get_last_reqest(void);
#ifdef __cplusplus
}
-#endif
+#endif
#endif /* ACTUATORS_H */
#define FSM_MAIN
-#include "robodata.h"
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
-#include "actuators.h"
#include <trgen.h>
-#include "match-timing.h"
-#include "eb2010misc.h"
#include <stdbool.h>
#include <ul_log.h>
+#include <shape_detect.h>
-UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
-
+#include "robodata.h"
+#include "actuators.h"
+#include "match-timing.h"
#include "common-states.h"
+#include "sub-states.h"
+
+UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
/************************************************************************
* Functions used in and called from all the (almost identical)
fsm->debug_name, robot_current_time(), \
name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+/************************************************************************
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
+ ************************************************************************/
+
+struct TrajectoryConstraints tcSlow, tcVerySlow;
-static void set_initial_position()
-{
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
-}
+/**
+ * Vector where all absolute positions of all detected targets are stored.
+ */
+std::vector<robot_pos_type> detected_target;
+
+/**
+ * Safe distance for target recognition
+ */
+const double approach_radius = TARGET_RADIUS_M + 3.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
-static void actuators_home()
+void set_initial_position()
{
- static int tmp = 0;
- act_vidle(VIDLE_UP - tmp, VIDLE_FAST_SPEED);
- tmp = 1 - tmp; // Force movement (we need to change the target position)
+ robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
}
-void start_entry()
+void actuators_home()
{
- pthread_t thid;
- robot.check_turn_safety = false;
- pthread_create(&thid, NULL, timing_thread, NULL);
- start_timer();
- act_camera_on();
+ act_crane(CRANE_UP);
+ act_magnet(0);
}
-// 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()
+/* Check if the new point is within the playground scene */
+bool goal_is_in_playground(double goalx, double goaly)
{
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
+ if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
+ return false;
+ else
+ return true;
}
-void start_go()
+/* Check if the new point is close to the robot */
+bool close_goal(double goalx, double goaly)
{
- sem_post(&robot.start);
- actuators_home();
- set_initial_position();
+ const double close = 0.5;
+ double x, y, phi;
+ robot_get_est_pos(&x, &y, &phi);
+
+ if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
+ return true;
+ else
+ return false;
}
-void start_exit()
+/**
+ * Take data from hokuyo and run shape detection on it.
+ *
+ * Absolute positions of all detected targets centers are stored in alobal variable (vector).
+ *
+ * @return True if at least one target detected, else false.
+ */
+static bool detect_target()
{
- robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
- act_camera_off();
-}
+ struct hokuyo_scan_type hokuyo = robot.hokuyo;
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
+ Shape_detect sd;
+ std::vector<Shape_detect::Arc> arcs;
+ sd.prepare(hokuyo.data);
+ sd.arc_detect(arcs);
-struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
+ // clear old targets
+ detected_target.clear();
-#define VIDLE_TIMEOUT 2000
+ if (arcs.size() > 0) {
+ robot_pos_type e, target, hok;
-/************************************************************************
- * States that form the "collect some oranges" subautomaton. Calling automaton
- * SHOULD ALWAYS call the "approach_the_slope" state.
- ************************************************************************/
+ robot_get_est_pos(&e.x, &e.y, &e.phi);
-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;
-}
+ double sinus = sin(e.phi);
+ double cosinus = cos(e.phi);
-static struct slope_approach_style *slope_approach_style_p;
+ // save absolute positions of all detected targets
+ for (int i = 0; i < arcs.size(); i++) {
+ Shape_detect::Arc *a = &arcs[i];
-/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
-FSM_STATE(approach_the_slope)
-{
- 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_MOTION_DONE:
- FSM_TRANSITION(climb_the_slope);
- 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:
- break;
- }
-}
+ hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
+ hok.y = (double)a->center.y / 1000.0;
-void inline enable_switches(bool enabled)
-{
- robot.use_left_switch = enabled;
- robot.use_right_switch = enabled;
- robot.use_back_switch = enabled;
-}
+ /* transform target position which is relative to Hokuyo
+ center to absolute position in space */
+ target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
+ target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
-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);
- break;
- case EV_RETURN:
- FSM_TRANSITION(sledge_down);
- break;
- case EV_TIMER:
- act_vidle(VIDLE_LOAD_PREPARE, 15);
- break;
- case EV_START:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
+ // filter those targets not in playground range
+ if (goal_is_in_playground(target.x, target.y))
+ detected_target.push_back(target);
+ }
+ }
+ return detected_target.size();
}
-/* subautomaton to load oranges in two stages */
-FSM_STATE_DECL(load_oranges2);
-FSM_STATE_DECL(load_oranges3);
-FSM_STATE(load_oranges)
+/**
+ * Calculates point to approach the target.
+ *
+ * @param target Position of the center of the target.
+ * @param approach Pointer to the the intersection point of circle around
+ * the target and line between robot center and target.
+ */
+void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
{
- 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);
- 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:
- break;
- }
-}
+ double xrobot, yrobot, phi;
+ double delta;
-FSM_STATE(load_oranges2)
-{
- 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);
- 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);
- break;
- }
-}
+ robot_get_est_pos(&xrobot, &yrobot, &phi);
-FSM_STATE(load_oranges3)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- act_vidle(VIDLE_MIDDLE+50, 0);
- FSM_TIMER(500);
- break;
- case EV_VIDLE_DONE:
- SUBFSM_RET(NULL);
- 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);
- break;
- }
-}
-
-FSM_STATE(sledge_down)
-{
- struct TrajectoryConstraints tc;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- tc = tcFast;
- tc.maxe = 0.5;
- robot_trajectory_new(&tc);
-
- 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);
- 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,
- 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);
- 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,
- 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);
- 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;
-
- break;
- }
-}
+ delta = distance(xrobot, yrobot, xtarget, ytarget);
-/************************************************************************
- * The "unload our oranges" subautomaton
- ************************************************************************/
+ *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
+ *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
-FSM_STATE(to_cntainer_and_unld)
-{
- 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);
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(1000); // FIXME: test this
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
- break;
- case EV_TIMER:
- FSM_TRANSITION(jerk);
- 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:
- break;
- }
+ *phi_approach = get_approach_angle(xtarget, ytarget);
}
-FSM_STATE(jerk)
+/**
+ * Calculates point to approach the target.
+ *
+ * @param target Position of the center of the target.
+ * @return Angle to approach the target form.
+ */
+double get_approach_angle(double xtarget, double ytarget)
{
- static char move_cnt;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- move_cnt = 0;
- robot_move_by(-0.05, NO_TURN(), &tcJerk);
- 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++;
- 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:
- break;
- }
-}
+ double xrobot, yrobot,phi;
-/************************************************************************
- * The "collect corns" subautomaton
- ************************************************************************/
+ robot_get_est_pos(&xrobot, &yrobot, &phi);
-static enum where_to_go {
- CORN,
- TURN_AROUND,
- CONTAINER,
- NO_MORE_CORN
-} where_to_go = CORN;
+ return atan2((ytarget - yrobot), (xtarget - xrobot));
+}
-static struct corn *corn_to_get;
-FSM_STATE(rush_corns_decider)
+/**
+ * FSM state for neighborhood observation.
+ *
+ * Detect targets using shape_detect.
+ * If no target detected, turn 120deg and try again.
+ * Scan all 360deg and then go back to move_around state.
+ *
+ * If target detected, go to approach_target state.
+ */
+FSM_STATE(survey)
{
- 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 */ {
- }
- 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:
- break;
- }
+ static char turn_cntr = 0;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("survey");
+#if 1 // FIXME just for test
+ if (detect_target()) {
+#else
+ if (turn_cntr > 1) {
+ robot_pos_type target;
+ detected_target.clear();
+ for (double i = 1; i < 5; i++) {
+ target.x = i;
+ target.y = i/2.0;
+ detected_target.push_back(target);
+ }
+#endif
+ // target detected, go to the target
+ FSM_TRANSITION(approach_target);
+ DBG_PRINT_EVENT("Target detected!");
+ } else {
+ // no target detected in this heading, turn 120°
+ robot_get_est_pos(&x, &y, &phi);
+ robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
+ turn_cntr++;
+ DBG_PRINT_EVENT("no target");
+ }
+ break;
+ case EV_TIMER:
+ if (turn_cntr > 2) {
+ FSM_TRANSITION(move_around);
+ } else {
+ FSM_TRANSITION(survey);
+ }
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_ERROR:
+ FSM_TRANSITION(move_around);
+ break;
+ case EV_EXIT:
+ turn_cntr = 0;
+ break;
+ }
}
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
+/**
+ * FSM state for approaching all detected targets.
+ *
+ * Try to approach target.
+ * If approach OK - go to subautomaton and do target recognition, touch and load.
+ * On subautomaton return check if target loaded/valid.
+ *
+ * If target loaded, go home.
+ * If target not valid, try next target if any.
+ * If approach not succesfull - go to move_around state.
+ */
+FSM_STATE(approach_target)
{
- 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_MOTION_DONE:
- cnt++;
- 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:
- break;
- }
+ static int target_cntr = 0;
+ int max_target = detected_target.size();
+ double x_target, y_tatget;
+ double x_approach, y_approach, phi_approach;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("approaching target");
+ x_target = detected_target[target_cntr].x;
+ y_tatget = detected_target[target_cntr].y;
+ target_cntr++;
+
+ printf("target %d / %d\n", target_cntr, max_target);
+
+ get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
+ robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.1), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ DBG_PRINT_EVENT("target approached");
+ SUBFSM_TRANSITION(recognize, NULL);
+ break;
+ case EV_RETURN:
+ if (robot.target_loaded) {
+ FSM_TRANSITION(go_home);
+ } else if (robot.target_valid) {
+ //FIXME target is valid but not loaded - try another approach direction
+
+ } else if (!robot.target_valid && (target_cntr < max_target)) {
+ // go for next target if any
+ FSM_TRANSITION(approach_target);
+ } else {
+ // go to new point and survey
+ FSM_TRANSITION(move_around);
+ }
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("can not approach target");
+ if (target_cntr < max_target) {
+ FSM_TRANSITION(approach_target);
+ } else {
+ FSM_TRANSITION(move_around);
+ }
+ break;
+ case EV_EXIT:
+ target_cntr = 0;
+ break;
+ }
}
-FSM_STATE(rush_the_corn)
+FSM_STATE(move_around)
{
- 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;
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(rush_last_cms);
- 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:
- break;
- }
+ double goalx, goaly;
+
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ do {
+ goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
+ goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
+ } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
+
+ robot_goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
+ DBG_PRINT_EVENT("new survey point");
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("can not access survey point");
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(survey);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_EXIT:
+ break;
+ }
}
-FSM_STATE(rush_last_cms)
+FSM_STATE(go_home)
{
- 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);
- 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++;
- 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;
- break;
- }
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("homing");
+ robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("ERROR: home position is not reachable!");
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_home);
+ break;
+ case EV_MOTION_DONE:
+ case EV_EXIT:
+ DBG_PRINT_EVENT("Mission completed!");
+ robot_exit();
+ break;
+ }
}
-// used to perform the maneuvre
-FSM_STATE(turn_around)
+/*
+FSM_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));
- 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:
- break;
- }
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ 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);
-
-void start_entry();
-void start_timer();
-void start_go();
-void start_exit();
+#define HOME_POS_X_M (PLAYGROUND_WIDTH_M / 2.0)
+#define HOME_POS_Y_M (PLAYGROUND_HEIGHT_M / 2.0)
+#define HOME_POS_ANG_DEG 90
+
+extern struct TrajectoryConstraints tcSlow, tcVerySlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+
+FSM_STATE_DECL(survey);
+FSM_STATE_DECL(approach_target);
+FSM_STATE_DECL(move_around);
+FSM_STATE_DECL(go_home);
+
+void set_initial_position(void);
+void actuators_home(void);
+bool goal_is_in_playground(double goalx, double goaly);
+bool close_goal(double goalx, double goaly);
+void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach);
+double get_approach_angle(double xtarget, double ytarget);
#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 "corns_configs.h"
-#include "actuators.h"
-#include "match-timing.h"
-#include "eb2010misc.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_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.transition_callback = trans_callback;
- //robot.fsm.motion.transition_callback = move_trans_callback;
-
- tcJerk = trajectoryConstraintsDefault;
- tcJerk.maxv = 1.5;
- tcJerk.maxacc = 5;
- tcJerk.maxomega = 2;
- tcFast = trajectoryConstraintsDefault;
- tcFast.maxv = 1;
- tcFast.maxacc = 0.3;
- tcFast.maxomega = 2;
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.3;
- tcSlow.maxacc = 0.1;
- tcVerySlow = trajectoryConstraintsDefault;
- tcVerySlow.maxv = 0.05;
- tcVerySlow.maxacc = 0.05;
-
- 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
-/**
- * @file corns_configs.h
- * @author Filip Jares
- *
- * @brief Corns Library
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "corns_configs.h"
-#include "robodim.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_corns_configs); /* Log domain name = ulogd + name of the file */
-
-#define STRING_LENGTH 64
-
-#define POSITION_DIMENSION 2
-/** CORNS GRID CONSTANTS */
-#define FIRST_CORN_X_OFFSET_M 0.15
-#define FIRST_CORN_Y_OFFSET_M 0.128
-#define CORNS_X_SPACING_M 0.45
-#define CORNS_Y_SPACING_M 0.25
-
-/* *********** DEFINITION OF CORNS POSITIONS IN PARTICULAR CONFIGURATIONS ************ */
-
-/**
- "System of coordinates:"
-
-@code
- Legend:
- b - blue starting area
- . - place for the corn
- o - place with the corn
- |
- ------------------------------------------------------------------------------------
- | | | here are the slopes
- | b | | and the plateau
- | | |
- |--------- ----------------------
- |
- | |
- | :
- | |
- | :
- 5 ->| . |
- | :
- 4 ->| . |
- | :
- 3 ->| . .
- |
- 2 ->| . .
- |
- 1 ->| . .
- |
- 0 ->| . .
- |
- ------------------------------------------------------------------------------------
- ^ ^ ^ ^
- | | | |
- 0 1 2 3
-@endcode
-*/
-
-const int all_corns[NUM_OF_ALL_CORNS][POSITION_DIMENSION] = {
- // first row
- {1, 0}, {3, 0}, {5, 0},
- // second row
- {0, 1}, {2, 1}, {4, 1}, {6, 1},
- // third row
- {1, 2}, {3, 2}, {5, 2},
- // fourth row
- {0, 3}, {2, 3}, {4, 3}, {6, 3},
- // fifth row
- {1, 4}, {5, 4},
- // sixth row
- {0, 5}, {6, 5}
-};
-
-const int left_side_corns_configurations[][2][POSITION_DIMENSION] = {
-
- /** @code lot no. 1
- 5 ->| .
- |
- 4 ->| .
- |
- 3 ->| o .
- |
- 2 ->| o
- |
- 1 ->| .
- @endcode */
- {
- {0, 3},
- {1, 2}
- },
-
- /** @code lot no. 2
- 5 ->| o
- |
- 4 ->| .
- |
- 3 ->| . .
- |
- 2 ->| o
- |
- 1 ->| .
- @endcode */
- {
- {0, 5},
- {1, 2}
- },
-
- /** @code lot no. 3
- 5 ->| .
- |
- 4 ->| .
- |
- 3 ->| . .
- |
- 2 ->| o
- |
- 1 ->| o
- @endcode */
- {
- {0, 1},
- {1, 2}
- },
-
- /** @code lot no. 4
- 5 ->| .
- |
- 4 ->| o
- |
- 3 ->| . .
- |
- 2 ->| .
- |
- 1 ->| o
- @endcode */
- {
- {0, 1},
- {1, 4}
- },
-
- /** @code lot no. 5
- 5 ->| o
- |
- 4 ->| o
- |
- 3 ->| . .
- |
- 2 ->| .
- |
- 1 ->| .
- @endcode */
- {
- {0, 5},
- {1, 4}
- },
-
- /** @code lot no. 6
- 5 ->| .
- |
- 4 ->| o
- |
- 3 ->| o .
- |
- 2 ->| .
- |
- 1 ->| .
- @endcode */
- {
- {0, 3},
- {1, 4}
- },
-
- /** @code lot no. 7
- 5 ->| .
- |
- 4 ->| .
- |
- 3 ->| o o
- |
- 2 ->| .
- |
- 1 ->| .
- @endcode */
- {
- {0, 3},
- {2, 3}
- },
-
- /** @code lot no. 8
- 5 ->| o
- |
- 4 ->| .
- |
- 3 ->| . o
- |
- 2 ->| .
- |
- 1 ->| .
- @endcode */
- {
- {0, 5},
- {2, 3}
- },
-
- /** @code lot no. 9
- 5 ->| .
- |
- 4 ->| .
- |
- 3 ->| . o
- |
- 2 ->| .
- |
- 1 ->| o
- @endcode */
- {
- {0, 1},
- {2, 3}
- }
-};
-
-
-const int center_corns_configurations[][2][POSITION_DIMENSION] = {
-
- /** @code lot no. 1
- 2 ->| . o
- |
- 1 ->| . .
- |
- 0 ->| o .
- |
- ------------------------------------------------------------------------------------
- @endcode */
- {
- {1, 0},
- {3, 2}
- },
-
- /** @code lot no. 2
- 2 ->| . o
- |
- 1 ->| . o
- |
- 0 ->| . .
- |
- ------------------------------------------------------------------------------------
- @endcode */
- {
- {2, 1},
- {3, 2}
- },
-
- /** @code lot no. 3
- 2 ->| . .
- |
- 1 ->| . .
- |
- 0 ->| o o
- |
- ------------------------------------------------------------------------------------
- @endcode */
- {
- {1, 0},
- {3, 0}
- },
-
- /** @code lot no. 4
- 2 ->| . .
- |
- 1 ->| . o
- |
- 0 ->| . o
- |
- ------------------------------------------------------------------------------------
- @endcode */
- {
- {2, 1},
- {3, 0}
- },
-};
-
-/* ************************** TO STRING and PRINT functions (for testing purposes) *** */
-
-char * position_indexes_to_string(const int position[POSITION_DIMENSION])
-{
- char * to_return = (char *) malloc(STRING_LENGTH*sizeof(char));
- sprintf(&to_return[0], "[%d, %d]", position[0], position[1]);
- return &to_return[0];
-}
-
-char * position_to_string(struct position * position)
-{
- char * to_return = (char *) malloc(STRING_LENGTH*sizeof(char));
- sprintf(&to_return[0], "[%0.3f, %0.3f]", position->x, position->y);
- return &to_return[0];
-}
-
-void print_positions_indexes(int **positions)
-{
- int i;
- for(i = 0; i < NUM_OF_FAKE_CORNS; i++) {
- ul_loginf("corn no. %i coords: %s\n",
- i,
- position_indexes_to_string(positions[i]));
- }
-}
-
-void print_corn_position(struct corn * corn)
-{
- ul_loginf("corn: %s is %s, %s collected\n", position_to_string(&corn->position),
- (corn->is_fake?"fake":"real"),
- (corn->was_collected?" was":"was not"));
-}
-
-void print_corns_positions(struct corns_group * corns_group)
-{
- struct corn * corn;
- int i;
-
- for(corn = corns_group->corns, i = 1; corn < &corns_group->corns[corns_group->corns_count]; corn++, i++) {
- ul_loginf("corn no. %2i coords: %s, is %s, %s collected\n",
- i,
- position_to_string(&corn->position),
- (corn->is_fake?"fake":"real"),
- (corn->was_collected?" was":"was not"));
- }
-}
-
-/* ************************** COPYING, MIRRORING & TRANSFORM functions ************** */
-
-/** FIXME prepoklada pole o delce 2 */
-void copy_corn_indexes(const int from_position[POSITION_DIMENSION], int *to_position)
-{
- int i;
- for(i = 0; i < POSITION_DIMENSION; i++) {
- to_position[i] = from_position[i];
- }
-}
-
-void copy_corns_indexes(const int from_array[][POSITION_DIMENSION], int** to_array,
- int from_start_index, int to_start_index, int count)
-{
- int n, n2;
- for(n=from_start_index, n2=to_start_index; n < (from_start_index + count); n++, n2++) {
- copy_corn_indexes(from_array[n], to_array[n2]);
- }
-}
-
-void mirror_corn_position(int position[POSITION_DIMENSION])
-{
- position[0] = NUM_OF_FAKE_CORNS - 1 - position[0];
-}
-
-void transform_corn_indexes_into_position(const int * position_indexes, struct position * position)
-{
- position->x = FIRST_CORN_X_OFFSET_M + CORNS_X_SPACING_M * position_indexes[0];
- position->y = FIRST_CORN_Y_OFFSET_M + CORNS_Y_SPACING_M * position_indexes[1];
-}
-
-void copy_position_values(struct position * source_pos, struct position * dest_pos)
-{
- dest_pos->x = source_pos->x;
- dest_pos->y = source_pos->y;
-}
-
-/* ************************** COMPARISON functions *********************************** */
-
-bool positions_are_equal(struct position * pos1, struct position * pos2)
-{
- if (pos1->x == pos2->x && pos1->y == pos2->y)
- return true;
- else
- return false;
-}
-
-bool contains_group_corn_with_position(struct corns_group * group, struct position * position)
-{
- struct corn * corn;
- for (corn = group->corns; corn < &group->corns[group->corns_count]; corn++) {
- if (positions_are_equal(&corn->position, position))
- return true;
- }
- return false;
-}
-
-/* ************************** CREATE & DISPOSE functions ************************** */
-
-/* create dynamic array of [NUM_OF_FAKE_CORNS][POSITION_DIMENSION] and return pointer to it */
-int ** create_positions()
-{
- int **positions = (int **) malloc(NUM_OF_FAKE_CORNS * sizeof(int *));
- positions[0] = (int *) calloc(NUM_OF_FAKE_CORNS * POSITION_DIMENSION, sizeof(int));
- int i;
- for(i = 1; i < NUM_OF_FAKE_CORNS; i++) {
- positions[i] = positions[0] + i*POSITION_DIMENSION;
- }
- return positions;
-}
-
-void dispose_corns_positions_indexes(int **positions)
-{
- free(positions[0]);
- free(positions);
-}
-
-void dispose_corns_group(struct corns_group * group)
-{
- free(group->corns);
- free(group);
-}
-
-/* ************************** GETTER functions (return indexs or struct corns_group)** */
-
-/* create dynamic array of corn positions for given corns configurations */
-int ** get_fake_corns_positions_indexes_for_configurations(int left_right_conf, int center_conf)
-{
- // each configuration has seven corns
- int **positions = create_positions();
-
- // two corns on the left side into to positions[0..1]
- copy_corns_indexes(left_side_corns_configurations[left_right_conf], positions, 0, 0, 2);
- // mirrored positions of the corns on the right side into positions[2..3]
- copy_corns_indexes(left_side_corns_configurations[left_right_conf], positions, 0, 2, 2);
- mirror_corn_position(positions[2]); // mirror "horizontally"
- mirror_corn_position(positions[3]);
- // two central corns' positions into positions[4..5]
- copy_corns_indexes(center_corns_configurations[center_conf], positions, 0, 4, 2);
- // position of the third central corn into positions[6] ...
- copy_corn_indexes(center_corns_configurations[center_conf][0], positions[6]);
- mirror_corn_position(positions[6]); // .. it is mirrored
-
- return positions;
-}
-
-struct corns_group * get_fake_corns(int left_right_conf, int center_conf)
-{
- // check parameters:
- if (left_right_conf < 0 || left_right_conf >= 9 || center_conf < 0 || center_conf >= 4) {
- perror("get_fake_corns(): illegal arugment");
- exit(EXIT_FAILURE);
- }
-
- // get fake corns' indexes
- int ** positions_indexes = get_fake_corns_positions_indexes_for_configurations(left_right_conf, center_conf);
-
- // prepare structure
- struct corns_group * group = malloc(sizeof(struct corns_group));
-
- // prepare its contents
- group->corns = malloc(NUM_OF_FAKE_CORNS * sizeof(struct corn));
- group->corns_count = NUM_OF_FAKE_CORNS;
-
- // fill its contents
- struct corn * corn; int i;
- for(corn = group->corns, i = 0; corn < &group->corns[NUM_OF_FAKE_CORNS]; corn++, i++) {
- corn->is_fake = true;
- corn->was_collected = false;
- transform_corn_indexes_into_position(positions_indexes[i], &corn->position);
- }
-
- // get rid of what's not needed any more
- dispose_corns_positions_indexes(positions_indexes);
-
- return group;
-}
-
-struct corns_group * get_all_corns(int left_right_conf, int center_conf)
-{
- // check parameters:
- if (left_right_conf < 0 || left_right_conf >= 9 || center_conf < 0 || center_conf >= 4) {
- perror("get_all_corns_(): illegal arugment");
- exit(EXIT_FAILURE);
- }
-
- // prepare result structure
- struct corns_group * group = malloc(sizeof(struct corns_group));
-
- // prepare its contents
- group->corns = malloc(NUM_OF_ALL_CORNS * sizeof(struct corn));
- group->corns_count = NUM_OF_ALL_CORNS;
-
- // copy fake corns into newly created structure of all corns
- struct corns_group * fake_group = get_fake_corns(left_right_conf, center_conf);
- memcpy(group->corns, fake_group->corns, NUM_OF_FAKE_CORNS * sizeof(struct corn));
-
- // fill in the rest of the corns
- struct corn * corn;
- for(corn = &group->corns[NUM_OF_FAKE_CORNS]; corn < &group->corns[NUM_OF_ALL_CORNS]; corn++) {
- int i;
- struct position some_corn_position;
- for(i = 0; i < NUM_OF_ALL_CORNS; i++) { // TODO: could optimize this loop
- transform_corn_indexes_into_position(all_corns[i], &some_corn_position);
- if (!contains_group_corn_with_position(fake_group, &some_corn_position)
- && !contains_group_corn_with_position(group, &some_corn_position)) {
- corn->is_fake = false;
- corn->was_collected = false;
- copy_position_values(&some_corn_position, &corn->position);
- break;
- }
- }
- }
-
- return group;
-}
-
-/*
-int main() // for testing only
-{
- int j, k;
- for(j=0; j<9; j++) {
- for(k=0; k<4; k++) {
- puts("---");
- struct corns_group * corns = get_all_corns(j,k);//get_fake_corns(j, k);
- ul_logdeb("corns configuration no. %d, %d:\n", j, k);
- print_corns_positions(corns);
- dispose_corns_group(corns);
- }
- }
-
- return 0;
-}
-*/
+++ /dev/null
-/**
- * @file corns_configs.h
- * @author Filip Jares
- *
- * @brief Corns Library interface
- */
-#ifndef __CORNS_CONFIGS_H
-#define __CORNS_CONFIGS_H
-
-#include <stdbool.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define NUM_OF_FAKE_CORNS 7
-#define NUM_OF_ALL_CORNS 18
-
-struct position {
- float x, y; /* In meters */
-};
-
-struct corn {
- struct position position;
- bool is_fake;
- bool was_collected;
-};
-
-struct corns_group {
- struct corn * corns;
- unsigned char corns_count;
-};
-
-/**
- * Returns structure containing fake corns in game configuration given by the parameters.
- *
- * @param left_right_conf configuration of center corns. Has to be 0 - 8.
- * @param center_conf configuration of side corns. Has to be 0 - 3.
- */
-struct corns_group * get_fake_corns(int left_right_conf, int center_conf);
-/**
- * Returns structure containing all corns in game configuration given by the parameters.
- *
- * @param left_right_conf configuration of center corns. Has to be 0 - 8.
- * @param center_conf configuration of side corns. Has to be 0 - 3.
- */
-struct corns_group * get_all_corns(int left_right_conf, int center_conf);
-/** Disposes corns_group properly */
-void dispose_corns_group(struct corns_group * group);
-/** Print all description of all corns in the corns_group structure pointed to by given argument */
-void print_corns_positions(struct corns_group * corns_group);
-/** Print corns int corns_group structure pointed to by given argument */
-void print_corn_position(struct corn * corn);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // __CORNS_CONFIGS_H
-
--- /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 <trgen.h>
+#include <ul_log.h>
+#include <path_planner.h>
+#include <can_msg_def.h>
+
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
+
+UL_LOG_CUST(ulogd_demo); /* Log domain name = ulogd + name of the file */
+
+/* initial and starting states */
+FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_start);
+
+/* mission competed */
+FSM_STATE_DECL(go_home);
+
+FSM_STATE(init)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.2;
+
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.05;
+ tcVerySlow.maxacc = 0.05;
+ tcVerySlow.maxomega = 0.5;
+
+ FSM_TRANSITION(wait_for_start);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(wait_for_start)
+{
+ pthread_t thid;
+ ul_logdeb("WAIT_FOR_START mode set\n");
+ ul_logdeb("DEMO mode set\n");
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ pthread_create(&thid, NULL, timing_thread, NULL);
+ set_initial_position();
+#ifdef PPC
+ FSM_TIMER(2000);
+#else
+ FSM_SIGNAL(MAIN, EV_START, NULL);
+#endif
+ break;
+ case EV_START:
+ /* start competition timer */
+ sem_post(&robot.start);
+ FSM_TRANSITION(survey);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ if (robot.start_state == START_PLUGGED_IN)
+ actuators_home();
+ break;
+ case EV_RETURN:
+ case EV_MOTION_ERROR:
+ case EV_MOTION_DONE:
+ case EV_CRANE_DONE:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
+int main()
+{
+ int rv;
+
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ srand(time(0));
+
+ 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 <map.h>
-#include <trgen.h>
-#include "robot.h"
-#include "corns_configs.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_eb2010misc); /* Log domain name = ulogd + name of the file */
-
-#include "eb2010misc.h"
-
-/************************************************************************
- * Functions to manipulate map, compute distances, choose corns etc.
- ************************************************************************/
-double x_coord(double x, enum which_side which_side)
-{
- switch(which_side) {
- case MINE:
- return x;
- break;
- case OPPONENTS:
- return PLAYGROUND_WIDTH_M - x;
- break;
- default:
- goto error;
- }
-error:
- ul_logfatal("ERROR enum which_side definition changed!!\n");
- robot_exit();
- return 0; // avoid warning
-}
-
-// returns pointer to next real (non-fake) corn which was not yet collected
-// TODO: note: use this for "short_time_to_end: situation only, otherwise
-// it is better to try to rush more corns at once
-struct corn * choose_next_corn()
-{
- Point cornPosition;
-
- double maxDistance = 0; // 2 * PLAYGROUND_HEIGHT_M; // "infinity"
- struct corn *minCorn = NULL, *corn;
- // robot.corns->corns[NUM_OF_FAKE_CORNS] is first non-fake corn in the robot.corns structure
- for(corn = &robot.corns->corns[NUM_OF_FAKE_CORNS]; corn < &robot.corns->corns[robot.corns->corns_count]; corn++) {
- cornPosition.x = corn->position.x;
- cornPosition.y = corn->position.y;
- double distance = cornPosition.distanceTo(containerPosition);
- ul_logmsg("maxDistance = %.3f; new corn dist = %.3f, it x position = %.3f; %s\n",
- maxDistance, distance, corn->position.x, (corn->was_collected?"W":"N"));
-
- if (distance > maxDistance && corn->position.x > 2.2 && corn->was_collected == false) {
- maxDistance = distance;
- minCorn = corn;
- ul_logmsg("\tchoose it\n");
- }
- }
-
- if (minCorn) ul_logmsg("\tmin distance was: %.3f ", maxDistance);
- return minCorn;
-}
-
-/**
- * Computes and returns line to point distance
- * @param[in] p the point coords
- * @param[in] lp1 coords of one of the points on the line
- * @param[in] lp2 coords of the second point on the line
- */
-double get_point_to_line_distance(const Point &p, const Point &lp1, const Point &lp2)
-{
- double distance = fabs((lp2.x - lp1.x)*(lp1.y-p.y) - (lp1.x - p.x)*(lp2.y - lp1.y))
- / sqrt((lp2.x - lp1.x)*(lp2.x - lp1.x) + (lp2.y - lp1.y)*(lp2.y - lp1.y));
- return distance;
-}
-
-Pos * get_corn_approach_position(struct corn *corn)
-{
- const Point approxContainerPosition(PLAYGROUND_WIDTH_M - 0.15, 0.02); // blue container Position
- Pos *p = new Pos; // robot position result
-
- Point cornPosition(corn->position.x, corn->position.y);
- double a = approxContainerPosition.angleTo(cornPosition);
-
- p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
- p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
- p->phi = a + M_PI;
-
- return p;
-}
-
-void remove_wall_around_corn(const double x, const double y)
-{
- ShmapSetCircleFlag(x, y, CORN_NEIGHBOURHOOD_RADIUS_M, 0, MAP_FLAG_WALL);
-}
-
-
+++ /dev/null
-#ifndef EB2010_MISC_H
-#define EB2010_MISC_H
-
-#include <trgen.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-enum which_side {
- MINE,
- OPPONENTS
-};
-
-enum which_set_of_oranges {
- NEAR_PLAYGROUND_BOUNDARY,
- NEAR_PLAYGROUND_CENTER
-};
-
-struct slope_approach_style {
- enum which_side which_side;
- enum which_set_of_oranges which_oranges;
- slope_approach_style(enum which_side _side, enum which_set_of_oranges _which_oranges):
- which_side(_side), which_oranges(_which_oranges) {}
-};
-
-static const Point containerPosition(PLAYGROUND_WIDTH_M - 0.25, 0.02); // blue container Position
-
-double x_coord(double x, enum which_side);
-
-struct corn * choose_next_corn();
-
-double get_point_to_line_distance(const Point &p, const Point &lp1, const Point &lp2);
-
-Pos * get_corn_approach_position(struct corn *corn);
-
-void remove_wall_around_corn(const double x, const double y);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* EB2010_MISC_H */
-
TARGET_ERROR // Fatal errror during planning
};
-// we may need something similar in future
-void invalidate(Point point)
-{
- //ShmapSetRectangleFlag(x-s, y-s, x+s, y+s, MAP_FLAG_INVALIDATE_WALL, 0)
-}
-
static double free_space_in_front_of_robot()
{
int i, i1, i2;
- int min = 10000, m;
+ int min = 4000, m;
i1 = HOKUYO_DEG_TO_INDEX(-45);
i2 = HOKUYO_DEG_TO_INDEX(+45);
for (i=MIN(i1, i2); i < MAX(i1, i2); i++) {
if (m > 19 && m < min)
min = m;
}
- return min/1000.0 - HOKUYO_CENTER_OFFSET_M;;
+ return min/1000.0 - (ROBOT_AXIS_TO_FRONT_M - HOKUYO_CENTER_OFFSET_M);
}
static bool obstackle_in_front_if_turn(Trajectory *t)
*/
static enum target_status new_goal(struct move_target *move_target, double start_in_future)
{
- enum target_status ret;
+ enum target_status ret;
double angle;
PathPoint * path = NULL;
PathPoint * tmp_point = NULL;
// Where to start trajectory planning?
get_future_pos(start_in_future, future_traj_point, time_ts);
Point start(future_traj_point.x, future_traj_point.y);
-
+
Trajectory *t = new Trajectory(move_target->tc, backward);
- /*
- // Clear all invalidate flags;
- ShmapSetRectangleFlag(0, 0, PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M, 0, MAP_FLAG_INVALIDATE_WALL);
- // Set invalidate flags if we are going to walled area
- if (false) // we may need this in future
- invalidate(start);
- */
-
if (move_target->heading.operation == TOP_ARRIVE_FROM) {
get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
&target.x, &target.y);
ret = TARGET_ERROR; break;
case PP_ERROR_GOAL_IS_OBSTACLE:
case PP_ERROR_GOAL_NOT_REACHABLE:
- ret = TARGET_INACC; break;
+ ret = TARGET_INACC; break;
default:
ret = TARGET_OK; break;
}
delete(t);
ret = TARGET_ERROR;
}
-
+
return ret;
}
* movehelper.cc. In case of error it sends the proper event to MAIN
* FSM.
*
- * @return TARGET_OK on succes, TARGET_ERROR on error.
+ * @return TARGET_OK on succes, TARGET_ERROR on error.
*/
static enum target_status new_trajectory(Trajectory *t)
{
}
}
-/**
+/**
* Handles new target signal from main FSM
- *
- * @param target
- *
- * @return
+ *
+ * @param target
+ *
+ * @return
*/
static enum target_status new_target(struct move_target *target)
{
return ret;
}
-/**
+/**
* Recalculates trajectory to previous move target specified by
* new_target()
*
- * @return
+ * @return
*/
enum target_status
recalculate_trajectory(void)
/* TODO: Different start for planning than esitmated position */
enum target_status ret;
current_target.use_planning = true;
- ret = new_goal(¤t_target, TRGEN_DELAY);
- switch (ret) {
+ ret = new_goal(¤t_target, TRGEN_DELAY);
+ switch (ret) {
case TARGET_OK:
- break;
- case TARGET_ERROR:
- ul_logerr("Target error on recalculation_in_progress\n");
- break;
- case TARGET_INACC:
+ break;
+ case TARGET_ERROR:
+ ul_logerr("Target error on recalculation_in_progress\n");
+ break;
+ case TARGET_INACC:
break;
}
FSM_STATE(movement)
{
+ static int obstacle_cntr = 0;
+
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
FSM_TRANSITION(wait_for_command);
break;
case EV_OBSTACLE:
+ stop();
+ if (obstacle_cntr++ > 5) {
+ FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
+ FSM_TRANSITION(wait_for_command);
+ break;
+ }
switch (recalculate_trajectory()) {
// We can go to the target:
case TARGET_OK: break;
// Target inaccessible because of an obstacle
- case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
+ case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
// Fatal error during planning
case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
ul_logerr("Target error\n");
}
break;
case EV_OBSTACLE_BEHIND:
- if (robot.moves_backward && robot.use_back_switch)
+ if (robot.moves_backward && robot.use_rear_bumper)
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_rear_bumper)
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:
case EV_EXIT:
stop();
break;
- case EV_OBSTACLE:
+ case EV_OBSTACLE:
case EV_RETURN:
case EV_TRAJECTORY_DONE:
case EV_NEW_TARGET:
FSM_STATE(wait_and_try_again)
{
+ static int target_inacc_cnt = 0;
+
switch (FSM_EVENT) {
case EV_ENTRY:
stop(); //FIXME: Not hard stop
case TARGET_OK: FSM_TRANSITION(movement); break;
// Target inaccessible because of an obstacle
case TARGET_INACC:
- FSM_TRANSITION(wait_and_try_again);
- ul_logerr("Inaccessible\n");
- FSM_TIMER(1000);
- /* FIXME (Filip): this could happen forever */
- break;
+ if (++target_inacc_cnt < 3) {
+ FSM_TRANSITION(wait_and_try_again);
+ ul_logerr("Inaccessible\n");
+ FSM_TIMER(1000);
+ /* FIXME (Filip): this could happen forever */
+ } else { /* go away if the point is not accessible, try max. 3x */
+ target_inacc_cnt = 0;
+ FSM_TRANSITION(wait_for_command);
+ FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
+ DBG_PRINT_EVENT("Target inaccessible, go to new target.");
+ }
+ break;
// Fatal error during planning
case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); 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 "corns_configs.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(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);
-
-/************************************************************************
- * INITIAL AND STARTING STATES
- ************************************************************************/
-
-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;
-
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
-
-void set_initial_position()
-{
- //FIXME:
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
-}
-
-void actuators_home()
-{
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
-}
-
-#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(climb_the_slope);
- 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:
- 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)
-{
- static int cnt = 0;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- 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);
- }
- 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)
-{
- 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_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());
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(2000);
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(sledge_down);
- break;
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(sledge_down)
-{
- 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());
- break;
- case EV_MOTION_DONE:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(to_container_diag);
- //FSM_TRANSITION(to_container_ortho);
- 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)
-{
- 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);
- break;
- case EV_TIMER:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(approach_next_corn);
- 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:
- break;
- }
-}
-
-FSM_STATE(to_container_ortho)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- 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)));
- break;
- case EV_MOTION_DONE:
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
- 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:
- 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)
-{
-
- 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 */ {
- }
- 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:
- break;
- }
-}
-
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
-{
- 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);
- 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:
- break;
- }
-}
-
-FSM_STATE(rush_the_corn)
-{
- 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;
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(experiment_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:
- break;
- }
-}
-
-// used to perform the maneuvre
-FSM_STATE(turn_around)
-{
- 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));
- break;
- case EV_MOTION_DONE:
- where_to_go = CORN;
- FSM_TRANSITION(experiment_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:
- 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;
- }
-}
-*/
* Parameters of Obstacle detection
*******************************************************************************/
-#define OBS_SIZE_M 0.2 /**< Expected size of detected obstacle */
-#define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
+#define IGNORE_CLOSER_THAN_M 2.0*MAP_CELL_SIZE_M /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
#define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
-#define OBS_FORGET_PERIOD 100 /**< The period of thread_obstacle_forgeting [ms] */
-#define OBS_FORGET_SEC 1 /**< Time to completely forget detected obstacle. */
-#define OBS_OFFSET 0.6
+#define OBS_FORGET_PERIOD 200 /**< The period of thread_obstacle_forgeting [ms] */
+#define OBS_FORGET_SEC 5 /**< Time to completely forget detected obstacle. */
void obstacle_detected_at(double x, double y, bool real_obstacle)
{
if (real_obstacle) {
/* The obstacle was detected here */
map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+ map->cells[ycell][xcell].detected_obstacle |= MAP_NEW_OBSTACLE;
}
/** Then all the cells arround obstacle cell are set as
/* 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(OBS_SIZE_M/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++) {
+ int safety_area = (int)ceil(0.2/MAP_CELL_SIZE_M);
+
+ for (i=(xcell-safety_area); i <= xcell+safety_area; i++) {
+ for (j=(ycell- safety_area); j <=ycell + safety_area; 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 */
+ distance(xx, yy, x, y) < ROBOT_DIAGONAL_RADIUS_M) {
map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
}
}
* @return
*/
-void obst_coord(struct robot_pos_type *e, const struct sharp_pos *s, double v, double *x, double *y)
+void obst_coord(struct robot_pos_type *e, const struct robot_pos_type *s, double v, double *x, double *y)
{
double sx, sy, sa;
sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
- sa = e->phi + s->ang;
+ sa = e->phi + s->phi;
*x = sx+v*cos(sa);
*y = sy+v*sin(sa);
//Pos p;
struct robot_pos_type e;
int i;
- struct sharp_pos beam;
+ struct robot_pos_type beam;
u_int16_t *data;
robot_get_est_pos(&e.x, &e.y, &e.phi);
-
+
beam.x = HOKUYO_CENTER_OFFSET_M;
beam.y = 0;
data = s->data;
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))))
+ beam.phi = HOKUYO_INDEX_TO_RAD(i);
+ if((beam.phi<(-70.0/180.0*M_PI))||((beam.phi>(70.0/180.0*M_PI))))
continue;
-
- if(data[i] > 19) {
+
+ if(data[i] > 19 && data[i] < 4000) {
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);
+ obstacle_detected_at(x, y, false);
}
-
}
}
/**
* 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.
if (val == 0) val = 1;
gettimeofday_ts(&ts);
-
+
sem_init(&timer, 0, 0);
while (1) {
__fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
forget_obstacles(val);
}
}
+
+void clear_flags_in_map(map_cell_flag_t flags)
+{
+ int x, y;
+ struct map *map = robot.map;
+
+ for (y=0;y<MAP_HEIGHT;y++){
+ for(x=0;x<MAP_WIDTH;x++){
+ map->cells[y][x].flags &= ~flags;
+ }
+ }
+}
#define _MAP_HANDLING_H
#include <robodim.h>
+#include <map.h>
void * thread_obstacle_forgeting(void * arg);
/*void update_map(struct sharps_type *s);*/
void update_map_hokuyo(struct hokuyo_scan_type *s);
+void clear_flags_in_map(map_cell_flag_t flags);
#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 3600
+#define TIME_TO_DEPOSITE_DEFAULT 3600
#else
-#define COMPETITION_TIME_DEFAULT 90
-#define TIME_TO_DEPOSITE_DEFAULT 60
+#define COMPETITION_TIME_DEFAULT 3600
+#define TIME_TO_DEPOSITE_DEFAULT 3600
#endif
/* competition time in seconds */
* @file motion-control.cc
* @author Michal Sojka <sojkam1@fel.cvut.cz>, Petr Beneš
* @date Fri Mar 20 10:36:59 2009
- *
- * @brief
- *
- *
+ *
+ * @brief
+ *
+ *
*/
//#define MOTION_DEBUG
//Controller gains
const struct balet_params k = {
- p_tangent: 3, // dx gain
- p_angle: 2, // dphi gain
- p_perpen: 5 // dy gain
-// p_tangent: 0.2, // dx gain
-// p_angle: 0.15, // dphi gain
-// p_perpen: 1 // dy gain
+ //p_tangent: 3, // dx gain
+ //p_angle: 2, // dphi gain
+ //p_perpen: 5 // dy gain
+ p_tangent: 0.2, // dx gain
+ p_angle: 0.15, // dphi gain
+ p_perpen: 1 // dy gain
};
#define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
static Trajectory *actual_trajectory;
static pthread_mutex_t actual_trajectory_lock;
-// Trajectory recalculation
+// Trajectory recalculation
sem_t recalculation_not_running;
sem_t measurement_received;
double x, y;
bool valid;
unsigned i;
-// const double times[] = { 0.5, 0.3, 0.1 }; // seconds
- const double times[] = { 0.3, 0.4, 0.5, 0.7, 0.9, 1.1 }; // seconds
+ const double times[] = { 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.5, 2.0 }; // seconds
for (i=0; i < sizeof(times)/sizeof(times[0]); i++) {
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;
perror("Warning: Cannot set RT priority for thread_prio_traj_follower()");
clock_gettime(CLOCK_REALTIME, &next);
-
+
while (1) {
ret = sem_timedwait(&measurement_received, &next);
-
+
if (ret == -1 && errno == ETIMEDOUT) {
next_period(&next, MOTION_PERIOD_NS);
if (measurement_ok) {
if (measurement_ok == 2) {
- fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!");
+ fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!\n");
}
measurement_ok--;
- }
+ }
} else {
next_period(&next, MEASURE_TIMEOUT_NS);
if (measurement_ok < 2) {
old = actual_trajectory;
gettimeofday(&tv_start, NULL);
actual_trajectory = t;
-#ifdef MOTION_LOG
+#ifdef MOTION_LOG
t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
#endif
if (old)
{
pthread_mutex_lock(&switch_to_trajectory_lock);
switch_to_trajectory = t;
- switch_time = time;
+ switch_time = time;
pthread_mutex_unlock(&switch_to_trajectory_lock);
struct timeval tv;
sem_post(&measurement_received);
}
-/**
+/**
* Initializes motion controller.
- *
- *
+ *
+ *
* @return Zero on success, non-zero otherwise.
*/
int motion_control_init()
robot.orte.motion_speed.right = 0;
robot.orte.motion_speed.left = 0;
- ORTEPublicationSend(robot.orte.publication_motion_speed);
+ ORTEPublicationSend(robot.orte.publication_motion_speed);
}
void robot_stop()
{
FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
+ robot.orte.motion_speed.left = 0xFFFF;
+ robot.orte.motion_speed.right = 0xFFFF;
+ ORTEPublicationSend(robot.orte.publication_motion_speed);
}
"main" : {
"EV_START" : "Changed state of start connector.",
- "EV_VIDLE_DONE" : "",
+ "EV_CRANE_DONE" : "",
+ "EV_CAMERA_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>
//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 */
-
- /* 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, 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 */
-
- /* 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 */
}
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()
#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.team_color = BLUE;
- if (robot.team_color == YELLOW) {
- ul_loginf("We are YELLOW!\n");
- } else {
- ul_loginf("We are BLUE!\n");
- }
+ robot_set_est_pos_trans(1, 1, DEG2RAD(0));
- robot_set_est_pos_trans(0.16, PLAYGROUND_HEIGHT_M - 0.16, DEG2RAD(-45));
-
robot.ignore_hokuyo = false;
robot.map = ShmapInit(1);
fill_in_known_areas_in_map();
block_signals();
/* initial values */
- robot.orte.motion_speed.left = 0;
- robot.orte.motion_speed.right = 0;
+ robot.orte.motion_speed.left = 0xFFFF;
+ robot.orte.motion_speed.right = 0xFFFF;
robot.orte.pwr_ctrl.voltage33 = 1;
robot.orte.pwr_ctrl.voltage50 = 1;
robot.orte.pwr_ctrl.voltage80 = 1;
- robot.orte.camera_control.on = true;
-
+ robot.orte.camera_control.on = false;
+
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_rear_bumper = 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);
#include <roboevent.h>
#include <fsm.h>
#include <robot_config.h>
+#include <can_msg_def.h>
//#include <ul_log.h>
//UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */
YELLOW
};
-enum robot_start_state {
- POWER_ON = 0,
- START_PLUGGED_IN,
- COMPETITION_STARTED,
-};
-
/**
* FSM
*/
#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_CRANE,
+
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_rear_bumper;
+ 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;
struct hokuyo_scan_type hokuyo;
bool ignore_hokuyo;
+ /* variables for target detection */
+ bool target_loaded;
+ bool target_valid;
+
struct map *map; /* Map for pathplanning (no locking) */
enum robot_status status[ROBOT_COMPONENT_NUMBER];
- char corns_conf_center;
- char corns_conf_side;
- struct corns_group *corns;
-
bool obstacle_avoidance_enabled;
/** is set to true in separate thread when there is short time left */
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
int robot_init() __attribute__ ((warn_unused_result));
int robot_start() __attribute__ ((warn_unused_result));
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 */
/*****FIXME:*****/
extern sem_t measurement_received;
-/* ----------------------------------------------------------------------
+/**
+ * Check rrobot component status
+ * @return zero if system OK, else 1
+ */
+int check_prestart_status()
+{
+ if ((robot.status[COMPONENT_MOTOR] == STATUS_FAILED) || (robot.status[COMPONENT_MOTOR] == STATUS_WARNING)) {
+ return 1;
+ } else if (robot.status[COMPONENT_ODOMETRY] == STATUS_FAILED) {
+ return 1;
+ } else if ((robot.status[COMPONENT_CAMERA] == STATUS_FAILED) || (robot.status[COMPONENT_CAMERA] == STATUS_WARNING)) {
+ return 1;
+ } else if (robot.status[COMPONENT_POWER] == STATUS_FAILED) {
+ return 1;
+ } else if (robot.status[COMPONENT_HOKUYO] == STATUS_FAILED) {
+ return 1;
+ } else if (robot.status[COMPONENT_CRANE] == STATUS_FAILED) {
+ return 1;
+ } else if (robot.status[COMPONENT_START] == STATUS_FAILED || robot.status[COMPONENT_START] == STATUS_WARNING) {
+ return 1;
+ } else {
+ return 0;
+ }
+}
+/* ----------------------------------------------------------------------
* PUBLISHER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
-void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
+void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
-
+
ROBOT_LOCK(ref_pos);
*instance = robot.ref_pos;
ROBOT_UNLOCK(ref_pos);
}
-void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
+void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
-
+
ROBOT_LOCK(est_pos_odo);
*instance = robot.est_pos_odo;
ROBOT_UNLOCK(est_pos_odo);
}
-void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
+void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
-
+
ROBOT_LOCK(est_pos_indep_odo);
*instance = robot.est_pos_indep_odo;
ROBOT_UNLOCK(est_pos_indep_odo);
}
-static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
+static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
- robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
+ robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
}
-void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
+static void send_system_status_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ system_status *instance = (system_status *)vinstance;
+
+ /* if any component not ready, send warning for display to show APP yellow */
+ if (check_prestart_status()) {
+ instance->system_condition = 1; // system warning
+ } else {
+ instance->system_condition = 0; // system OK
+ }
+}
+
+void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
}
-/* ----------------------------------------------------------------------
+/* ----------------------------------------------------------------------
* SUBSCRIBER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
double dleft, dright, dtang, dphi;
static bool first_run = true;
/* spocitat prevodovy pomer */
- const double n = (double)(1.0 / 1.0);
+ const double n = (double)(1.0 / 1.0);
/* vzdalenost na pulz IRC */
const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
first_run = false;
break;
}
-
+
dleft = ((robot.odo_data.left - instance->left) >> 8) * c; // TODO >> 8 ?
dright = ((instance->right - robot.odo_data.right) >> 8) * c;
robot.est_pos_indep_odo.y += dtang*sin(a);
robot.est_pos_indep_odo.phi += dphi;
ROBOT_UNLOCK(est_pos_indep_odo);
-
+
ROBOT_LOCK(odo_data);
robot.odo_data = *instance;
ROBOT_UNLOCK(odo_data);
-
+
robot.indep_odometry_works = true;
-
+
/* wake up motion-control/thread_trajectory_follower */
sem_post(&measurement_received);
- //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
+ robot.status[COMPONENT_ODOMETRY] = STATUS_OK;
break;
case DEADLINE:
robot.indep_odometry_works = false;
- //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
+ robot.status[COMPONENT_ODOMETRY] = STATUS_FAILED;
DBG("ORTE deadline occurred - odo_data receive\n");
break;
}
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)(28.0 / 1.0);
/* vzdalenost na pulz IRC */
const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-
+
switch (info->status) {
case NEW_DATA:
if (first_run) {
first_run = false;
break;
}
-
+
dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
robot.odometry_works = true;
- robot.status[COMPONENT_MOTOR] = STATUS_OK;
+ //robot.status[COMPONENT_MOTOR] = STATUS_OK;
break;
case DEADLINE:
robot.odometry_works = false;
void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ struct motion_status_type *m = (struct motion_status_type *)vinstance;
+
switch (info->status) {
case NEW_DATA:
+ if (m->err_left == 0 && m->err_right == 0)
+ robot.status[COMPONENT_MOTOR] = STATUS_OK;
+ else
+ robot.status[COMPONENT_MOTOR] = STATUS_WARNING;
break;
case DEADLINE:
DBG("ORTE deadline occurred - motion_status receive\n");
* pluggin in and out start connector. */
switch (robot.start_state) {
case POWER_ON:
- if (!instance->start_condition) {
- robot.status[COMPONENT_START] = STATUS_WARNING;
+ robot.status[COMPONENT_START] = STATUS_WARNING;
+ if (instance->start_condition == START_PLUGGED_IN) {
robot.start_state = START_PLUGGED_IN;
ul_logmsg("START_PLUGGED_IN\n");
}
case START_PLUGGED_IN:
robot.status[COMPONENT_START] = STATUS_OK;
- /* Competition starts when plugged out */
- if (instance->start_condition) {
+ /* Competition starts when plugged out, check component status before start */
+ if ((instance->start_condition == COMPETITION_STARTED) && !check_prestart_status()) {
FSM_SIGNAL(MAIN, EV_START, NULL);
robot.start_state = COMPETITION_STARTED;
ul_logmsg("STARTED\n");
break;
case COMPETITION_STARTED: {
+ robot.status[COMPONENT_START] = STATUS_WARNING;
/* Subsequent plug-in stops the robot */
static int num = 0;
- if (!instance->start_condition) {
- robot.status[COMPONENT_START] = STATUS_WARNING;
+ if (instance->start_condition == START_PLUGGED_IN) {
if (++num == 10)
robot_exit();
}
}
}
-// FIXME: this is not up to date (Filip, 2010)
void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+ static bool last_response = false;
switch (info->status) {
case NEW_DATA: {
- ROBOT_LOCK(camera_result);
- robot.corns_conf_side = instance->side;
- robot.corns_conf_center = instance->center;
- ROBOT_UNLOCK(camera_result);
- robot.status[COMPONENT_CAMERA] = STATUS_OK;
+ if (instance->error) {
+ robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
+ } else if (instance->data_valid && instance->data_valid != last_response) {
+ ROBOT_LOCK(camera_result);
+ robot.target_valid = instance->target_valid;
+ ROBOT_UNLOCK(camera_result);
+ FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
+ printf("robot_orte: valid: %d\n", instance->target_valid);
+
+ robot.status[COMPONENT_CAMERA] = STATUS_OK;
+ }
+ last_response = instance->data_valid;
break;
}
case DEADLINE:
break;
}
}
-/* ----------------------------------------------------------------------
- * SUBSCRIBER CALLBACKS - EB2008
- * ---------------------------------------------------------------------- */
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
+ struct crane_status_type *instance = (struct crane_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_VIDLE]=STATUS_OK;
+ robot.status[COMPONENT_CRANE]=STATUS_OK;
else
- robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
+ robot.status[COMPONENT_CRANE]=STATUS_WARNING;
if (instance->response != last_response &&
- instance->response == act_vidle_get_last_reqest())
- FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
+ instance->response == act_crane_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_CRANE_DONE, NULL);
last_response = instance->response;
break;
case DEADLINE:
- robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
+ robot.status[COMPONENT_CRANE]=STATUS_FAILED;
DBG("ORTE deadline occurred - actuator_status receive\n");
break;
}
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)
- FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
- if ((!last_left && instance->bumper_left) ||
- (!last_right && 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);
- }
+ if ((!last_strategy && instance->strategy) && (robot.start_state == POWER_ON)) {
+ FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
}
- last_right = instance->bumper_right;
- last_left = instance->bumper_left;
+ 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;
+ switch (info->status) {
+ case NEW_DATA:
+ if (!instance->bumper_rear) {
+ FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+ }
+ if ((!instance->bumper_left) ||
+ (!instance->bumper_right)) {
+ FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
+ }
+ break;
+ case DEADLINE:
+ break;
+ }
+}
+
+
#define HIST_CNT 5
#if 0
static int cmp_double(const void *v1, const void *v2)
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_system_status_create(&robot.orte, send_system_status_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_crane_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_magnet_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_crane_status_create(&robot.orte, rcv_crane_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 "eb2010misc.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_strategy_12_oranges); /* Log domain name = ulogd + name of the file */
-
-static FSM_STATE_DECL(pick_our_oranges);
-
-FSM_STATE(start_12_oranges)
-{
- 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_our_oranges);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- start_timer();
- break;
- case EV_EXIT:
- start_exit();
- break;
- case EV_SWITCH_STRATEGY:
- FSM_TRANSITION(start_opp_corn);
- break;
- default:;
- }
-}
-
-
-#undef FSM_STATE_VISIBILITY
-#define FSM_STATE_VISIBILITY static
-
-FSM_STATE_DECL(pick_rest_of_our_oranges);
-FSM_STATE_DECL(unload_oranges);
-FSM_STATE_DECL(pick_opp_oranges);
-FSM_STATE_DECL(opp_corns);
-
-FSM_STATE(pick_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(pick_rest_of_our_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(pick_rest_of_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_CENTER)); break;
- case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(unload_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
- case EV_RETURN: FSM_TRANSITION(pick_opp_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE_DECL(pick_rest_of_opp_oranges);
-FSM_STATE(pick_opp_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(OPPONENTS, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(pick_rest_of_opp_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE_DECL(unload_opp_oranges);
-FSM_STATE(pick_rest_of_opp_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(OPPONENTS, NEAR_PLAYGROUND_CENTER)); break;
- case EV_RETURN: FSM_TRANSITION(unload_opp_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(unload_opp_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
-
-FSM_STATE(opp_corns)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_next_corn, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
+++ /dev/null
-#include "common-states.h"
-#include "robot.h"
-#include "eb2010misc.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_strategy_opp_corn); /* Log domain name = ulogd + name of the file */
-
-static FSM_STATE_DECL(pick_our_oranges);
-
-FSM_STATE(start_opp_corn)
-{
- 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_our_oranges);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- start_timer();
- break;
- case EV_EXIT:
- start_exit();
- break;
- case EV_SWITCH_STRATEGY:
- FSM_TRANSITION(start_opp_oranges);
- break;
- default:;
- }
-}
-
-
-#undef FSM_STATE_VISIBILITY
-#define FSM_STATE_VISIBILITY static
-
-FSM_STATE_DECL(unload_oranges);
-FSM_STATE_DECL(opp_corns);
-
-
-FSM_STATE(pick_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(unload_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
-
-FSM_STATE(opp_corns)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_next_corn, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
+++ /dev/null
-#include "common-states.h"
-#include "robot.h"
-#include "eb2010misc.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_strategy_opp_oranges); /* Log domain name = ulogd + name of the file */
-
-static FSM_STATE_DECL(pick_our_oranges);
-
-FSM_STATE(start_opp_oranges)
-{
- 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_our_oranges);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- start_timer();
- break;
- case EV_EXIT:
- start_exit();
- break;
- case EV_SWITCH_STRATEGY:
- FSM_TRANSITION(start_six_oranges);
- break;
- default:;
- }
-}
-
-
-#undef FSM_STATE_VISIBILITY
-#define FSM_STATE_VISIBILITY static
-
-FSM_STATE_DECL(unload_oranges);
-FSM_STATE_DECL(pick_opp_oranges);
-FSM_STATE_DECL(opp_corns);
-
-FSM_STATE(pick_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(unload_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
- /* FIXME: do something more meaningfull the next time */
- case EV_RETURN: FSM_TRANSITION(pick_opp_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(pick_opp_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(OPPONENTS, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(opp_corns)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_next_corn, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
+++ /dev/null
-#include "common-states.h"
-#include "robot.h"
-#include "eb2010misc.h"
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_strategy_six_oranges); /* Log domain name = ulogd + name of the file */
-
-static FSM_STATE_DECL(pick_our_oranges);
-
-FSM_STATE(start_six_oranges)
-{
- 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_our_oranges);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- start_timer();
- break;
- case EV_EXIT:
- start_exit();
- break;
- case EV_SWITCH_STRATEGY:
- FSM_TRANSITION(start_12_oranges);
- break;
- default:;
- }
-}
-
-
-#undef FSM_STATE_VISIBILITY
-#define FSM_STATE_VISIBILITY static
-
-FSM_STATE_DECL(pick_rest_of_our_oranges);
-FSM_STATE_DECL(unload_oranges);
-FSM_STATE_DECL(opp_corns);
-
-FSM_STATE(pick_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_BOUNDARY)); break;
- case EV_RETURN: FSM_TRANSITION(pick_rest_of_our_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(pick_rest_of_our_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_CENTER)); break;
- case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
- default: break;
- }
-}
-
-FSM_STATE(unload_oranges)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
-
-FSM_STATE(opp_corns)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: SUBFSM_TRANSITION(approach_next_corn, NULL); break;
- case EV_RETURN: FSM_TRANSITION(opp_corns); break;
- default: break;
- }
-}
--- /dev/null
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include <trgen.h>
+#include <stdbool.h>
+#include <ul_log.h>
+#include <hokuyo.h>
+
+#include "robodata.h"
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
+#include "sub-states.h"
+
+UL_LOG_CUST(ulogd_sub_states); /* Log domain name = ulogd + name of the file */
+
+extern struct TrajectoryConstraints tcSlow, tcVerySlow;
+
+/************************************************************************
+ * 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)
+
+
+#define HOK_MIN_MM (ROBOT_AXIS_TO_FRONT_MM - HOKUYO_CENTER_OFFSET_MM)
+#define HOK_MIN_M (HOK_MIN_MM/1000.0)
+#define HOK_MAX_MM (1000)
+
+/**
+ * Count the angle to turn by to see the minimum distance in the robot axis.
+ * @param alpha poiner to the angle to turn by
+ * @param minimum pointer to the minimum distance from hokuyo
+ */
+void get_hokuyo_min(double *alpha, double *minimum)
+{
+ double beta;
+ double min = 1000;
+ int min_i;
+
+ struct hokuyo_scan_type scan = robot.hokuyo;
+ uint16_t *data = scan.data;
+
+ for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
+ beta = HOKUYO_INDEX_TO_RAD(i);
+ if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
+ continue;
+
+ if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
+ if (data[i] < min ) {
+ min = data[i];
+ min_i = i;
+ }
+ // printf("min: %f, beta: %f\n", min, beta);
+ }
+ }
+ min /= 1000.0;
+ beta = HOKUYO_INDEX_TO_RAD(min_i);
+ *alpha = atan2((min * sin(beta)), (HOKUYO_CENTER_OFFSET_M + min * cos(beta)));
+ *minimum = min - HOK_MIN_M;
+
+ printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
+}
+
+void enable_obstacle_avoidance(bool enable)
+{
+ robot.obstacle_avoidance_enabled = enable;
+ robot.ignore_hokuyo = !enable;
+ robot.check_turn_safety = enable;
+}
+
+FSM_STATE(recognize)
+{
+ static int camera_delay = 0;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("recognition");
+ act_camera_on();
+#if PPC
+ FSM_TIMER(1000);
+#else
+ robot.target_valid = false;
+ FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
+#endif
+ break;
+ case EV_TIMER:
+ if (++camera_delay > 10) {
+ // waiting for 10 seconds now, return back
+ DBG_PRINT_EVENT("camera: No response!");
+ ROBOT_LOCK(camera_result);
+ robot.target_valid = false;
+ ROBOT_UNLOCK(camera_result);
+ SUBFSM_RET(NULL);
+ } else {
+ FSM_TIMER(1000);
+ }
+ break;
+ case EV_CAMERA_DONE:
+ act_camera_off();
+ if (robot.target_valid) {
+ DBG_PRINT_EVENT("camera: Target valid!");
+ FSM_TRANSITION(get_target_turn);
+ } else {
+ DBG_PRINT_EVENT("camera: Target not valid!");
+ robot.target_loaded = false;
+ SUBFSM_RET(NULL);
+ }
+ break;
+ case EV_EXIT:
+ camera_delay = 0;
+ break;
+ }
+}
+
+FSM_STATE(get_target_turn)
+{
+ const double close = 0.05;
+ double alpha, minimum;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("turn");
+ enable_obstacle_avoidance(false);
+ get_hokuyo_min(&alpha, &minimum);
+ robot_get_est_pos(&x, &y, &phi);
+ robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_MOTION_DONE:
+ get_hokuyo_min(&alpha, &minimum);
+ if (minimum < close) {
+ FSM_TRANSITION(get_target_load);
+ } else {
+ FSM_TRANSITION(get_target_touch);
+ }
+ break;
+ case EV_MOTION_ERROR:
+ enable_obstacle_avoidance(true);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_touch)
+{
+ const double step = 0.02;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("touch");
+ robot_move_by(step, NO_TURN(), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(get_target_turn);
+ break;
+ case EV_MOTION_ERROR:
+ enable_obstacle_avoidance(true);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_load)
+{
+ static int direction = 0;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ direction = 0;
+ act_magnet(1);
+ act_crane(CRANE_DOWN);
+ break;
+ case EV_TIMER:
+ act_crane(CRANE_UP);
+ robot.target_loaded = true;
+ FSM_TRANSITION(get_target_back);
+ break;
+ case EV_CRANE_DONE:
+ if (direction == 0) {
+ direction = 1;
+ FSM_TIMER(1000);
+ }
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_back)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_move_by(-0.3, NO_TURN(), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_CRANE_DONE:
+ break;
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ enable_obstacle_avoidance(true);
+ break;
+ }
+}
+
+/*
+FSM_STATE()
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+*/
\ No newline at end of file
--- /dev/null
+#ifndef SUB_STATES_H
+#define SUB_STATES_H
+
+#define FSM_MAIN
+
+#include "roboevent.h"
+#include <fsm.h>
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+FSM_STATE_DECL(recognize);
+FSM_STATE_DECL(get_target_turn);
+FSM_STATE_DECL(get_target_touch);
+FSM_STATE_DECL(get_target_load);
+FSM_STATE_DECL(get_target_back);
+
+void get_hokuyo_min(double *alpha, double *minimum);
+
+#endif
\ No newline at end of file
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
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:
#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"
FSM_STATE(test_vidle)
void MainWindow::addRobomonAtlantisTab()
{
- RobomonAtlantis *robomonAtlantis = new RobomonAtlantis();
- tabWidget->addTab(robomonAtlantis, tr("Robomon - Temples of Atlantis"));
+ RobomonAtlantis *robomonAtlantis = new RobomonAtlantis(statusBar());
+ tabWidget->addTab(robomonAtlantis, tr("Robomon - Demo"));
tabWidget->setCurrentWidget(robomonAtlantis);
connect(showMap, SIGNAL(toggled( bool )), robomonAtlantis, SLOT(showMap(bool)));
connect(showTrails, SIGNAL(toggled( bool )), robomonAtlantis, SLOT(showTrails(bool)));
--- /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
* License: GNU GPL v.2
*/
#include <QGraphicsSceneMouseEvent>
#include <QGraphicsScene>
#include <QGraphicsRectItem>
+#include <QImage>
+
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 22
+#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))
+#define PLAYGROUND_WIDTH PLAYGROUND_WIDTH_MM
+#define PLAYGROUND_HEIGHT PLAYGROUND_HEIGHT_MM
+#define PLAYGROUND_COLOR lightGray
-/**
- * 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);
-}
+//grid
+#define GRID_WIDTH 1000
+#define GRID_HEIGHT 1000
PlaygroundScene::PlaygroundScene(QObject *parent)
: QGraphicsScene(parent)
{
using namespace Qt;
QGraphicsRectItem *tempRect;
-
+ QGraphicsLineItem *tempLine;
+
/* 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(PLAYGROUND_WIDTH, 0, BORDER_WIDTH, PLAYGROUND_HEIGHT), QPen(), QBrush(BORDER_COLOR)); //right
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);
+
+ /* horizontal grid */
+ for (int i = 0; i < (PLAYGROUND_HEIGHT / GRID_HEIGHT); i++) {
+ tempLine = addLine(QLine( 0, PLAYGROUND_HEIGHT - i*GRID_HEIGHT, PLAYGROUND_WIDTH, PLAYGROUND_HEIGHT - i*GRID_HEIGHT), QPen());
+ tempLine->setZValue(4);
}
-
- /* queens */
-// QGraphicsEllipseItem *queen = NULL;
- for(int i = 0; i < QUEEN_CNT; i++){
- putQueen(tomato, queen[i].x, queen[i].y);
+
+ /* vertical grid */
+ for (int i = 0; i < (PLAYGROUND_WIDTH / GRID_WIDTH); i++) {
+ tempLine = addLine(QLine(PLAYGROUND_WIDTH - i*GRID_WIDTH, PLAYGROUND_HEIGHT, PLAYGROUND_WIDTH - i*GRID_WIDTH, 0), QPen());
+ tempLine->setZValue(4);
}
-
+
/* obstacles */
obstacle = new QGraphicsEllipseItem(0, 0, SIM_OBST_SIZE_M*1000, SIM_OBST_SIZE_M*1000);
obstacle->translate(-SIM_OBST_SIZE_M*1000/2,-SIM_OBST_SIZE_M*1000/2);
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
+#define SIM_OBST_SIZE_M 0.2
class PlaygroundScene : public QGraphicsScene
{
~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 putCorn(struct corn * corn);
- void putBonus(QGraphicsEllipseItem *g, int centerX, int centerY);
- 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
#include "playgroundview.h"
#include "trail.h"
+
#include <QCoreApplication>
#include <QEvent>
#include <QKeyEvent>
#include <QDebug>
#include <QMessageBox>
-RobomonAtlantis::RobomonAtlantis(QWidget *parent)
- : QWidget(parent), motorSimulation(orte)
+RobomonAtlantis::RobomonAtlantis(QStatusBar *_statusBar)
+ : QWidget(0), statusBar(_statusBar), motorSimulation(orte)
{
QFont font;
font.setPointSize(7);
createOrte();
createRobots();
createActions();
+ createMap();
// connect(vidle, SIGNAL(valueChanged(int)),
// robotEstPosBest, SLOT(setVidle(int)));
motorSimulationCheckBox->setToolTip("Subscribes to motion_speed and\nbased on this publishes motion_irc.");
layout->addWidget(motorSimulationCheckBox);
- startPlug = new QCheckBox("&Start plug");
- layout->addWidget(startPlug);
+// startPlug = new QCheckBox("&Start plug");
+// layout->addWidget(startPlug);
colorChoser = new QCheckBox("&Team color");
layout->addWidget(colorChoser);
void RobomonAtlantis::createActuatorsGroupBox()
{
- actuatorsGroupBox = new QGroupBox(tr("Actuators"));
- actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
- QHBoxLayout *layout = new QHBoxLayout();
-// vidle = new QDial();
-
-// vidle->setMinimum(VIDLE_VYSIP);
-// vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP);
-// vidle->setEnabled(true);
+ actuatorsGroupBox = new QGroupBox(tr("Status"));
+ actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Maximum);
+ QGridLayout *layout = new QGridLayout();
- //createMotorsGroupBox();
+ layout->addWidget(MiscGui::createLabel("APP:"), 1, 0);
+ system_status = new QLabel();
+ system_status->setMinimumWidth(100);
+ system_status->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ layout->addWidget(system_status, 1, 1);
- layout->setAlignment(Qt::AlignLeft);
-// layout->addWidget(vidle);
- //layout->addWidget(enginesGroupBox);
- actuatorsGroupBox->setLayout(layout);
+ actuatorsGroupBox->setLayout(layout);
}
void RobomonAtlantis::createPowerGroupBox()
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(3);
+ mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
+
+
+ playgroundScene->addItem(mapImage);
+}
+
/**********************************************************************
* GUI actions
**********************************************************************/
// connect(stopMotorsPushButton, SIGNAL(clicked()),
// this, SLOT(stopMotors()));
- connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
- connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
+// connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
/* obstacle simulation */
simulationEnabled = 0;
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];
color = lightGray;
- if ((cell->flags & MAP_FLAG_WALL) &&
- (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
+ if (cell->flags & MAP_FLAG_WALL)
color = darkYellow;
if (cell->flags & MAP_FLAG_IGNORE_OBST)
color = darkGreen;
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);
}
}
}
this, SLOT(simulateObstaclesHokuyo()));
obstacleSimulationTimer->start(100);
setMouseTracking(true);
+ hokuyoScan->setVisible(true);
+
+
} else {
if (obstacleSimulationTimer)
delete obstacleSimulationTimer;
- //double distance = 0.8;
+
+ hokuyoScan->setVisible(false); /* hide hokuyo scan*/
+
}
}
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_CRANE_CMD):
+ robotEstPosBest->setCrane(orte.crane_cmd.req_pos);
break;
case QEVENT(QEV_REFERENCE_POSITION):
emit actualPositionReceivedSignal();
case QEVENT(QEV_FSM_MOTION):
fsm_motion_state->setText(orte.fsm_motion.state_name);
break;
+ case QEVENT(QEV_STSTEM_STATUS):
+ if (orte.system_status.system_condition)
+ system_status->setText("WARNING");
+ else
+ system_status->setText("OK");
+ break;
default:
if (event->type() == QEvent::Close)
closeEvent((QCloseEvent *)event);
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
robottype_subscriber_est_pos_best_create(&orte,
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
+ robottype_subscriber_system_status_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_STSTEM_STATUS));
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_crane_cmd_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_CRANE_CMD));
robottype_subscriber_fsm_main_create(&orte,
rcv_fsm_main_cb, this);
robottype_subscriber_fsm_motion_create(&orte,
rcv_fsm_motion_cb, this);
robottype_subscriber_fsm_act_create(&orte,
rcv_fsm_act_cb, this);
- robottype_subscriber_motion_speed_create(&orte, NULL, NULL);
+ robottype_subscriber_motion_speed_create(&orte, NULL, NULL);
/* power management */
orte.pwr_ctrl.voltage33 = true;
/* Get segment identificator in a read only mode */
segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
if(segmentId == -1) {
- QMessageBox::critical(this, "robomon",
- "Unable to open shared memory segment!");
- return;
+ statusBar->showMessage("No external map found - creating a new map.");
}
/* Init Shmap */
{
struct robot_pos_type e = orte.est_pos_best;
double sensor_a;
- struct sharp_pos s;
+ struct robot_pos_type s;
s.x = HOKUYO_CENTER_OFFSET_M;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.phi = 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;
+ sensor_a = e.phi + s.phi;
const double sensorRange = 4.0; /*[meters]*/
ORTEPublicationSend(orte.publication_robot_cmd);
}
-void RobomonAtlantis::setTeamColor(int plug)
-{
- orte.robot_switches.team_color = plug ? 1 : 0;
- ORTEPublicationSend(orte.publication_robot_switches);
-}
-
void RobomonAtlantis::setMotorSimulation(int state)
{
if (state) {
#include "PlaygroundScene.h"
#include "playgroundview.h"
#include "Robot.h"
+#include "Map.h"
#include <roboorte_robottype.h>
#include "trail.h"
#include "hokuyoscan.h"
+#include <QMainWindow>
class QHBoxLayout;
class QVBoxLayout;
class QSlider;
class QProgressBar;
class QFont;
+class QImage;
class MotorSimulation : QObject {
Q_OBJECT
Q_OBJECT
public:
- RobomonAtlantis(QWidget *parent = 0);
+ RobomonAtlantis(QStatusBar *_statusBar = 0);
protected:
bool event(QEvent *event);
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 simulateObstaclesHokuyo();
void changeObstacle(QPointF position);
void sendStart(int plug);
- void setTeamColor(int plug);
void setMotorSimulation(int state);
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void motionStatusReceived();
void actualPositionReceived();
void createRobots();
void createActions();
+ void createMap();
+
+ QStatusBar *statusBar;
QVBoxLayout *leftLayout;
QVBoxLayout *rightLayout;
QLabel *fsm_main_state;
QLabel *fsm_act_state;
QLabel *fsm_motion_state;
+ QLabel *system_status;
QCheckBox *startPlug;
QCheckBox *colorChoser;
public:
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);
class MotorSimulation motorSimulation;
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void createOrte();
this->pen = pen;
this->brush = brush;
setVisible(false);
- setVidle(0);
+ setCrane(0);
moveRobot(1, 1, 0);
}
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, -ROBOT_CRANE_LENGTH_M*1000,
+ ROBOT_WIDTH_MM, ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM + ROBOT_CRANE_LENGTH_M*1000);
}
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[] = {
+ if (crane != 0) {
+ const float crane_angle = (float)(crane - CRANE_DOWN)/(CRANE_UP-CRANE_DOWN)*M_PI/2.0;
+ const float y = -ROBOT_CRANE_LENGTH_M*1000*cos(crane_angle);
+ QLineF crane[] = {
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);
+ painter->drawLines(crane, 4);
}
painter->setFont(QFont("Arial", 40, 0, 0));
setVisible(show);
}
-void Robot::setVidle(int value)
+void Robot::setCrane(int value)
{
QRectF r = boundingRect();
//r.setBottom(0);
- vidle = value;
+ crane = value;
update(r);
}
void moveRobot(double x, double y, double angle);
public slots:
void mySetVisible(bool show);
- void setVidle(int value);
+ void setCrane(int value);
private:
- int vidle;
+ int crane;
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
}
}
+void rcv_system_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *arg)
+{
+ Q_UNUSED(vinstance);
+ switch (info->status) {
+ case NEW_DATA:
+ POST_QEVENT(arg, QEV_STSTEM_STATUS);
+ break;
+ case DEADLINE:
+ break;
+ }
+}
+
void generic_rcv_cb(const ORTERecvInfo *info, void *vinstance, void *arg)
{
OrteCallbackInfo *i = (OrteCallbackInfo*)arg;
QEV_FSM_ACT,
QEV_FSM_MOTION,
QEV_HOKUYO_SCAN,
- QEV_VIDLE_CMD,
+ QEV_CRANE_CMD,
+ QEV_STSTEM_STATUS,
};
class OrteCallbackInfo {
void *vinstance, void *arg);
void rcv_fsm_motion_cb(const ORTERecvInfo *info,
void *vinstance, void *arg);
+void rcv_system_status_cb(const ORTERecvInfo *info,
+ void *vinstance, void *arg);
void generic_rcv_cb(const ORTERecvInfo *info, void *vinstance, void *arg);
#endif /* ROBOMON_ORTE_H */
-Subproject commit 0d162fa62da77df8556024aad00392b842db9bef
+Subproject commit d77f7efa386c01359ae120a9532f2b96a62eb141
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)
}
($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,
struct motion_speed {
short left;
- short right;
+ short right;
};
struct motion_status {
};
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 {
- boolean start_condition;
+ char start_condition;
};
-struct robot_switches {
- boolean bumper_pressed;
+struct system_status {
+ char system_condition;
+};
+
+struct robot_bumpers {
+ boolean bumper_rear;
boolean bumper_left;
boolean bumper_right;
- octet team_color;
+};
+
+struct robot_switches {
+ octet strategy;
};
struct can_msg {
unsigned short data[681];
};
-/** Request for hokuyo pitch (angle) */
-struct hokuyo_pitch {
- /** Hokuyo angle (0..down, 255..up)*/
- octet angle;
+// Actuators
+struct magnet_cmd {
+ unsigned short on;
};
-// Actuators
-struct vidle_cmd {
+struct crane_cmd {
unsigned short req_pos;
- octet speed;
+ unsigned short speed;
};
/** Status sent from actuators */
-struct vidle_status {
+struct crane_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_*
-};
-
-// FIXME: What's this??? M.S.
-struct binary_data {
- octet data;
+ octet flags; // Zero when everything OK, otherwise see CAN_CRANE_FLAG_*
};
struct fsm_state {
const error ERR_NO_VIDEO = 0x01;
const error ERR_NO_FRAME = 0x02;
- struct result {
- octet side;
- octet center;
- double sideDist;
- double centerDist;
- error error;
- };
+ struct result {
+ boolean data_valid;
+ boolean target_valid;
+ error error;
+ };
};
/** Command from display - see uoled.h/UDE_recieve_cmd_t */
/* Definition of ORTE variables - input for roboortegen.pl */
-type=vidle_status topic=vidle_status deadline=0.5
-type=vidle_cmd topic=vidle_cmd
-type=binary_data topic=binary_data
+type=crane_status topic=crane_status deadline=1
+type=crane_cmd topic=crane_cmd
+type=magnet_cmd topic=magnet_cmd
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=robot_pos topic=est_pos_best deadline=1
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=motion_speed topic=motion_speed deadline=0.3 pubdelay=0.1
type=motion_status topic=motion_status deadline=1.5
-type=pwr_alert topic=pwr_alert
-type=pwr_ctrl topic=pwr_ctrl
-type=pwr_voltage topic=pwr_voltage
+type=pwr_alert topic=pwr_alert deadline=1
+type=pwr_ctrl topic=pwr_ctrl deadline=1
+type=pwr_voltage topic=pwr_voltage deadline=1
type=robot_pos topic=ref_pos
type=robot_cmd topic=robot_cmd deadline=1
type=robot_switches topic=robot_switches deadline=1
-type=camera_result topic=camera_result
+type=robot_bumpers topic=robot_bumpers deadline=1
+type=camera_result topic=camera_result deadline=1
type=camera_control topic=camera_control pubdelay=1
type=fsm_state topic=fsm_main pubdelay=1
type=fsm_state topic=fsm_act pubdelay=1
type=fsm_state topic=fsm_motion pubdelay=1
type=display_cmd topic=display_cmd
+type=system_status topic=system_status deadline=1
projects / eurobot / public.git / commitdiff