summaryrefslogtreecommitdiff
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorNicolas Schodet2012-04-18 01:59:15 +0200
committerNicolas Schodet2012-05-01 10:11:37 +0200
commitdc59b1c284bf412878e80a102574fde4b60ef7c1 (patch)
treeccb2dc4b07baa738b829eb94ea684f446ccb44fc /digital/io-hub/src
parent929dab13061cfe82cce6125dfb4adb40c3ef6639 (diff)
digital/io-hub/src/guybrush: add pressure regulation
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/guybrush/Makefile1
-rw-r--r--digital/io-hub/src/guybrush/main.c19
-rw-r--r--digital/io-hub/src/guybrush/pressure.c144
-rw-r--r--digital/io-hub/src/guybrush/pressure.h39
4 files changed, 203 insertions, 0 deletions
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 */