[submodule "src/ulan-app"]
path = src/ulan-app
url = git://ulan.git.sourceforge.net/gitroot/ulan/ulan-app
+[submodule "src/3rdparty/qt"]
+ path = src/3rdparty/qt
+ url = git://gitorious.org/qt/qt.git
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+SUBDIRS = $(ALL_OMK_SUBDIRS)
--- /dev/null
+../_infrastructure/Makefile.rules
\ No newline at end of file
--- /dev/null
+CROSS_COMPILE = arm-linux-gnueabi-
+CC = $(CROSS_COMPILE)gcc
+CXX = $(CROSS_COMPILE)g++
+AS = $(CROSS_COMPILE)as
+AR = $(CROSS_COMPILE)ar
+STRIP = $(CROSS_COMPILE)strip
+
+CFLAGS = -Wall
+CXXFLAGS = $(CFLAGS)
+
+#INCLUDES = $(-I/home/zandar/programovani/embedded/qt4/target/include)
+
+IDL_COMPILER = orte-idl
+IDL_COMPILER=$(MAKERULES_DIR)/../host/_compiled/bin/orte-idl
+
+CONFIG_QT4_DIR = $(OUTPUT_DIR)/_compiled/qt
+CONFIG_OC_ETH_ORTE_IDL=n
+LN_HEADERS=y
+OMIT_KERNEL_PASSES=y
+CFLAGS = -O2 -g -Wall
+CXXFLAGS = -O2 -g -Wall
+
--- /dev/null
+../../src/display-qt/
\ No newline at end of file
--- /dev/null
+../../src/orte
\ No newline at end of file
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+library-pass_HOOKS = install-qt
+
+config.status: $(SOURCES_DIR)/qt-src/configure
+ cp -a $(SOURCES_DIR)/linux-arm-gumstix-g++ $(SOURCES_DIR)/qt-src/mkspecs/qws
+ $< -embedded arm --prefix=$(OUTPUT_DIR)/_compiled/qt -xplatform qws/linux-arm-gumstix-g++ -opensource -confirm-license -little-endian -qt-gfx-linuxfb
+
+lib/libQtCore.so: config.status
+ $(MAKE)
+
+$(OUTPUT_DIR)/_compiled/qt/libQtCore.so: lib/libQtCore.so
+ echo $@: $^
+ $(MAKE) install
+ touch $@
+
+.PHONY: install-qt
+install-qt: $(OUTPUT_DIR)/_compiled/qt/libQtCore.so
--- /dev/null
+#
+# qmake configuration for building with arm-linux-gnueabi-g++ - used
+# for Eurobot team Flamingos. It would be better if we use the same
+# settings from OMK's config.target, but I do not know how to achieve
+# that.
+#
+
+include(../../common/g++.conf)
+include(../../common/linux.conf)
+include(../../common/qws.conf)
+
+# modifications to g++.conf
+QMAKE_CC = arm-linux-gnueabi-gcc
+QMAKE_CXX = arm-linux-gnueabi-g++
+QMAKE_LINK = arm-linux-gnueabi-g++
+QMAKE_LINK_SHLIB = arm-linux-gnueabi-g++
+
+# modifications to linux.conf
+QMAKE_AR = arm-linux-gnueabi-ar cqs
+QMAKE_OBJCOPY = arm-linux-gnueabi-objcopy
+QMAKE_STRIP = arm-linux-gnueabi-strip
+
+load(qt_config)
--- /dev/null
+/****************************************************************************
+**
+** Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: Nokia Corporation (qt-info@nokia.com)
+**
+** This file is part of the qmake spec of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** Commercial Usage
+** Licensees holding valid Qt Commercial licenses may use this file in
+** accordance with the Qt Commercial License Agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and Nokia.
+**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 2.1 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 2.1 requirements
+** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
+**
+** In addition, as a special exception, Nokia gives you certain additional
+** rights. These rights are described in the Nokia Qt LGPL Exception
+** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 3.0 as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU General Public License version 3.0 requirements will be
+** met: http://www.gnu.org/copyleft/gpl.html.
+**
+** If you have questions regarding the use of this file, please contact
+** Nokia at qt-info@nokia.com.
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "../../linux-g++/qplatformdefs.h"
--- /dev/null
+../../../src/3rdparty/qt
\ No newline at end of file
--- /dev/null
+../../src/robodim
\ No newline at end of file
--- /dev/null
+../../src/robomath
\ No newline at end of file
--- /dev/null
+../../src/types
\ No newline at end of file
+++ /dev/null
-../../src/pxmc
\ No newline at end of file
--- /dev/null
+../../src/pxmc/libs4c/pxmc_bsp/
\ No newline at end of file
--- /dev/null
+../../src/pxmc/libs4c/pxmc_core/
\ No newline at end of file
--- /dev/null
+../../src/barcode/
\ No newline at end of file
update_submodule src/ulut
update_submodule robot-root
update_submodule src/v4l/v4l-utils
-#update_submodule src/pxmc
+update_submodule src/pxmc
#update_submodule src/linux-shark
--- /dev/null
+Subproject commit 0828c69ad805406568f6eef2bffbbd54b04dea41
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+bin_PROGRAMS = barcode
+
+barcode_SOURCES = barcode.c
+
+include_HEADERS = barcode.h
+
+lib_LOADLIBES = pthread roboorte robottype orte sercom
--- /dev/null
+#include <stdio.h>
+#include <unistd.h>
+#include <sercom.h>
+#include <string.h>
+#include <pthread.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <termios.h>
+#include "barcode.h"
+
+
+static struct sercom_data sercom;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+
+typedef enum {
+ AIM_OFF,
+ AIM_ON,
+ BEEP,
+ CMD_ACK,
+ CMD_NAK,
+ LED_OFF,
+ LED_ON,
+ PARAM_DEFAULTS,
+ PARAM_REQUEST,
+ PARAM_SEND,
+ REQUEST_REVISION,
+ SCAN_DISABLE,
+ SCAN_ENABLE,
+ SLEEP,
+ START_DECODE,
+ STOP_DECODE,
+ WAKEUP,
+ CUSTOM_DEFAULTS
+} commandes;
+
+
+unsigned char getOpcode(commandes command)
+{
+ switch (command)
+ {
+ case AIM_OFF:
+ return 0xC4;
+ break;
+ case AIM_ON:
+ return 0xC5;
+ break;
+ case BEEP:
+ return 0xE6;
+ break;
+ case CMD_ACK:
+ return 0xD0;
+ break;
+ case CMD_NAK:
+ return 0xD1;
+ break;
+ case LED_OFF:
+ return 0xE8;
+ break;
+ case LED_ON:
+ return 0xE7;
+ break;
+ case PARAM_DEFAULTS:
+ return 0xC8;
+ break;
+ case PARAM_REQUEST:
+ return 0xC7;
+ break;
+ case PARAM_SEND:
+ return 0xC6;
+ break;
+ case REQUEST_REVISION:
+ return 0xA3;
+ break;
+ case SCAN_DISABLE:
+ return 0xEA;
+ break;
+ case SCAN_ENABLE:
+ return 0xE9;
+ break;
+ case SLEEP:
+ return 0xEB;
+ break;
+ case START_DECODE:
+ return 0xE4;
+ break;
+ case STOP_DECODE:
+ return 0xE5;
+ break;
+ case WAKEUP:
+ return 0x00;
+ break;
+ case CUSTOM_DEFAULTS:
+ return 0x12;
+ break;
+ }
+}
+
+int send_command(commandes command, unsigned char *data, int dataLength)
+{
+ unsigned char length;
+ int i; //pomocne cyklitko
+
+ if (data==NULL) {
+ length=7;
+ }
+ else {
+ length=dataLength+6;
+ printf("\ndelka paketu je %d\n", length);
+ }
+
+ unsigned char message[length];
+
+ //delka zpravy
+ message[0]=length-2;
+ //Opcode
+ message[1]=getOpcode(command);
+ //odesilatel
+ message[2]=0x04;
+ //status
+ message[3]=0x00; //to se musi najit, co jaky bit dela
+
+ if (data!=NULL) {
+ //nahraju zpravu
+ for (i=0; i<dataLength; i++)
+ message[4+i]=data[i];
+ }
+ else {
+ //necham prazdna data
+ message[4]=0x00;
+ }
+
+ //spocte checksum
+ count_checksum(message, length);
+
+
+
+ printf("\ncelkovy paket je:\n");
+ for (i=0; i<length; i++)
+ printf("0x%0.2X ", message[i]);
+ printf("\n");
+
+ barcode_write(message, length);
+
+
+ return 0;
+}
+
+void count_checksum(unsigned char *message, int length)
+{
+ //to jako nevim jak spocitat
+ message[length-1]=0xAA;
+ message[length-2]=0xBB;
+}
+
+
+int barcode_sercom_init(char * tty, void(*sighandler)(int))
+{
+ int ret;
+ strcpy((char *)&sercom.devname, tty);
+ sercom.baudrate = 9600;
+ sercom.parity = SERCOM_PARNONE;
+ sercom.databits = 8;
+ sercom.stopbits = 1;
+ sercom.mode = 0;
+ sercom.sighandler = NULL;
+ ret = sercom_open(&sercom);
+ if (ret < 0) {
+ return -1;
+ }
+ else {
+#define EBTEST
+#ifdef EBTEST
+ /* this part is used for testing purposess with ebboard,
+ remove this when conected to real scanner */
+ /* toogle DTR and RTS signal to drive the ebboard from reset */
+ int status;
+
+ ioctl(sercom.fd, TIOCMGET, &status); /* get the serial port status */
+
+ status &= ~TIOCM_DTR;
+ status &= ~TIOCM_RTS;
+
+ ioctl(sercom.fd, TIOCMSET, &status); /* set the serial port status */
+#endif
+ printf("Open serial connection to barcode scanner!");
+ return 0;
+ }
+}
+
+
+int barcode_read(unsigned char *buff, int size)
+{
+ int ret;
+ pthread_mutex_lock(&mutex);
+ ret = read(sercom.fd, buff, size);
+ pthread_mutex_unlock(&mutex);
+
+ if (ret != size) {
+ printf("READ FAILED!\n");
+ return -1;
+ }
+ return ret;
+}
+
+int barcode_write(unsigned char *buff, int size)
+{
+ int ret;
+ int i,j;
+ pthread_mutex_lock(&mutex);
+ printf("sending command (%2d bytes): ", size);
+
+
+ ret = write(sercom.fd, buff, size);
+ pthread_mutex_unlock(&mutex);
+ if (ret != size) {
+ printf("uoled: WRITE FAILED!\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+int main(void)
+{
+ char rcmd[4];
+
+ barcode_sercom_init("/dev/ttyUSB0", NULL);
+
+
+ send_command(AIM_OFF, NULL, 0);
+
+
+ //cekam na odpoved, coz je delka prijimane zpravy
+ barcode_read(rcmd,1);
+
+ printf("\ndelka nadchazejici zpravy 0x%0.2X\n", rcmd[0]);
+
+
+
+ return 0;
+
+ while (1) {
+ barcode_read(rcmd, 4);
+ putchar(rcmd[0]);
+ putchar(rcmd[1]);
+ putchar(rcmd[2]);
+ putchar('\n');
+
+ //printf("%s", rcmd);
+ }
+
+ return 0;
+}
{
ui->setupUi(this);
+ /* create the window without the title bar */
+ Qt::WindowFlags flags = this->windowFlags();
+ flags |= Qt::FramelessWindowHint;
+ this->setWindowFlags(flags);
+
+ //cursor will be hidden
+ setCursor(QCursor(Qt::BlankCursor));
+
//na zacatku nazname polohu
this->pos.positionIsActual=false;
vyvolan signal pro pohnuti tocitka
*/
- //pomocna promenna pro ASCII tocitko
- aliveState=0;
-
QTimer *timer = new QTimer(this);
timer->start(500);
connect(timer, SIGNAL(timeout()), this, SLOT(alive()));
void DisplayQT::paintEvent(QPaintEvent *)
{
//umisteni stredu kompasu
- int const x=405;
- int const y=105;
+ int const x=410;
+ int const y=95;
int const dimension=80;
- static const QPoint hourHand[3] = {
+ static const QPoint arrow[3] = {
QPoint(4, 4),
QPoint(-4, 4),
QPoint(0, -((dimension/2)-15))
//namalovani sipky p.t.k. je aktualni pozice
if(pos.positionIsActual){
painter.rotate(90-pos.phi);
- painter.drawConvexPolygon(hourHand, 3);
+ painter.drawConvexPolygon(arrow, 3);
}
painter.restore();
void DisplayQT::alive(void)
{
+ static char aliveState=0;
if(++aliveState==4)
aliveState=0;
private:
- char aliveState;
char teamColor;
struct position
<x>0</x>
<y>0</y>
<width>480</width>
- <height>282</height>
+ <height>272</height>
</rect>
</property>
<property name="windowTitle">
<property name="geometry">
<rect>
<x>10</x>
- <y>80</y>
+ <y>70</y>
<width>41</width>
<height>61</height>
</rect>
<widget class="QLabel" name="team_color_label">
<property name="geometry">
<rect>
- <x>0</x>
- <y>60</y>
+ <x>10</x>
+ <y>50</y>
<width>81</width>
<height>17</height>
</rect>
<property name="geometry">
<rect>
<x>10</x>
- <y>160</y>
+ <y>140</y>
<width>321</width>
<height>67</height>
</rect>
<property name="geometry">
<rect>
<x>0</x>
- <y>260</y>
+ <y>250</y>
<width>481</width>
<height>21</height>
</rect>
<property name="geometry">
<rect>
<x>350</x>
- <y>160</y>
+ <y>140</y>
<width>121</width>
<height>94</height>
</rect>
\page compilation How to compile it
+\section Prerequisites
+
+To compile the software, you need the following programs and libraries.
+- python
+- pkg-config
+- libidl
+- libpopt
+- OpenCV
+- libfft
+- Qt version 4.x
+- libusb
+
+On a Debian-based system, it is sufficient to run
+\verbatim
+apt-get install build-essential python pkg-config libidl-dev libpopt-dev libcv-dev libhighgui-dev libfftw3-dev qt4-dev-tools libusb-dev
+\endverbatim
+
+\section Preparation
+
Before first compilation, you should execute
<tt>build/prepare_infrastructure script</tt>.
\verbatim
--- /dev/null
+# 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_vidle
+
+eb_vidle_SOURCES = main.c fsm.c fsm_vidle.c uar.c vhn.c
+eb_vidle_LIBS = can ebb
+
+link_VARIANTS = flash
--- /dev/null
+#ifndef DEFH
+#define DEFH
+
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
+
+#endif
--- /dev/null
+#include "fsm.h"
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state)
+{
+ fsm->current_state = initial_state;
+ fsm->current_state(fsm, EVENT_ENTRY);
+}
+
+void run_fsm(struct fsm *fsm){
+ fsm->last_state = fsm->current_state; // set actual state
+ fsm->current_state(fsm, EVENT_DO); // change parameter
+
+ if(fsm->last_state != fsm->current_state){ // if state was changed
+ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
+ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// events of each state of state machine
+enum event {
+ EVENT_ENTRY,
+ EVENT_DO,
+ EVENT_EXIT
+};
+
+struct fsm;
+
+typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr
+
+struct fsm {
+ state_fcn current_state; // current state
+ state_fcn last_state; // last state
+ int32_t act_pos; // actual position
+ int32_t req_pos; // requested position
+ int32_t req_spd;
+ int32_t req_target;
+ volatile int32_t can_req_spd;
+ volatile uint32_t can_req_position; // next requested position
+ int32_t start_pos;
+ uint32_t can_response; // when the move is done, the value here equals to the req_pos
+ uint8_t flags; //< CAN flags bits (defined in can_msg_def.h)
+ uint32_t time_start; /* For timeout detection */
+ bool trigger_can_send;
+};
+
+void init_fsm(struct fsm *fsm, state_fcn initial_state);
+void run_fsm(struct fsm *fsm);
+
+#endif
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+#include <adc.h>
+
+#define DBG_ENTRY() do { \
+ send_rs_str(__func__); \
+ send_rs_str(": entry\n"); \
+ } while(0);
+
+static void wait_for_cmd(struct fsm *fsm, enum event event);
+static void move(struct fsm *fsm, enum event event);
+
+
+void fsm_vidle_init(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->can_req_position = fsm->act_pos;
+ fsm->flags |= CAN_VIDLE_INITIALIZING;
+ break;
+ case EVENT_DO:
+ /* TODO: Homing */
+ fsm->flags &= ~CAN_VIDLE_INITIALIZING;
+ fsm->current_state = wait_for_cmd;
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void stop()
+{
+ engine_A_pwm(0);
+ engine_A_en(ENGINE_EN_OFF);
+}
+
+
+#define DEAD_ZONE 10
+static bool do_control(struct fsm *fsm, int P)
+{
+ bool finished;
+ int e = fsm->req_pos - fsm->act_pos;
+ int action = (P*e) / 10; // ORIGINAL: int action = P*e;
+
+ engine_A_dir(action < 0);
+//#define BANG_BANG
+#ifdef BANG_BANG
+ action = action > 0 ? action : -action;
+ action = action > 40 ? 100 : 0;
+#else
+ action = action > 0 ? action : -action;
+#endif
+ engine_A_pwm(action);
+ engine_A_en(ENGINE_EN_ON);
+
+
+ if (fsm->req_target > fsm->start_pos)
+ finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
+ else
+ finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
+
+ return finished;
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ static int last_can_req_pos = 0;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ do_control(fsm, 2);
+ if (fsm->can_req_position != last_can_req_pos &&
+ fsm->can_req_position != fsm->req_target) {
+ last_can_req_pos = fsm->can_req_position;
+ fsm->req_target = fsm->can_req_position;
+ fsm->req_spd = fsm->can_req_spd;
+ fsm->current_state = move;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
+static void move(struct fsm *fsm, enum event event)
+{
+ static int counter;
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ counter = 0;
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->start_pos = fsm->act_pos;
+ if(fsm->req_spd == 0)
+ fsm->req_pos = fsm->req_target;
+ else
+ fsm->req_pos = fsm->start_pos;
+ break;
+ case EVENT_DO:
+ if (fsm->can_req_position != fsm->req_target) {
+ fsm->flags |= CAN_VIDLE_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+ if(fsm->req_spd != 0 && counter++ >= 10)
+ {
+ counter = 0;
+ if(fsm->req_target > fsm->start_pos) {
+ fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+ } else {
+ fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+ }
+ }
+ if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
+ fsm->flags |= CAN_VIDLE_TIMEOUT;
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ fsm->req_pos = fsm->act_pos;
+ }
+ finished = do_control(fsm, fsm->req_spd ? 2 : 2);
+ if (finished && fsm->req_pos == fsm->req_target) {
+ fsm->can_response = fsm->req_target;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop();
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
--- /dev/null
+
+/**
+ * @file main.c
+ *
+ *
+ * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
+ *
+ * @addtogroup fork
+ */
+
+
+/**
+ * @defgroup fork Vidle (fork) application
+ */
+/**
+ * @ingroup fork
+ * @{
+ */
+
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <types.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#include <adc.h>
+#include "vhn.h"
+
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+
+#define ADC_ISR 1
+
+#define TIMER_IRQ_PRIORITY 5
+
+
+struct fsm fsm_jaws;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+
+
+
+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); // STOP pwm is in percent, range 0~100~200
+
+ /* vhn_init(); */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+ /* set interrupt vector */
+ ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+ ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+ /* enable interrupt */
+ VICIntEnable = 1<<source;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+/** timer0 & ISR **/
+
+void timer0_irq() __attribute__((interrupt));
+volatile uint32_t timer_msec = 0, timer_usec = 0;
+
+void init_timer0(uint32_t prescale, uint32_t period) {
+ T0TCR = 0x2; /* reset */
+ T0PR = prescale - 1;
+ T0MR0 = period;
+ T0MCR = 0x3; /* match0: reset & irq */
+ T0EMR = 0; /* no ext. match */
+ T0CCR = 0x0; /* no capture */
+ T0TCR = 0x1; /* go! */
+}
+
+void timer0_irq() {
+ static unsigned cnt1khz = 0;
+
+ /* reset timer irq */
+ T0IR = -1;
+
+ /* increment timer_usec */
+ timer_usec += 10;
+ /* increment msec @1kHz */
+ if (++cnt1khz == 100) {
+ cnt1khz = 0;
+ ++timer_msec;
+ }
+
+ /* int acknowledge */
+ VICVectAddr = 0;
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+// #define START_PIN 15 // start pin
+// #define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+// #define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+// #define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
+
+// void start_button(void)
+// {
+// can_msg_t msg;
+// bool start_condition;
+// static bool last_start_condition = 0;
+//
+// static int count = 0;
+// static uint32_t next_send = 0;
+//
+//
+// start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+//
+// if (start_condition != last_start_condition) {
+// last_start_condition = start_condition;
+// count = 0;
+// next_send = timer_msec; /* Send now */
+// }
+//
+// if (timer_msec >= next_send) {
+// msg.id = CAN_ROBOT_CMD;
+// msg.flags = 0;
+// msg.dlc = 1;
+// msg.data[0] = start_condition;
+// /*while*/ (can_tx_msg(&msg));
+//
+// if (count < START_SEND_FAST_COUNT) {
+// count++;
+// next_send = timer_msec + START_SEND_PRIOD_FAST;
+// } else
+// next_send = timer_msec + START_SEND_PRIOD_SLOW;
+// }
+//
+//
+// }
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ uint32_t spd;
+ can_msg_t rx_msg;
+ uint32_t req =0;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
+
+
+ deb_led_on(LEDB);
+
+ switch (rx_msg.id)
+ {
+ case CAN_VIDLE_CMD:
+ deb_led_on(LEDB);
+ req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+ spd = rx_msg.data[2];
+
+ if (req >= 0x150 && req <= 0x3e0) {
+ fsm_jaws.flags &= ~CAN_JAWS_OUT_OF_BOUNDS;
+ fsm_jaws.can_req_position = req;// save new req position of lift
+ fsm_jaws.can_req_spd = spd;// save new req spd of lift
+ } else
+ fsm_jaws.flags |= CAN_JAWS_OUT_OF_BOUNDS;
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+ init_motors();
+
+ /* init timer0 */
+ init_timer0(1, CPU_APB_HZ/100000);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
+
+ init_uart();
+ init_adc(ADC_ISR);
+
+
+}
+/*********************************************************/
+void can_send_status(void)
+{
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_JAWS_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = (fsm_jaws.act_pos >> 8) & 0xFF;
+ tx_msg.data[1] = fsm_jaws.act_pos & 0xFF;
+ tx_msg.data[2] = (fsm_jaws.can_response >> 8) & 0xFF;
+ tx_msg.data[3] = fsm_jaws.can_response & 0xFF;
+ tx_msg.data[4] = fsm_jaws.flags;
+ /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+void dbg_print_time()
+{
+ char str[10];
+ unsigned t = timer_usec, i;
+ memset(str, ' ', sizeof(str));
+ str[9] = 0;
+ str[8] = '\n';
+ for (i=7; t > 0; i--) {
+ str[i] = t%10 + '0';
+ t /= 10;
+ }
+ send_rs_str(str);
+}
+
+void fsm_jaws_init(struct fsm *fsm, enum event event);
+
+
+void blink_led()
+{
+ static uint32_t led_time = 0;
+
+ if(timer_msec >= led_time + 500)
+ {
+ led_time = timer_msec;
+ /* static int up;
+ if (up == 0)
+ fsm_vidle.can_req_position = 0x380;
+ if (up == 6)
+ fsm_vidle.can_req_position = 0x1e0;
+ up = (up+1)%12;
+ */
+ deb_led_change(LEDG);
+ send_rs_int(fsm_jaws.act_pos);
+ send_rs_str("\n");
+ }
+}
+
+
+// #define BUMPER_PIN 17 // bumper pin (SCK1/P0_17)
+// #define COLOR_PIN 3 // change color of dress pin (SDA1/P0_3)
+//
+// #define BUMPER_LEFT 19 // left bumper MOSI1/P0_19
+// #define BUMPER_RIGHT 9 // right bumper RXD1/P0_9
+
+#define BUMPER_REAR 17 // rear bumper in the lift (SCK1/P0_17)
+
+#define BUMPER_LEFT 19 // left bumper MOSI1/P0_19
+#define BUMPER_RIGHT 9 // right bumper RXD1/P0_9
+
+#define BUMPER_LEFT_CORNER 16 // left bumper in rear corner
+#define BUMPER_RIGHT_CORNER 13 // right bumper in rear corner
+
+void handle_bumper()
+{
+ static uint32_t bumper_time = 0;
+ char sw = 0;
+
+ if (timer_msec >= bumper_time + 100)
+ {
+ can_msg_t tx_msg;
+
+ bumper_time = timer_msec;
+
+
+
+ if (IO0PIN & (1<<BUMPER_REAR))
+ sw &= ~CAN_SWITCH_BUMPER;
+ else
+ sw |= CAN_SWITCH_BUMPER;
+
+ if (IO0PIN & (1<<BUMPER_LEFT_CORNER))
+ sw |= CAN_SWITCH_CORNER_LEFT;
+ else
+ sw &= ~CAN_SWITCH_CORNER_LEFT;
+
+ if (IO0PIN & (1<<BUMPER_RIGHT_CORNER))
+ sw |= CAN_SWITCH_CORNER_RIGHT;
+ else
+ sw &= ~CAN_SWITCH_CORNER_RIGHT;
+
+ if (IO0PIN & (1<<BUMPER_LEFT))
+ sw &= ~CAN_SWITCH_LEFT;
+ else
+ sw |= CAN_SWITCH_LEFT;
+
+ if (IO0PIN & (1<<BUMPER_RIGHT))
+ sw &= ~CAN_SWITCH_RIGHT;
+ else
+ sw |= CAN_SWITCH_RIGHT;
+
+// if (sw & CAN_SWITCH_COLOR)
+// deb_led_off(LEDY);
+// else
+// deb_led_on(LEDY);
+
+ if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT|BUMPER_RIGHT_CORNER|BUMPER_LEFT_CORNER))
+ deb_led_on(LEDR);
+ else
+ deb_led_off(LEDR);
+
+ /* TODO: Put color to the second bit */
+
+ tx_msg.id = CAN_ROBOT_SWITCHES;
+ tx_msg.dlc = 1;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = sw;
+
+ can_tx_msg(&tx_msg);
+ }
+}
+
+// void test_vhn()
+// {
+// vhn_init();
+//
+// char dir = ENGINE_DIR_BW;
+// int spd = 0;
+// while(1) {
+// spd = (spd+1)%100;
+// if (spd == 0) {
+// dir = !dir;
+// vhn_reset();
+// }
+// vhn_speed(spd, dir);
+// send_rs_int(spd);
+// uart_send_char(' ');
+// send_rs_int(dir);
+// send_rs_str(" Vidle started\n");
+// /* char ch = uart_get_char(); */
+// /* switch (ch) { */
+// /* case '-': */
+// /* dir = !dir; */
+// /* break; */
+// /* case '0': */
+// /* vhn_speed(0, dir); */
+// /* break; */
+// /* case '1': */
+// /* vhn_speed(0x40, dir); */
+// /* break; */
+// /* case '2': */
+// /* vhn_speed(0x80, dir); */
+// /* break; */
+// /* case '3': */
+// /* vhn_speed(0xa0, dir); */
+// /* break; */
+// /* default: */
+// /* ch='?'; */
+// /* } */
+// /* uart_send_char(ch); */
+// // blink_led();
+// }
+//
+// }
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t status_time = timer_usec;
+
+
+ init_periphery();
+
+ /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09 */
+ //SET_PIN(PINSEL1, 1, PINSEL_0);
+ //SET_PIN(PINSEL1, 3, PINSEL_0);
+
+
+
+
+// SET_PIN(PINSEL0, START_PIN, PINSEL_0); // inicializace start pinu
+ SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, 1, PINSEL_0); // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
+
+ SET_PIN(PINSEL1, 3, PINSEL_0);
+ SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
+
+
+// IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
+ IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));
+
+ //IO1DIR &= ~(3<<20);
+
+ send_rs_str("Jaws started\n");
+ // The above send_rs_str is importat - we wait for the first AD conversion to be finished
+ fsm_jaws
+.act_pos = adc_val[0];
+ init_fsm(&fsm_jaws
+, &fsm_jaws_init
+);
+
+/* test_vhn(); */
+/* return; */
+
+ while(1){
+ if(timer_usec >= main_time + 1000)
+ {
+ main_time = timer_usec;
+
+ //dbg_print_time();
+
+ fsm_jaws
+.act_pos = adc_val[0];
+
+
+ run_fsm(&fsm_jaws
+);
+ }
+
+ if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
+ fsm_jaws
+.trigger_can_send) { //or when something important happen
+ fsm_jaws
+.trigger_can_send = false;
+ status_time = timer_msec; //save new time, when message was sent
+ can_send_status();
+
+ }
+
+// start_button();
+ handle_bumper();
+
+ blink_led();
+ }
+}
+
+/** @} */
--- /dev/null
+#include <uart.h>
+
+
+
+
+/**
+ * Send one char to uart.
+ */
+void uart_send_char(char val)
+{
+ uart0SendCh(val);
+}
+
+/**
+ * Read one char from uart.
+ */
+char uart_get_char(void)
+{
+ return uart0GetCh();
+}
+
+/**
+ * Initialize UART - platform dependent
+ */
+void init_uart(void)
+{
+ init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
+}
+
+
+/**
+ * Send string to serial output in ASCII code. .
+ * @param data[] string to print
+ */
+void send_rs_str(const char data[])
+{
+
+ int i = 0;
+ int j = 0;
+
+ for (j = 0; j < 255; j++)
+ {
+ if(data[j] == 0) break;
+ }
+
+ for (i= 0 ; i < j; i++)
+ {
+ uart_send_char(data[i]);
+ }
+}
+
+/**
+ * Send int value to serial output in ASCII code. Removes unused zeros.
+ * @param val value to print
+ */
+void send_rs_int(int val)
+{
+ char dat[8];
+ int i;
+ int pom = 0;
+
+ for(i = 0; i < 8; i++)
+ {
+ dat[i] = (val & 0xF) + 0x30;
+ if(dat[i] > '9')
+ dat[i] += 7;
+ val >>= 4;
+ }
+
+ for(i = 0; i < 8; i++)
+ {
+ if((pom == 0) & (dat[7-i] == '0'))
+ {
+ if((i == 6) | (i == 7))
+ uart_send_char('0');
+ continue;
+ }
+ pom = 1;
+ uart_send_char(dat[7-i]);
+ }
+
+}
--- /dev/null
+#ifndef _UAR_H
+#define _UAR_H
+
+
+void send_rs_int(int val);
+void send_rs_str(const char data[]);
+void init_uart(void);
+char uart_get_char(void);
+void uart_send_char(char val);
+
+
+#endif
+
--- /dev/null
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "engine.h"
+#include "uar.h"
+#include "def.h"
+#include "vhn.h"
+
+
+void vhn_init(void)
+{
+ init_engine_B();
+ PINSEL2 &= ~(3<<2);
+ IO1DIR |= (VHN_EN_A | VHN_EN_B | VHN_IN_A | VHN_IN_B);
+ IO1SET = (VHN_EN_A | VHN_EN_B | VHN_IN_A | VHN_IN_B);
+}
+
+void vhn_speed(uint8_t speed, uint8_t dir) {
+
+ if(speed == 0)
+ {
+ IO1CLR = (VHN_IN_A | VHN_IN_B);
+ return;
+ }
+
+ if(dir == ENGINE_DIR_FW)
+ {
+ IO1SET = VHN_IN_A;
+ IO1CLR = VHN_IN_B;
+ }
+ else
+ {
+ IO1CLR = VHN_IN_A;
+ IO1SET = VHN_IN_B;
+ }
+
+ //IO1CLR = (VHN_EN_A | VHN_EN_A);
+ IO1SET = (VHN_EN_A | VHN_EN_A);
+ engine_B_pwm(100-speed);
+
+}
+
+void vhn_reset()
+{
+ volatile int i = 10000;
+ deb_led_on(LEDR);
+ IO1CLR = VHN_IN_A|VHN_IN_B;
+ while(i--);
+ //while (IO1PIN & (VHN_EN_A | VHN_EN_B) != (VHN_EN_A | VHN_EN_B));
+ IO1SET = VHN_IN_A|VHN_IN_B;
+ deb_led_off(LEDR);
+}
--- /dev/null
+/* VHN2SP30 H-bridge control */
+
+#ifndef __VHN_H
+#define __VHN_H
+
+ #include "def.h"
+
+ #define VHN_EN_A (1<<16) /* P1.16 = GPIO1 = GPIO port 2 */
+ #define VHN_EN_B (1<<17) /* P1.17 = GPIO2 = GPIO port 1 */
+ #define VHN_IN_A (1<<18) /* P1.18 = GPIO3 = GPIO port 4 */
+ #define VHN_IN_B (1<<19) /* P1.19 = GPIO4 = GPIO port 3 */
+
+
+ void vhn_init(void);
+ void vhn_reset(void);
+ void vhn_speed(uint8_t speed, uint8_t dir);
+
+
+
+
+
+#endif /*__VHN_H*/
+/*
+ * 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 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");
+/*
+ * 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");
-Subproject commit dcb49770b8aaa099920fd387b96203811788b8d7
+Subproject commit 9f5ccc0b6b6e5360437710eec58f5fd9a6744bec