From 78d2f3d295c56e976c13b187b7723670411e4101 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 24 Apr 2013 22:44:30 +0200 Subject: digital/io-hub/src/apbirthday, host/simu: add cherries cannon simulation --- digital/ai/tools/test_simu_control_apbirthday.py | 5 +++ digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/robot.cc | 4 ++ digital/io-hub/src/apbirthday/robot.hh | 10 ++++- digital/io-hub/src/common-cc/potentiometer.cc | 51 ---------------------- digital/io-hub/src/common-cc/potentiometer.hh | 42 ------------------ digital/io-hub/src/common-cc/potentiometer.host.cc | 43 ++++++++++++++++++ digital/io-hub/src/common-cc/potentiometer.host.hh | 42 ++++++++++++++++++ .../io-hub/src/common-cc/potentiometer.stm32.cc | 51 ++++++++++++++++++++++ .../io-hub/src/common-cc/potentiometer.stm32.hh | 42 ++++++++++++++++++ digital/io-hub/tools/io_hub/mex.py | 22 +++++++++- host/simu/model/table_eurobot2013.py | 8 ++++ host/simu/robots/apbirthday/link/bag.py | 2 +- host/simu/robots/apbirthday/model/bag.py | 3 +- host/simu/robots/apbirthday/model/cannon.py | 27 +++++++++++- host/simu/robots/apbirthday/view/robot.py | 3 ++ host/simu/view/table_eurobot2013.py | 14 ++++++ 17 files changed, 271 insertions(+), 100 deletions(-) delete mode 100644 digital/io-hub/src/common-cc/potentiometer.cc delete mode 100644 digital/io-hub/src/common-cc/potentiometer.hh create mode 100644 digital/io-hub/src/common-cc/potentiometer.host.cc create mode 100644 digital/io-hub/src/common-cc/potentiometer.host.hh create mode 100644 digital/io-hub/src/common-cc/potentiometer.stm32.cc create mode 100644 digital/io-hub/src/common-cc/potentiometer.stm32.hh diff --git a/digital/ai/tools/test_simu_control_apbirthday.py b/digital/ai/tools/test_simu_control_apbirthday.py index 6260ffa0..403bb9bf 100644 --- a/digital/ai/tools/test_simu_control_apbirthday.py +++ b/digital/ai/tools/test_simu_control_apbirthday.py @@ -61,6 +61,11 @@ class TestSimuControl (TestSimu): out_button ('Push near in/out', 'cake_push_near_in', 'cake_push_near_out') out_button ('Plate arm up/down', 'cherry_plate_up', 'cherry_plate_down') out_button ('Plate clamp', 'cherry_plate_clamp') + cannon_var = IntVar () + def cannon_cmd (): + self.io.potentiometer (0, 256 if cannon_var.get () else 0) + Checkbutton (self.control_frame, text = 'Fire!', indicatoron = 0, + variable = cannon_var, command = cannon_cmd).pack () self.backward_var = IntVar () self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 19296e0f..c07574bd 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -5,7 +5,7 @@ PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ simu_report.host.cc zb_avrisp.stm32.cc \ i2c_queue.cc asserv.cc mimot.cc beacon.cc \ - potentiometer.cc \ + potentiometer.host.cc potentiometer.stm32.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ radar.cc radar_2013.cc obstacles.cc path.cc strat.cc \ outputs.cc \ diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 7137ea3e..e5910d74 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -35,7 +35,11 @@ Robot::Robot () : main_i2c_queue_ (hardware.main_i2c), zb_i2c_queue_ (hardware.zb_i2c), asserv (main_i2c_queue_, BOT_SCALE), mimot (main_i2c_queue_), +#ifdef TARGET_host + pot_regul (hardware.host), +#else pot_regul (main_i2c_queue_, 0x7), +#endif beacon (zb_i2c_queue_), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 556e29eb..3d9a62ce 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -26,7 +26,11 @@ #include "hardware.hh" #include "asserv.hh" #include "beacon.hh" -#include "potentiometer.hh" +#ifdef TARGET_host +# include "potentiometer.host.hh" +#else +# include "potentiometer.stm32.hh" +#endif #include "fsm_queue.hh" #include "chrono.hh" #include "pressure.hh" @@ -72,7 +76,11 @@ class Robot : public ucoo::Proto::Handler /// Public access to mimot class. Mimot mimot; /// Public access to potentiometer class. +#ifdef TARGET_host + PotentiometerHost pot_regul; +#else Potentiometer pot_regul; +#endif /// Public access to beacon class. Beacon beacon; private: diff --git a/digital/io-hub/src/common-cc/potentiometer.cc b/digital/io-hub/src/common-cc/potentiometer.cc deleted file mode 100644 index a5a519bb..00000000 --- a/digital/io-hub/src/common-cc/potentiometer.cc +++ /dev/null @@ -1,51 +0,0 @@ -// 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 "potentiometer.hh" - -Potentiometer::Potentiometer (I2cQueue &queue, int a2a1a0) - : I2cQueue::Slave (queue, 0x50 | (a2a1a0 << 1), 0) -{ -} - -void -Potentiometer::recv_status (const uint8_t *status) -{ -} - -void -Potentiometer::send_command (int mem_addr, int data) -{ - uint8_t buf[] = { - (uint8_t) ((mem_addr << 4) | ((data >> 8) & 1)), - (uint8_t) (data & 0xff), - }; - send (buf, sizeof (buf), I2cQueue::RAW); -} - -void -Potentiometer::set_wiper (int index, int data, bool eeprom) -{ - send_command (index + (eeprom ? 2 : 0), data); -} - diff --git a/digital/io-hub/src/common-cc/potentiometer.hh b/digital/io-hub/src/common-cc/potentiometer.hh deleted file mode 100644 index a7204af8..00000000 --- a/digital/io-hub/src/common-cc/potentiometer.hh +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef potentiometer_hh -#define potentiometer_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 "i2c_queue.hh" - -/// I2C potentiometer, MCP4661. -class Potentiometer : public I2cQueue::Slave -{ - public: - /// Constructor with address variable part. - Potentiometer (I2cQueue &queue, int a2a1a0); - /// See I2cQueue::Slave::recv_status. - void recv_status (const uint8_t *status); - /// Send a low level command. - void send_command (int mem_addr, int data); - /// Set wiper to specified level. - void set_wiper (int index, int data, bool eeprom = false); -}; - -#endif // potentiometer_hh diff --git a/digital/io-hub/src/common-cc/potentiometer.host.cc b/digital/io-hub/src/common-cc/potentiometer.host.cc new file mode 100644 index 00000000..50670e17 --- /dev/null +++ b/digital/io-hub/src/common-cc/potentiometer.host.cc @@ -0,0 +1,43 @@ +// 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 "potentiometer.host.hh" + +PotentiometerHost::PotentiometerHost (ucoo::Host &host) + : node_ (host.get_node ()) +{ + std::string instance (host.get_instance ()); + mtype_ = node_.reserve (instance + ":potentiometer"); +} + +void +PotentiometerHost::set_wiper (int index, int data, bool eeprom) +{ + if (!eeprom) + { + ucoo::mex::Msg msg (mtype_); + msg.push ("BH") << index << data; + node_.send (msg); + } +} + diff --git a/digital/io-hub/src/common-cc/potentiometer.host.hh b/digital/io-hub/src/common-cc/potentiometer.host.hh new file mode 100644 index 00000000..bc6969ff --- /dev/null +++ b/digital/io-hub/src/common-cc/potentiometer.host.hh @@ -0,0 +1,42 @@ +#ifndef potentiometer_host_hh +#define potentiometer_host_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/arch/host/host.hh" +#include "defs.hh" + +/// Potentiometer, Host stub. +class PotentiometerHost +{ + public: + /// Constructor. + PotentiometerHost (ucoo::Host &host); + /// Set wiper to specified level. + void set_wiper (int index, int data, bool eeprom = false); + private: + ucoo::mex::Node &node_; + ucoo::mex::mtype_t mtype_; +}; + +#endif // potentiometer_host_hh diff --git a/digital/io-hub/src/common-cc/potentiometer.stm32.cc b/digital/io-hub/src/common-cc/potentiometer.stm32.cc new file mode 100644 index 00000000..637e2511 --- /dev/null +++ b/digital/io-hub/src/common-cc/potentiometer.stm32.cc @@ -0,0 +1,51 @@ +// 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 "potentiometer.stm32.hh" + +Potentiometer::Potentiometer (I2cQueue &queue, int a2a1a0) + : I2cQueue::Slave (queue, 0x50 | (a2a1a0 << 1), 0) +{ +} + +void +Potentiometer::recv_status (const uint8_t *status) +{ +} + +void +Potentiometer::send_command (int mem_addr, int data) +{ + uint8_t buf[] = { + (uint8_t) ((mem_addr << 4) | ((data >> 8) & 1)), + (uint8_t) (data & 0xff), + }; + send (buf, sizeof (buf), I2cQueue::RAW); +} + +void +Potentiometer::set_wiper (int index, int data, bool eeprom) +{ + send_command (index + (eeprom ? 2 : 0), data); +} + diff --git a/digital/io-hub/src/common-cc/potentiometer.stm32.hh b/digital/io-hub/src/common-cc/potentiometer.stm32.hh new file mode 100644 index 00000000..8683ae27 --- /dev/null +++ b/digital/io-hub/src/common-cc/potentiometer.stm32.hh @@ -0,0 +1,42 @@ +#ifndef potentiometer_stm32_hh +#define potentiometer_stm32_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 "i2c_queue.hh" + +/// I2C potentiometer, MCP4661. +class Potentiometer : public I2cQueue::Slave +{ + public: + /// Constructor with address variable part. + Potentiometer (I2cQueue &queue, int a2a1a0); + /// See I2cQueue::Slave::recv_status. + void recv_status (const uint8_t *status); + /// Send a low level command. + void send_command (int mem_addr, int data); + /// Set wiper to specified level. + void set_wiper (int index, int data, bool eeprom = false); +}; + +#endif // potentiometer_stm32_hh diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index ff3bea61..2a27a4ce 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -187,6 +187,24 @@ class Mex: m.push ('b', c) self.node.send (m) + class Potentiometer (Observable): + """I2C Potentiometer. + + - wiper: list of wiper values (0 ... 1). + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.wiper = [ 0, 0 ] + node.register (instance + ':potentiometer', self.__handle) + + def __handle (self, msg): + index, value = msg.pop ('BH') + value = value / 256. + self.wiper[index] = value + self.notify () + class Path (Observable): """Path finding algorithm report. @@ -256,7 +274,7 @@ class Mex: def __init__ (self, node, instance = 'io-hub0', pwm_nb = 0, contact_nb = 0, output_nb = 0, gpios = False, - adc_channels = False, codebar = False): + adc_channels = False, codebar = False, potentiometer = False): self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) if pwm_nb: self.pwm = tuple (self.PWM () for i in range (0, pwm_nb)) @@ -278,6 +296,8 @@ class Mex: self.__codebar_pack = self.Codebar.Pack (node, instance) self.codebar = tuple (self.Codebar (self.__codebar_pack, i) for i in (0, 1)) + if potentiometer: + self.potentiometer = self.Potentiometer (node, instance) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) self.debug_draw = self.DebugDraw (node, instance) diff --git a/host/simu/model/table_eurobot2013.py b/host/simu/model/table_eurobot2013.py index d73b0564..0181ac00 100644 --- a/host/simu/model/table_eurobot2013.py +++ b/host/simu/model/table_eurobot2013.py @@ -22,6 +22,7 @@ # # }}} """Table model for Eurobot 2013.""" +from utils.observable import Observable import simu.model.table from simu.model.round_obstacle import RoundObstacle from simu.model.rectangular_obstacle import RectangularObstacle @@ -29,6 +30,12 @@ from math import pi import math import random +class Cherries (Observable): + + def __init__ (self): + Observable.__init__ (self) + self.cherries = [ ] + class Table (simu.model.table.Table): def __init__ (self, cards = None): @@ -94,6 +101,7 @@ class Table (simu.model.table.Table): for py in (250, 600, 1000, 1400, 1750): add_plate ((200, py), False) add_plate ((3000 - 200, py), True) + self.cherries = Cherries () # Gifts. self.gifts = [ ] def add_gift (pos, color): diff --git a/host/simu/robots/apbirthday/link/bag.py b/host/simu/robots/apbirthday/link/bag.py index f27f65fa..52c5c75f 100644 --- a/host/simu/robots/apbirthday/link/bag.py +++ b/host/simu/robots/apbirthday/link/bag.py @@ -34,7 +34,7 @@ class Bag: self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance, aux_nb = 0) self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance, gpios = True, - adc_channels = True) + adc_channels = True, potentiometer = True) for gpio in io_hub.apbirthday.gpios: setattr (self, gpio, MexGpio (self.io_hub.gpios, gpio)) adc_channels = self.io_hub.adc_channels diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py index bbf5438e..c7330598 100644 --- a/host/simu/robots/apbirthday/model/bag.py +++ b/host/simu/robots/apbirthday/model/bag.py @@ -82,6 +82,7 @@ class Bag: link_bag.cherry_plate_clamp, scheduler, 0., 1., 10., 10., 0.), (Switch (link_bag.cherry_plate_left_contact), - Switch (link_bag.cherry_plate_right_contact))) + Switch (link_bag.cherry_plate_right_contact)), + link_bag.io_hub.potentiometer) self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/apbirthday/model/cannon.py b/host/simu/robots/apbirthday/model/cannon.py index 74810fd6..4fc78fd8 100644 --- a/host/simu/robots/apbirthday/model/cannon.py +++ b/host/simu/robots/apbirthday/model/cannon.py @@ -25,22 +25,29 @@ from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix from simu.utils.vector import vector +import random +from math import pi class Cannon (Observable): + # TODO: update distance with real robot. + cannon_hit = (1000, 80) + def __init__ (self, table, robot_position, - arm_cyl, clamp_cyl, contacts): + arm_cyl, clamp_cyl, contacts, pot): Observable.__init__ (self) self.table = table self.robot_position = robot_position self.arm_cyl = arm_cyl self.clamp_cyl = clamp_cyl self.contacts = contacts + self.pot = pot self.plate = None self.cherries = [ ] self.robot_position.register (self.__robot_position_notified) self.arm_cyl.register (self.__arm_notified) self.clamp_cyl.register (self.__arm_notified) + self.pot.register (self.__pot_notified) def __robot_position_notified (self): if self.robot_position.pos is None: @@ -82,8 +89,24 @@ class Cannon (Observable): elif (self.plate is not None and self.plate.cherries and self.arm_cyl.pos < .1): # Load cherries. - self.cherries = self.plate.cherries + self.cherries.extend (self.plate.cherries) self.plate.cherries = [ ] + self.__pot_notified () + self.notify () + + def __pot_notified (self): + if self.cherries and self.pot.wiper[0] > 0.5: + m = TransMatrix () + m.translate (self.robot_position.pos) + m.rotate (self.robot_position.angle) + hit = vector (*m.apply (self.cannon_hit)) + for c in self.cherries: + c.pos = hit + vector.polar (random.uniform (-pi, pi), + random.uniform (0, 50)) + c.notify () + self.table.cherries.cherries.extend (self.cherries) + self.table.cherries.notify () + self.cherries = [ ] self.notify () def __plate_drop_point (self): diff --git a/host/simu/robots/apbirthday/view/robot.py b/host/simu/robots/apbirthday/view/robot.py index e4153232..9675aff5 100644 --- a/host/simu/robots/apbirthday/view/robot.py +++ b/host/simu/robots/apbirthday/view/robot.py @@ -27,6 +27,7 @@ from simu.view.table_eurobot2013 import PINK, colors COLOR_ROBOT = '#000000' COLOR_AXES = '#202040' +COLOR_CANNON = '#808080' class Robot (simu.inter.drawable.Drawable): @@ -69,6 +70,8 @@ class Robot (simu.inter.drawable.Drawable): # Draw robot body. self.draw_polygon ((102, 140), (102, -140), (-108, -140), (-108, 70), (-58, 140), fill = COLOR_ROBOT) + self.draw_circle ((70, self.cannon_model.cannon_hit[1]), 20, + fill = COLOR_CANNON) # Draw Robot axis. self.draw_line ((-50, 0), (50, 0), fill = COLOR_AXES, arrow = 'last') diff --git a/host/simu/view/table_eurobot2013.py b/host/simu/view/table_eurobot2013.py index 6acd847d..abb87906 100644 --- a/host/simu/view/table_eurobot2013.py +++ b/host/simu/view/table_eurobot2013.py @@ -90,6 +90,19 @@ class Plate (Drawable): self.draw_circle (c.pos, c.radius, fill = colors[c.color]) Drawable.draw (self) +class Cherries (Drawable): + + def __init__ (self, onto, model): + Drawable.__init__ (self, onto) + self.model = model + self.model.register (self.update) + + def draw (self): + self.reset () + for c in self.model.cherries: + if c.pos: + self.draw_circle (c.pos, c.radius, fill = colors[c.color]) + class Gift (Drawable): def __init__ (self, onto, model): @@ -122,6 +135,7 @@ class Table (Drawable): Glass (self, e) for e in self.model.plates: Plate (self, e) + Cherries (self, self.model.cherries) for e in self.model.gifts: Gift (self, e) -- cgit v1.2.3