From f25899b41e109ed0de31327386a8c6763a9fbef5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 23 Mar 2013 23:07:02 +0100 Subject: digital/io-hub/src/apbirthday: add input/output commands --- digital/io-hub/src/apbirthday/Makefile | 1 + digital/io-hub/src/apbirthday/hardware.hh | 2 + digital/io-hub/src/apbirthday/robot.cc | 86 ++++++++++++++++++++++++++++++- digital/io-hub/src/apbirthday/robot.hh | 9 ++++ digital/io-hub/src/common-cc/outputs.cc | 69 +++++++++++++++++++++++++ digital/io-hub/src/common-cc/outputs.hh | 60 +++++++++++++++++++++ 6 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/common-cc/outputs.cc create mode 100644 digital/io-hub/src/common-cc/outputs.hh (limited to 'digital') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 96766788..8e27941c 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -4,6 +4,7 @@ TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ i2c_queue.cc asserv.cc chrono.host.cc chrono.stm32.cc \ + outputs.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/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 6660156e..3cf5a50f 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -82,6 +82,8 @@ struct Hardware gift_out, gift_in, ballon_funny_action, pneum_open; + static const int inputs_nb = 9; + static const int outputs_nb = 21; Hardware (); // Wait until next cycle. void wait (); diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 1873c7f2..87b08b18 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -37,10 +37,48 @@ Robot::Robot () zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb), chrono (90000 - 1000), + outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), - stats_chrono_ (false), stats_chrono_last_s_ (-1) + stats_chrono_ (false), stats_chrono_last_s_ (-1), + stats_inputs_ (0) { robot = this; + unsigned i = 0; + inputs_[i++] = &hardware.raw_jack; + inputs_[i++] = &hardware.ihm_color; + inputs_[i++] = &hardware.ihm_strat; + inputs_[i++] = &hardware.ihm_robot_nb; + inputs_[i++] = &hardware.ihm_lol; + inputs_[i++] = &hardware.ihm_emerg_stop; + inputs_[i++] = &hardware.glass_contact; + inputs_[i++] = &hardware.cherry_plate_left_contact; + inputs_[i++] = &hardware.cherry_plate_right_contact; + ucoo::assert (i == lengthof (inputs_)); + i = 0; + outputs_[i++] = &hardware.cherry_bad_out; + outputs_[i++] = &hardware.cherry_bad_in; + outputs_[i++] = &hardware.cherry_plate_up; + outputs_[i++] = &hardware.cherry_plate_down; + outputs_[i++] = &hardware.cherry_plate_clamp; + outputs_[i++] = &hardware.cake_arm_out; + outputs_[i++] = &hardware.cake_arm_in; + outputs_[i++] = &hardware.cake_push_far_out; + outputs_[i++] = &hardware.cake_push_far_in; + outputs_[i++] = &hardware.cake_push_near_out; + outputs_[i++] = &hardware.cake_push_near_in; + outputs_[i++] = &hardware.glass_lower_clamp_close; + outputs_[i++] = &hardware.glass_lower_clamp_open; + outputs_[i++] = &hardware.glass_upper_clamp_close; + outputs_[i++] = &hardware.glass_upper_clamp_open; + outputs_[i++] = &hardware.glass_upper_clamp_up; + outputs_[i++] = &hardware.glass_upper_clamp_down; + outputs_[i++] = &hardware.gift_out; + outputs_[i++] = &hardware.gift_in; + outputs_[i++] = &hardware.ballon_funny_action; + outputs_[i++] = &hardware.pneum_open; + ucoo::assert (i == lengthof (outputs_)); + for (i = 0; i < lengthof (outputs_); i++) + outputs_[i]->output (); } void @@ -50,6 +88,7 @@ Robot::main_loop () { // Wait until next cycle. hardware.wait (); + outputs_set_.update (); // Handle communications. bool sync = main_i2c_queue_.sync (); // Handle events if synchronised. @@ -120,6 +159,34 @@ 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 ('o', 5): + // Set/clear/toggle outputs. + // - 1d: mask. + // - 1b: 00 to clear, 01 to set, 02 to toggle. + { + uint32_t mask = + ucoo::bytes_pack (args[0], args[1], args[2], args[3]); + if (args[4] < 3) + outputs_set_.command (Outputs::Command (args[4]), mask); + else + { + proto.send ('?'); + return; + } + } + break; + case c ('o', 6): + // Toggle outputs for a short time. + // - 1d: mask. + // - 1w: duration. + { + uint32_t mask = + ucoo::bytes_pack (args[0], args[1], args[2], args[3]); + outputs_set_.command (Outputs::TOGGLE, mask); + outputs_set_.command_later (Outputs::TOGGLE, mask, + ucoo::bytes_pack (args[4], args[5])); + } + break; case c ('C', 1): // Chrono stats. // 1B: start chrono if non-zero. @@ -128,6 +195,12 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size chrono.start (); stats_proto_ = &proto; break; + case c ('I', 1): + // Input stats. + // 1B: stat interval. + stats_inputs_cpt_ = stats_inputs_ = args[0]; + stats_proto_ = &proto; + break; default: proto.send ('?'); return; @@ -150,5 +223,16 @@ Robot::proto_stats () stats_chrono_last_s_ = s; } } + if (stats_inputs_ && !--stats_inputs_cpt_) + { + uint32_t inputs = 0; + for (unsigned int i = 0; i < lengthof (inputs_); i++) + { + if (inputs_[i]->get ()) + inputs |= 1 << i; + } + stats_proto_->send ('I', "L", inputs); + stats_inputs_cpt_ = stats_inputs_; + } } diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 08cb9cf6..f7d5e57a 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -27,6 +27,7 @@ #include "asserv.hh" #include "fsm_queue.hh" #include "chrono.hh" +#include "outputs.hh" #include "ucoolib/base/proto/proto.hh" @@ -62,12 +63,20 @@ class Robot : public ucoo::Proto::Handler /// Public access to chrono. Chrono chrono; private: + /// All inputs. + ucoo::Io *inputs_[Hardware::inputs_nb]; + /// All outputs. + ucoo::Io *outputs_[Hardware::outputs_nb]; + /// Handle set of outputs. + Outputs outputs_set_; /// Proto used for stats. ucoo::Proto *stats_proto_; /// Enable chrono stats. bool stats_chrono_; /// Last stated second. int stats_chrono_last_s_; + /// Input stats interval and counter. + int stats_inputs_, stats_inputs_cpt_; }; /// Global instance pointer. diff --git a/digital/io-hub/src/common-cc/outputs.cc b/digital/io-hub/src/common-cc/outputs.cc new file mode 100644 index 00000000..4fb99b96 --- /dev/null +++ b/digital/io-hub/src/common-cc/outputs.cc @@ -0,0 +1,69 @@ +// 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 "outputs.hh" + +Outputs::Outputs (ucoo::Io *outputs[], int outputs_nb) + : outputs_ (outputs), outputs_nb_ (outputs_nb), + def_delay_ (0) +{ +} + +void +Outputs::command (Command c, uint32_t mask) +{ + for (int i = 0; i < outputs_nb_; i++) + { + if (mask & (1 << i)) + { + switch (c) + { + case RESET: + outputs_[i]->reset (); + break; + case SET: + outputs_[i]->set (); + break; + case TOGGLE: + outputs_[i]->toggle (); + break; + } + } + } +} + +void +Outputs::command_later (Command c, uint32_t mask, int delay) +{ + def_command_ = c; + def_mask_ = mask; + def_delay_ = delay; +} + +void +Outputs::update () +{ + if (def_delay_ && !--def_delay_) + command (def_command_, def_mask_); +} + diff --git a/digital/io-hub/src/common-cc/outputs.hh b/digital/io-hub/src/common-cc/outputs.hh new file mode 100644 index 00000000..6a9992f6 --- /dev/null +++ b/digital/io-hub/src/common-cc/outputs.hh @@ -0,0 +1,60 @@ +#ifndef outputs_hh +#define outputs_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" +#include "ucoolib/intf/io.hh" + +/// Handle a set of outputs. +class Outputs +{ + public: + enum Command { + RESET, + SET, + TOGGLE, + }; + public: + /// Constructor. + Outputs (ucoo::Io *outputs[], int outputs_nb); + /// Apply command on a set of output now. + void command (Command c, uint32_t mask); + /// Apply deferred command, can only be one active. + void command_later (Command c, uint32_t mask, int delay); + /// To be called at each cycle to handle deferred commands. + void update (); + private: + /// Output table. + ucoo::Io **outputs_; + /// Output table size. + int outputs_nb_; + /// Deferred command. + Command def_command_; + /// Deferred mask. + uint32_t def_mask_; + /// Remaining delay until deferred command, or 0 if not active. + int def_delay_; +}; + +#endif // outputs_hh -- cgit v1.2.3