From e095c76e453fa3b840424cabc02b5812bd9cb1a9 Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Thu, 9 May 2013 06:02:43 +0200 Subject: digital/io-hub/src/apbirthday: add RGB class --- digital/io-hub/src/apbirthday/Makefile | 2 + digital/io-hub/src/apbirthday/cannon.cc | 3 + digital/io-hub/src/apbirthday/hardware.hh | 1 + digital/io-hub/src/apbirthday/hardware.stm32.cc | 4 + digital/io-hub/src/apbirthday/rgb.cc | 58 +++++ digital/io-hub/src/apbirthday/rgb.hh | 92 ++++++++ digital/io-hub/src/apbirthday/rgb.host.cc | 83 +++++++ digital/io-hub/src/apbirthday/rgb.stm32.cc | 296 ++++++++++++++++++++++++ digital/io-hub/src/apbirthday/robot.cc | 21 +- digital/io-hub/src/apbirthday/robot.hh | 5 + 10 files changed, 564 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/apbirthday/rgb.cc create mode 100644 digital/io-hub/src/apbirthday/rgb.hh create mode 100644 digital/io-hub/src/apbirthday/rgb.host.cc create mode 100644 digital/io-hub/src/apbirthday/rgb.stm32.cc (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 69c7027b..0d35b813 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -8,6 +8,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ potentiometer.host.cc potentiometer.stm32.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ servo.host.cc servo.stm32.cc \ + rgb.host.cc rgb.stm32.cc rgb.cc \ radar.cc radar_2013.cc obstacles.cc path.cc path_2013.cc strat.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc cannon.cc drinks.cc plate.cc \ @@ -46,6 +47,7 @@ clean.project: rm -f usb zb_uart angfsm_gen_arm_AI.h angfsm_gen_arm_AI.arm.c obj/main.stm32f4.o: angfsm_gen_arm_AI.h +obj/rgb.stm32.stm32f4.o: angfsm_gen_arm_AI.h angfsm_gen_arm_AI.arm.c: angfsm_gen_arm_AI.h angfsm_gen_arm_AI.h: apbirthday.host ./$< $(ANGFSM_OPTIONS) --ang-gen arm && mv angfsm_gen_arm_AI.c angfsm_gen_arm_AI.arm.c diff --git a/digital/io-hub/src/apbirthday/cannon.cc b/digital/io-hub/src/apbirthday/cannon.cc index 33e17590..4e91d750 100644 --- a/digital/io-hub/src/apbirthday/cannon.cc +++ b/digital/io-hub/src/apbirthday/cannon.cc @@ -115,6 +115,8 @@ FSM_TRANS (CANNON_READY, cannon_fire, CANNON_FIRING) // Start the blower Cannon::blower_on (); // Start RGB sensor + robot->rgb.calibrate_cannon_sensor (); + robot->rgb.start_cannon_color (); } FSM_TRANS_TIMEOUT (CANNON_FIRING, 1250, CANNON_READY) @@ -122,6 +124,7 @@ FSM_TRANS_TIMEOUT (CANNON_FIRING, 1250, CANNON_READY) // Stop the blower Cannon::blower_off (); // Stop the RGB sensor + robot->rgb.stop_cannon_color (); robot->fsm_queue.post (FSM_EVENT (cannon_fire_ok)); } diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index d53d3ec8..41b47d03 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -87,6 +87,7 @@ struct Hardware glass_upper_clamp_up, glass_upper_clamp_down, gift_out, gift_in, ballon_funny_action, + rgb_candle_near, rgb_candle_far, rgb_cannon, pneum_open; ucoo::Gpio dist0_sync, dist1_sync, dist2_sync, dist3_sync; diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index 26510abc..661e9db0 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -28,6 +28,7 @@ #include #include "ucoolib/hal/gpio/gpio.hh" #include "ucoolib/utils/crc.hh" +#include "rgb.hh" #include "zb_avrisp.stm32.hh" @@ -57,6 +58,9 @@ 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 (GPIOA, 10), + rgb_candle_near (GPIOE, 15), + rgb_candle_far (GPIOE, 10), + rgb_cannon (GPIOE, 13), pneum_open (GPIOD, 5), dist0_sync (GPIOC, 0), dist1_sync (GPIOC, 1), dist2_sync (GPIOC, 2), dist3_sync (GPIOC, 3), diff --git a/digital/io-hub/src/apbirthday/rgb.cc b/digital/io-hub/src/apbirthday/rgb.cc new file mode 100644 index 00000000..8e8cc188 --- /dev/null +++ b/digital/io-hub/src/apbirthday/rgb.cc @@ -0,0 +1,58 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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 "rgb.hh" + +int +Rgb::rgb_to_hue (int r, int g, int b) +{ + uint8_t min = 255, max = 0, hue; + + // find minimal between r, g and b + if (r < min) + min = r; + if (g < min) + min = g; + if (b < min) + min = b; + + // find maximal between r, g and b + if (r > max) + max = r; + if (g > max) + max = g; + if (b > max) + max = b; + + if (min == max) + return 0; + + /* Compute hue */ + if (max == r) + hue = 0 + 43 * (g - b) / (max - min); + else if (max == g) + hue = 85 + 43 * (b - r) / (max - min); + else /* max == b */ + hue = 171 + 43 * (r - g) / (max - min); + return hue; +} diff --git a/digital/io-hub/src/apbirthday/rgb.hh b/digital/io-hub/src/apbirthday/rgb.hh new file mode 100644 index 00000000..b935c632 --- /dev/null +++ b/digital/io-hub/src/apbirthday/rgb.hh @@ -0,0 +1,92 @@ +#ifndef rgb_hh +#define rgb_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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/hal/gpio/gpio.hh" + +/* This value was determined during tests using a BLUE filter */ +/* This will only be used for the cannon method */ +#define BASIC_GREY 190 + +class Rgb +{ + public: + Rgb (ucoo::Gpio *en_cannon, + ucoo::Gpio *en_candle_far, + ucoo::Gpio *en_candle_near); + + enum color + { + WHITE, + RED, + GREEN, + BLUE, + UNKNOWN, + NOTHING + }; + + enum rgb_type + { + CANNON, + CANDLE_NEAR, + CANDLE_FAR, + RGB_TYPE_NB + }; + + /* Enable the RGB timer, and setup the Input Capture block */ + void enable (); + void update (); + void calibrate_cannon_sensor (); + void start_cannon_color (); + void stop_cannon_color (); + enum color get_candle_color (); + enum color get_candle_near_color (); + enum color get_candle_far_color (); + void get_colors (rgb_type type, uint16_t &w, uint16_t &r, uint16_t &g, + uint16_t &b); + + void ic_isr (); + + private: + /* setup a filter to measure one color, this is for all RGB */ + void setup_color (int color); + void enable_sensor (); + void disable_sensor (); + void do_measure (rgb_type type, bool router); + int rgb_to_hue (int r, int g, int b); + + ucoo::Gpio* g[RGB_TYPE_NB]; + enum rgb_type type_; + uint32_t cannon_offset_; + bool router_; + bool seen_color_, seen_white_; + int measure_cnt_; + int cur_color_; + int router_timer_; + uint32_t last_color_value_; + uint32_t color_value_[4]; +}; + +#endif // rgb_hh diff --git a/digital/io-hub/src/apbirthday/rgb.host.cc b/digital/io-hub/src/apbirthday/rgb.host.cc new file mode 100644 index 00000000..1c60d035 --- /dev/null +++ b/digital/io-hub/src/apbirthday/rgb.host.cc @@ -0,0 +1,83 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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/hal/gpio/gpio.hh" +#include "rgb.hh" +#include "robot.hh" + +Rgb::Rgb (ucoo::Gpio *en_cannon, + ucoo::Gpio *en_candle_far, + ucoo::Gpio *en_candle_near) +{ +} + +void +Rgb::enable () +{ +} + +void +Rgb::update () +{ +} + +void +Rgb::calibrate_cannon_sensor () +{ +} + +void +Rgb::start_cannon_color () +{ +} + +void +Rgb::stop_cannon_color () +{ +} + +enum Rgb::color +Rgb::get_candle_far_color () +{ + return robot->rgb.get_candle_color (); +} + +enum Rgb::color +Rgb::get_candle_near_color () +{ + return robot->rgb.get_candle_color (); +} + +enum Rgb::color +Rgb::get_candle_color () +{ + return WHITE; +} + +void +Rgb::get_colors (rgb_type type, uint16_t &w, uint16_t &r, uint16_t &g, + uint16_t &b) +{ + w = r = g = b = 0; +} + diff --git a/digital/io-hub/src/apbirthday/rgb.stm32.cc b/digital/io-hub/src/apbirthday/rgb.stm32.cc new file mode 100644 index 00000000..512a5e8a --- /dev/null +++ b/digital/io-hub/src/apbirthday/rgb.stm32.cc @@ -0,0 +1,296 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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 "rgb.hh" +#include "robot.hh" + +#include +#include +#include +#include + +// The interupt fonction will call one in our class. +extern "C" +{ + void tim1_cc_isr () { robot->rgb.ic_isr (); } +} + +Rgb::Rgb (ucoo::Gpio *en_cannon, + ucoo::Gpio *en_candle_far, + ucoo::Gpio *en_candle_near) +{ + g[CANNON] = en_cannon; + g[CANNON]->output (); + g[CANDLE_NEAR] = en_candle_near; + g[CANDLE_NEAR]->output (); + g[CANDLE_FAR] = en_candle_far; + g[CANDLE_FAR]->output (); + router_timer_ = 0; +} + +void +Rgb::enable () +{ + // Disable the sensor to be sure + int c; + for (c = (int) CANNON; c < (int) RGB_TYPE_NB; c++) + { + type_ = (enum Rgb::rgb_type) c; + disable_sensor (); + } + + // Enable clock for Timer 1. + rcc_peripheral_enable_clock (&RCC_APB2ENR, RCC_APB2ENR_TIM1EN); + + /* Output GPIO */ + gpio_mode_setup (GPIOE, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9); + gpio_set_af (GPIOE, GPIO_AF1, GPIO9); + + nvic_enable_irq (NVIC_TIM1_CC_IRQ); + + timer_reset (TIM1); + timer_set_mode (TIM1, + TIM_CR1_CKD_CK_INT, // Internal 72 MHz clock + TIM_CR1_CMS_EDGE, // Edge synchronization + TIM_CR1_DIR_UP); // Upward counter + + timer_set_prescaler (TIM1, 75); + timer_set_period (TIM1, 0xFFFF); + timer_set_repetition_counter (TIM1, 0); + timer_continuous_mode (TIM1); + + /* Configure Input Capture Channel 1 */ + timer_ic_set_input (TIM1, TIM_IC1, TIM_IC_IN_TI1); + timer_ic_set_filter (TIM1, TIM_IC1, TIM_IC_OFF); + timer_ic_set_polarity (TIM1, TIM_IC1, TIM_IC_RISING); + timer_ic_set_prescaler (TIM1, TIM_IC1, TIM_IC_PSC_OFF); + timer_ic_enable (TIM1, TIM_IC1); + timer_clear_flag (TIM1, TIM_SR_CC1IF); + timer_disable_irq (TIM1, TIM_DIER_CC1IE); + + timer_enable_counter (TIM1); +} + +void +Rgb::setup_color (int color) +{ + ucoo::Gpio S2 (GPIOE, 8); + ucoo::Gpio S3 (GPIOE, 11); + S2.output (); + S3.output (); + + switch (color) + { + case RED: + S2.set (0); + S3.set (0); + break; + case BLUE: + S2.set (0); + S3.set (1); + break; + case WHITE: + S2.set (1); + S3.set (0); + break; + case GREEN: + S2.set (1); + S3.set (1); + break; + } +} + +void +Rgb::calibrate_cannon_sensor () +{ + // Do a measure to get current light situation + do_measure (CANNON, false); + cannon_offset_ = BASIC_GREY - color_value_[BLUE]; +} + +void +Rgb::enable_sensor () +{ + g[type_]->set (0); + timer_clear_flag (TIM1, TIM_SR_CC1IF); + timer_enable_irq (TIM1, TIM_DIER_CC1IE); +} + +void +Rgb::disable_sensor () +{ + g[type_]->set (1); + timer_disable_irq (TIM1, TIM_DIER_CC1IE); +} + +void +Rgb::do_measure (rgb_type type, bool router) +{ + type_ = type; + router_ = router; + measure_cnt_ = 0; + if (router) + cur_color_ = BLUE; + else + cur_color_ = WHITE; + setup_color (cur_color_); + seen_color_ = seen_white_ = false; + enable_sensor (); + // The interupt will disable the sensor + while (!router && g[type]->get() == 0) + ; + // Grab the results in... color_value_ +} + +void +Rgb::start_cannon_color () +{ + do_measure (CANNON, true); +} + +void +Rgb::stop_cannon_color () +{ + disable_sensor (); +} + +void +Rgb::update () +{ + if (router_timer_) + { + --router_timer_; + if (router_timer_ == 0) + { + robot->hardware.cherry_bad_out.set (false); + } + } +} + +enum Rgb::color +Rgb::get_candle_far_color () +{ + do_measure (CANDLE_FAR, false); + return Rgb::get_candle_color (); +} + +enum Rgb::color +Rgb::get_candle_near_color () +{ + do_measure (CANDLE_NEAR, false); + return Rgb::get_candle_color (); +} + +enum Rgb::color +Rgb::get_candle_color () +{ + uint32_t min_color = (uint32_t) (-1); + uint32_t results = NOTHING; + uint32_t color; + + // We don't care for white level + for (color = WHITE; color != UNKNOWN; color++) + { + if (color == BLUE) + color_value_[color] *= 0.8; + else if (color == RED) + color_value_[color] *= 1.2; + + if (color_value_[color] < min_color) + { + results = color; + min_color = color_value_[color]; + } + color_value_[color] = 0; + } + + return (enum Rgb::color) results; +} + +void +Rgb::get_colors (rgb_type type, uint16_t &w, uint16_t &r, uint16_t &g, + uint16_t &b) +{ + do_measure (type, false); + w = color_value_[WHITE]; + r = color_value_[RED]; + g = color_value_[GREEN]; + b = color_value_[BLUE]; +} + +void +Rgb::ic_isr () +{ + uint16_t cc = TIM1_CCR1; + + // Throw away the first measurement + if (measure_cnt_ == 0) + { + measure_cnt_++; + } + else if (measure_cnt_ == 1) + { + last_color_value_ = cc; + measure_cnt_++; + } + else if (measure_cnt_ == 2) + { + color_value_[cur_color_] = (cc - last_color_value_) & 0xffff; + + if (router_) + { + uint16_t v = color_value_[BLUE] + cannon_offset_; + if (v > 120 && v < 165) + { + // we don't know if it's blue or red, and we don't care + // It's colored and must be thrown out ! + seen_color_ = true; + } + else if (v > 40 && v < 100) + { + seen_white_ = true; + } + // Check if we are seeing GREY again + else if (v > 180 && v < 210) + { + // If we have seen color + if (seen_color_ && !seen_white_) + { + robot->hardware.cherry_bad_out.set (true); + router_timer_ = 4; + } + seen_color_ = seen_white_ = false; + } + } + else + { + cur_color_++; + if (cur_color_ == UNKNOWN) + disable_sensor (); + else + setup_color (cur_color_); + } + + measure_cnt_ = 0; + } +} diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 82a54e27..ad1538ea 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -66,12 +66,13 @@ Robot::Robot () hardware.adc_dist3.get_resolution () * 2040 / 3300), radar_ (usdist0_, usdist1_, usdist2_, usdist3_), candles (1), + rgb (&hardware.rgb_cannon, &hardware.rgb_candle_far, &hardware.rgb_candle_near), fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), stats_asserv_ (0), stats_beacon_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), - stats_inputs_ (0), stats_usdist_ (0), stats_cake_ (0), stats_pressure_ (0) + stats_inputs_ (0), stats_usdist_ (0), stats_cake_ (0), stats_pressure_ (0), stats_rgb_ (0) { robot = this; // Fill I/O arrays. @@ -122,6 +123,7 @@ Robot::Robot () usdist1_.disable (); usdist2_.disable (); usdist3_.disable (); + rgb.enable (); } void @@ -197,6 +199,8 @@ Robot::main_loop () hardware.zb_handle (); // Send stats. proto_stats (); + // RGB Update Router + rgb.update (); } } @@ -398,6 +402,14 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size stats_pressure_cpt_ = stats_pressure_ = args[0]; stats_proto_ = &proto; break; + case c ('R', 2): + // RGB stats. + // 1B: sensor type. + // 1B: stat interval. + stats_rgb_type_ = args[0]; + stats_rgb_cpt_ = stats_rgb_ = args[1]; + stats_proto_ = &proto; + break; case c ('P', 1): // Plate test // 1B: @@ -585,6 +597,13 @@ Robot::proto_stats () stats_proto_->send ('F', "H", pressure.get ()); stats_pressure_cpt_ = stats_pressure_; } + if (stats_rgb_ && !--stats_rgb_cpt_) + { + uint16_t w, r, g, b; + rgb.get_colors ((Rgb::rgb_type) stats_rgb_type_, w, r, g, b); + stats_proto_->send ('R', "HHHH", w, r, g, b); + stats_rgb_cpt_ = stats_rgb_; + } } void diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index e4c66477..85b67112 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -47,6 +47,7 @@ #include "drinks.hh" #include "plate.hh" #include "cannon.hh" +#include "rgb.hh" #include "ucoolib/base/proto/proto.hh" #include "ucoolib/dev/usdist/usdist.hh" @@ -131,6 +132,8 @@ class Robot : public ucoo::Proto::Handler Plate plate; /// Cannon Cannon cannon; + /// RGB + Rgb rgb; private: /// FSM debug mode. enum FsmDebugState @@ -167,6 +170,8 @@ class Robot : public ucoo::Proto::Handler int stats_cake_, stats_cake_cpt_; /// Pressure stats interval and counter. int stats_pressure_, stats_pressure_cpt_; + /// RGB stats interval, counter and type. + int stats_rgb_, stats_rgb_cpt_, stats_rgb_type_; }; /// Global instance pointer. -- cgit v1.2.3