From dc59b1c284bf412878e80a102574fde4b60ef7c1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 18 Apr 2012 01:59:15 +0200 Subject: digital/io-hub/src/guybrush: add pressure regulation --- digital/io-hub/src/guybrush/Makefile | 1 + digital/io-hub/src/guybrush/main.c | 19 +++++ digital/io-hub/src/guybrush/pressure.c | 144 +++++++++++++++++++++++++++++++++ digital/io-hub/src/guybrush/pressure.h | 39 +++++++++ 4 files changed, 203 insertions(+) create mode 100644 digital/io-hub/src/guybrush/pressure.c create mode 100644 digital/io-hub/src/guybrush/pressure.h diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile index 7a0e8bc2..7e3db6fd 100644 --- a/digital/io-hub/src/guybrush/Makefile +++ b/digital/io-hub/src/guybrush/Makefile @@ -5,6 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c top.c \ radar_defs.c radar.c path.c move.c \ + pressure.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ contact.avr.c contact.host.c \ output.c output.host.c \ diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index c7ef204a..f2d8d4e7 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -42,6 +42,7 @@ #include "contact.h" #include "output.h" #include "radar.h" +#include "pressure.h" #define FSM_NAME AI #include "fsm.h" @@ -79,6 +80,9 @@ static uint8_t main_stats_contact_, main_stats_contact_cpt_; /** US sensors stats counters. */ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; +/** Pressure stats counters. */ +static uint8_t main_stats_pressure_, main_stats_pressure_cpt_; + /** Main initialisation. */ static void main_init (void) @@ -186,6 +190,7 @@ main_loop (void) move_obstacles_update (); simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } + pressure_update (); /* Update AI modules. */ path_decay (); /* Only manage events if slaves are synchronised. */ @@ -213,6 +218,11 @@ main_loop (void) usdist_mm[3]); main_stats_usdist_cpt_ = main_stats_usdist_; } + if (main_stats_pressure_ && !--main_stats_pressure_cpt_) + { + proto_send1w ('F', pressure_get ()); + main_stats_pressure_cpt_ = main_stats_pressure_; + } } } @@ -267,6 +277,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) } } break; + case c ('f', 2): + /* Set low pressure threshold. + * 1w: sensor value, 1024 is full scale. */ + pressure_set (v8_to_v16 (args[0], args[1])); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): @@ -281,6 +296,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* US sensors stats. */ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; break; + case c ('F', 1): + /* Pressure stats. */ + main_stats_pressure_ = main_stats_pressure_cpt_ = args[0]; + break; default: /* Unknown commands */ proto_send0 ('?'); diff --git a/digital/io-hub/src/guybrush/pressure.c b/digital/io-hub/src/guybrush/pressure.c new file mode 100644 index 00000000..f60d226d --- /dev/null +++ b/digital/io-hub/src/guybrush/pressure.c @@ -0,0 +1,144 @@ +/* pressure.c */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 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 "common.h" +#include "pressure.h" + +#include "modules/adc/adc.h" + +#include "output.h" +#include "mimot.h" + +/** Hysteresis margin. */ +#define PRESSURE_MARGIN (1024 / 8) + +/** Pressure sensor connected ADC. */ +#define PRESSURE_SENSOR_ADC 4 + +/** Pressure FSM states. */ +enum +{ + /** Start state, circuit is closed, no pressure. */ + PRESSURE_INITIAL, + /** Idle, pressure is OK, circuit is open. */ + PRESSURE_IDLE, + /** Starting the pump, with open circuit. */ + PRESSURE_STARTING, + /** Pumping until the pressure is OK, circuit is closed. */ + PRESSURE_PUMPING, +}; + +/** Global context. */ +struct pressure_t +{ + /** Pressure sensor threshold values (initialized to 0). */ + uint16_t low, high; + /** Pressure FSM current state. */ + uint8_t state; + /** Wait counter before the next action. */ + uint8_t wait; +}; +static struct pressure_t pressure; + +/** Interval at which the sensor is read. */ +#define PRESSURE_INTERVAL 25 + +/** Rest duration after a pumping session. */ +#define PRESSURE_REST 250 + +/** Wait duration before the pump is started. */ +#define PRESSURE_OPEN_WAIT 125 + +/** Wait duration after the pump has been started before circuit is closed. */ +#define PRESSURE_START_WAIT 50 + +void +pressure_update (void) +{ + if (pressure.wait) + { + pressure.wait--; + } + else + { + switch (pressure.state) + { + case PRESSURE_INITIAL: + if (pressure.low) + { + /* Open circuit. */ + IO_SET (OUTPUT_PNEUM_OPEN); + pressure.state++; + pressure.wait = PRESSURE_OPEN_WAIT; + } + break; + case PRESSURE_IDLE: + if (pressure_get () < pressure.low) + { + /* Start pump. */ + mimot_motor_output_set (1, 0x3ff); + pressure.state++; + pressure.wait = PRESSURE_START_WAIT; + } + else + pressure.wait = PRESSURE_INTERVAL; + break; + case PRESSURE_STARTING: + /* Started, close circuit. */ + IO_CLR (OUTPUT_PNEUM_OPEN); + pressure.state++; + break; + case PRESSURE_PUMPING: + /* Pump until high. */ + if (pressure_get () > pressure.high) + { + /* Done. Stop. Open circuit. */ + mimot_motor1_free (); + IO_SET (OUTPUT_PNEUM_OPEN); + pressure.state = PRESSURE_IDLE; + pressure.wait = PRESSURE_REST; + } + else + pressure.wait = PRESSURE_INTERVAL; + break; + } + } +} + +void +pressure_set (uint16_t low) +{ + pressure.low = low; + pressure.high = low + PRESSURE_MARGIN; +} + +uint16_t +pressure_get (void) +{ + adc_start (PRESSURE_SENSOR_ADC); + while (!adc_checkf ()) + ; + return adc_read (); +} + diff --git a/digital/io-hub/src/guybrush/pressure.h b/digital/io-hub/src/guybrush/pressure.h new file mode 100644 index 00000000..6f87f842 --- /dev/null +++ b/digital/io-hub/src/guybrush/pressure.h @@ -0,0 +1,39 @@ +#ifndef pressure_h +#define pressure_h +/* pressure.h */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 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. + * + * }}} */ + +/** Handle pressure sensor and pump. */ + +void +pressure_update (void); + +void +pressure_set (uint16_t low); + +uint16_t +pressure_get (void); + +#endif /* pressure_h */ -- cgit v1.2.3