From 52d4a4917e49e3864cb50cb37cfd86537e42a287 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 26 Mar 2013 21:00:36 +0100 Subject: digital/io-hub/src/apbirthday: add US sensors ADC --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/hardware.hh | 7 +++++++ digital/io-hub/src/apbirthday/hardware.host.cc | 6 +++++- digital/io-hub/src/apbirthday/hardware.stm32.cc | 8 +++++++- digital/io-hub/src/apbirthday/robot.cc | 18 +++++++++++++++++- digital/io-hub/src/apbirthday/robot.hh | 2 ++ digital/io-hub/tools/io_hub/mex.py | 7 ++++++- host/simu/robots/apbirthday/link/bag.py | 10 +++++++++- host/simu/robots/apbirthday/model/bag.py | 12 ++++++++++++ host/simu/robots/apbirthday/view/bag.py | 3 +++ 10 files changed, 69 insertions(+), 6 deletions(-) diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 473acdff..02cd0d4e 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -10,7 +10,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ $(AVR_SOURCES) -MODULES = utils hal/gpio hal/uart hal/usb hal/i2c hal/spi \ +MODULES = utils hal/gpio hal/adc hal/uart hal/usb hal/i2c hal/spi \ base/proto dev/avrisp # Hack mode, include old code from AVR modules. diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index f3378170..5713f921 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -26,6 +26,7 @@ #include "ucoolib/hal/uart/uart.hh" #include "ucoolib/hal/gpio/gpio.hh" +#include "ucoolib/hal/adc/adc.hh" #include "ucoolib/hal/i2c/i2c.hh" #ifdef TARGET_stm32 # include "ucoolib/hal/usb/usb.hh" @@ -84,6 +85,12 @@ struct Hardware pneum_open; static const int inputs_nb = 9; static const int outputs_nb = 21; +#ifdef TARGET_stm32 + ucoo::AdcHard adc; + ucoo::AdcHardChannel adc_dist0, adc_dist1, adc_dist2, adc_dist3; +#else + ucoo::AdcHost adc_dist0, adc_dist1, adc_dist2, adc_dist3; +#endif Hardware (); // Wait until next cycle. void wait (); diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index ba25401b..1cffaa7e 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -61,7 +61,11 @@ Hardware::Hardware () gift_out (host, "gift_out"), gift_in (host, "gift_in"), ballon_funny_action (host, "ballon_funny_action"), - pneum_open (host, "pneum_open") + pneum_open (host, "pneum_open"), + adc_dist0 (host, "dist0", 1 << 12), + adc_dist1 (host, "dist1", 1 << 12), + adc_dist2 (host, "dist2", 1 << 12), + adc_dist3 (host, "dist3", 1 << 12) { dev_uart.block (false); zb_uart.block (false); diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index a3cd4c1d..ed051a4a 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -54,7 +54,10 @@ Hardware::Hardware () glass_upper_clamp_up (GPIOD, 3), glass_upper_clamp_down (GPIOC, 10), gift_out (GPIOD, 4), gift_in (GPIOC, 11), ballon_funny_action (GPIOB, 7), - pneum_open (GPIOD, 5) + pneum_open (GPIOD, 5), + adc (0), + adc_dist0 (adc, 0), adc_dist1 (adc, 1), + adc_dist2 (adc, 2), adc_dist3 (adc, 3) { rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPAEN); rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN); @@ -84,6 +87,9 @@ Hardware::Hardware () gpio_set_af (GPIOA, GPIO_AF4, GPIO8); gpio_set_af (GPIOC, GPIO_AF4, GPIO9); main_i2c.enable (); + // ADC. + gpio_mode_setup (GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, + GPIO0 | GPIO1 | GPIO2 | GPIO3); // Cycle timer, 4 ms period. rcc_peripheral_enable_clock (&RCC_APB1ENR, RCC_APB1ENR_TIM3EN); TIM3_PSC = 2 * rcc_ppre1_frequency / 1000000 - 1; // 1 µs prescaler diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 2dc3de95..d8b47387 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -41,7 +41,7 @@ Robot::Robot () outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), - stats_inputs_ (0) + stats_inputs_ (0), stats_usdist_ (0) { robot = this; unsigned i = 0; @@ -212,6 +212,12 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size stats_inputs_cpt_ = stats_inputs_ = args[0]; stats_proto_ = &proto; break; + case c ('U', 1): + // US distance sensors stats. + // 1B: stat interval. + stats_usdist_cpt_ = stats_usdist_ = args[0]; + stats_proto_ = &proto; + break; default: proto.send ('?'); return; @@ -245,5 +251,15 @@ Robot::proto_stats () stats_proto_->send ('I', "L", inputs); stats_inputs_cpt_ = stats_inputs_; } + if (stats_usdist_ && !--stats_usdist_cpt_) + { + // TODO: this is a hack, only work in simulation. + stats_proto_->send ('U', "HHHH", + hardware.adc_dist0.read (), + hardware.adc_dist1.read (), + hardware.adc_dist2.read (), + hardware.adc_dist3.read ()); + stats_usdist_cpt_ = stats_usdist_; + } } diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 99997fd0..492b6e2d 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -88,6 +88,8 @@ class Robot : public ucoo::Proto::Handler int stats_chrono_last_s_; /// Input stats interval and counter. int stats_inputs_, stats_inputs_cpt_; + /// US distance sensors stats interval and counter. + int stats_usdist_, stats_usdist_cpt_; }; /// Global instance pointer. diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 9ad38a69..ff3bea61 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -26,6 +26,7 @@ from utils.observable import Observable import simu.mex.msg import simu.link.mex_gpio +import simu.link.mex_adc_channel ADC_NB = 8 @@ -254,7 +255,8 @@ class Mex: self.notify () def __init__ (self, node, instance = 'io-hub0', - pwm_nb = 0, contact_nb = 0, output_nb = 0, gpios = False, codebar = False): + pwm_nb = 0, contact_nb = 0, output_nb = 0, gpios = False, + adc_channels = False, codebar = 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)) @@ -269,6 +271,9 @@ class Mex: self.output) if gpios: self.gpios = simu.link.mex_gpio.MexGpio.Pack (node, instance) + if adc_channels: + self.adc_channels = simu.link.mex_adc_channel.MexAdcChannel.Pack ( + node, instance) if codebar: self.__codebar_pack = self.Codebar.Pack (node, instance) self.codebar = tuple (self.Codebar (self.__codebar_pack, i) diff --git a/host/simu/robots/apbirthday/link/bag.py b/host/simu/robots/apbirthday/link/bag.py index bc81b694..f2401a21 100644 --- a/host/simu/robots/apbirthday/link/bag.py +++ b/host/simu/robots/apbirthday/link/bag.py @@ -25,13 +25,15 @@ import io_hub.mex import asserv.mex from simu.link.mex_gpio import MexGpio +from simu.link.mex_adc_channel import MexAdcChannel class Bag: def __init__ (self, node, instance = 'robot0'): 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) + self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance, gpios = True, + adc_channels = True) gpios = ('raw_jack', 'ihm_color', 'ihm_strat', 'ihm_robot_nb', 'ihm_lol', 'ihm_emerg_stop', 'glass_contact', 'cherry_plate_left_contact', 'cherry_plate_right_contact', @@ -45,3 +47,9 @@ class Bag: 'gift_in', 'ballon_funny_action', 'pneum_open') for gpio in gpios: setattr (self, gpio, MexGpio (self.io_hub.gpios, gpio)) + self.adc_dist = [ + MexAdcChannel (self.io_hub.adc_channels, 'dist0'), + MexAdcChannel (self.io_hub.adc_channels, 'dist1'), + MexAdcChannel (self.io_hub.adc_channels, 'dist2'), + MexAdcChannel (self.io_hub.adc_channels, 'dist3'), + ] diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py index aa79f9d4..bef099f9 100644 --- a/host/simu/robots/apbirthday/model/bag.py +++ b/host/simu/robots/apbirthday/model/bag.py @@ -25,6 +25,8 @@ from simu.model.switch import Switch from simu.model.position import Position from simu.model.round_obstacle import RoundObstacle +from simu.model.distance_sensor_sensopart import DistanceSensorSensopart +from math import pi import random class Bag: @@ -37,4 +39,14 @@ class Bag: self.beacon = RoundObstacle (40, 5) table.obstacles.append (self.beacon) self.position = Position (link_bag.asserv.position, [ self.beacon ]) + self.distance_sensor = [ + DistanceSensorSensopart (link_bag.adc_dist[0], scheduler, table, + (100, 120), 0, (self.position, ), 4), + DistanceSensorSensopart (link_bag.adc_dist[1], scheduler, table, + (100, -120), 0, (self.position, ), 4), + DistanceSensorSensopart (link_bag.adc_dist[2], scheduler, table, + (-50, 120), pi, (self.position, ), 4), + DistanceSensorSensopart (link_bag.adc_dist[3], scheduler, table, + (-50, -120), pi, (self.position, ), 4), + ] diff --git a/host/simu/robots/apbirthday/view/bag.py b/host/simu/robots/apbirthday/view/bag.py index 8ced9b43..dfea21fa 100644 --- a/host/simu/robots/apbirthday/view/bag.py +++ b/host/simu/robots/apbirthday/view/bag.py @@ -23,6 +23,7 @@ # }}} """APBirthday bag of views.""" from simu.view.switch import Switch +from simu.view.distance_sensor_us import DistanceSensorUS from simu.robots.apbirthday.view.robot import Robot class Bag: @@ -32,4 +33,6 @@ class Bag: self.color_switch = Switch (sensor_frame, model_bag.color_switch, 'Color') self.robot = Robot (table, model_bag.position) + self.distance_sensor = [DistanceSensorUS (self.robot, ds) + for ds in model_bag.distance_sensor] -- cgit v1.2.3