From 06baa6c156dba0be37a21bddee96994a2c44f5bd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 23 Mar 2013 12:27:37 +0100 Subject: digital/io-hub/src/apbirthday: add chrono --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/robot.cc | 31 ++++++++++++++++- digital/io-hub/src/apbirthday/robot.hh | 12 +++++++ digital/io-hub/src/common-cc/chrono.hh | 48 +++++++++++++++++++++++++ digital/io-hub/src/common-cc/chrono.host.cc | 48 +++++++++++++++++++++++++ digital/io-hub/src/common-cc/chrono.stm32.cc | 52 ++++++++++++++++++++++++++++ digital/io-hub/src/common-cc/init.cc | 2 +- 7 files changed, 192 insertions(+), 3 deletions(-) create mode 100644 digital/io-hub/src/common-cc/chrono.hh create mode 100644 digital/io-hub/src/common-cc/chrono.host.cc create mode 100644 digital/io-hub/src/common-cc/chrono.stm32.cc (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index d0567efd..96766788 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -3,7 +3,7 @@ BASE = ../../../ucoolib TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ - i2c_queue.cc asserv.cc \ + i2c_queue.cc asserv.cc chrono.host.cc chrono.stm32.cc \ top.cc init.cc move.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ $(AVR_SOURCES) diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index cfdc0c19..1873c7f2 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -35,7 +35,10 @@ Robot::Robot () asserv (main_i2c_queue_, BOT_SCALE), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), - usb_proto (*this, hardware.usb) + usb_proto (*this, hardware.usb), + chrono (90000 - 1000), + stats_proto_ (0), + stats_chrono_ (false), stats_chrono_last_s_ (-1) { robot = this; } @@ -56,6 +59,8 @@ Robot::main_loop () dev_proto.accept (); zb_proto.accept (); usb_proto.accept (); + // Send stats. + proto_stats (); } } @@ -115,6 +120,14 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size asserv.goto_xy (pos, Asserv::DirectionConsign (args[4])); } break; + case c ('C', 1): + // Chrono stats. + // 1B: start chrono if non-zero. + stats_chrono_ = true; + if (args[0]) + chrono.start (); + stats_proto_ = &proto; + break; default: proto.send ('?'); return; @@ -123,3 +136,19 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size proto.send_buf (cmd, args, size); } +void +Robot::proto_stats () +{ + if (!stats_proto_) + return; + if (stats_chrono_) + { + int s = chrono.remaining_time_ms () / 1000; + if (s != stats_chrono_last_s_) + { + stats_proto_->send ('C', "b", s); + stats_chrono_last_s_ = s; + } + } +} + diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 1c6af976..08cb9cf6 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -26,6 +26,7 @@ #include "hardware.hh" #include "asserv.hh" #include "fsm_queue.hh" +#include "chrono.hh" #include "ucoolib/base/proto/proto.hh" @@ -41,6 +42,8 @@ class Robot : public ucoo::Proto::Handler bool fsm_gen_event (); /// Receive proto messages. void proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size); + /// Send stats. + void proto_stats (); public: /// Public access to hardware class. Hardware hardware; @@ -56,6 +59,15 @@ class Robot : public ucoo::Proto::Handler public: /// Public access to deferred events FSM queue. FsmQueue fsm_queue; + /// Public access to chrono. + Chrono chrono; + private: + /// Proto used for stats. + ucoo::Proto *stats_proto_; + /// Enable chrono stats. + bool stats_chrono_; + /// Last stated second. + int stats_chrono_last_s_; }; /// Global instance pointer. diff --git a/digital/io-hub/src/common-cc/chrono.hh b/digital/io-hub/src/common-cc/chrono.hh new file mode 100644 index 00000000..11c35984 --- /dev/null +++ b/digital/io-hub/src/common-cc/chrono.hh @@ -0,0 +1,48 @@ +#ifndef chrono_hh +#define chrono_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "ucoolib/common.hh" + +/// Match chrono. +class Chrono +{ + public: + /// Constructor, with the match duration. + Chrono (int duration_ms); + /// Start match. + void start (); + /// Get remaining time until end of match, negative if match is over. + int remaining_time_ms () const; + private: + /// Match duration. + int duration_ms_; + /// True if match started yet. + bool started_; +#ifdef TARGET_host + uint32_t start_date_; +#endif +}; + +#endif // chrono_hh diff --git a/digital/io-hub/src/common-cc/chrono.host.cc b/digital/io-hub/src/common-cc/chrono.host.cc new file mode 100644 index 00000000..7d17c576 --- /dev/null +++ b/digital/io-hub/src/common-cc/chrono.host.cc @@ -0,0 +1,48 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "chrono.hh" + +#include "robot.hh" + +Chrono::Chrono (int duration_ms) + : duration_ms_ (duration_ms), started_ (false) +{ +} + +void +Chrono::start () +{ + started_ = true; + start_date_ = robot->hardware.host.get_node ().date (); +} + +int +Chrono::remaining_time_ms () const +{ + int now = 0; + if (started_) + now = robot->hardware.host.get_node ().date () - start_date_; + return duration_ms_ - now; +} + diff --git a/digital/io-hub/src/common-cc/chrono.stm32.cc b/digital/io-hub/src/common-cc/chrono.stm32.cc new file mode 100644 index 00000000..19839503 --- /dev/null +++ b/digital/io-hub/src/common-cc/chrono.stm32.cc @@ -0,0 +1,52 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "chrono.hh" + +#include +#include + +Chrono::Chrono (int duration_ms) + : duration_ms_ (duration_ms), started_ (false) +{ +} + +void +Chrono::start () +{ + started_ = true; + rcc_peripheral_enable_clock (&RCC_APB1ENR, RCC_APB1ENR_TIM2EN); + TIM2_PSC = 2 * rcc_ppre1_frequency / 1000 - 1; // 1 ms prescaler + TIM2_ARR = 3600 * 1000; + TIM2_EGR = TIM_EGR_UG; + TIM2_CR1 = TIM_CR1_OPM | TIM_CR1_CEN; +} + +int +Chrono::remaining_time_ms () const +{ + int now = 0; + if (started_) + now = TIM2_CNT; + return duration_ms_ - now; +} diff --git a/digital/io-hub/src/common-cc/init.cc b/digital/io-hub/src/common-cc/init.cc index 6d8b9255..761e4485 100644 --- a/digital/io-hub/src/common-cc/init.cc +++ b/digital/io-hub/src/common-cc/init.cc @@ -158,7 +158,7 @@ FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success, FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED) { - // TODO: robot->chrono.start (); + robot->chrono.start (); robot->fsm_queue.post (FSM_EVENT (init_start_round)); } -- cgit v1.2.3