summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/goto.c7
-rw-r--r--n/asserv/src/main.c13
-rw-r--r--n/asserv/src/taz.c212
3 files changed, 232 insertions, 0 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 8aa5e91..c427af9 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -44,6 +44,8 @@ int8_t goto_s;
int32_t goto_eps = 20L << 8;
/** Debug values. */
int32_t goto_dx, goto_dy, goto_dl, goto_da;
+/** Movement finished. */
+uint8_t goto_finish;
/* +AutoDec */
/* -AutoDec */
@@ -65,6 +67,7 @@ goto_linear_mode (void)
{
speed_left_aim = 0;
speed_right_aim = 0;
+ goto_finish = 1;
}
else
{
@@ -104,6 +107,7 @@ goto_position_mode (void)
{
speed_left_aim = 0;
speed_right_aim = 0;
+ goto_finish = 1;
}
else
{
@@ -130,6 +134,7 @@ goto_position_exact_mode (void)
{
speed_left_aim = 0;
speed_right_aim = 0;
+ goto_finish = 1;
}
else
{
@@ -166,6 +171,8 @@ goto_ftw_mode (void)
speed_right = 0;
speed_right_aim = 0;
}
+ if (!(PINC & (_BV (0) || _BV (7))))
+ goto_finish = 1;
}
/** Update the speed according to the desired destination. */
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index e6fb72b..9eeca45 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -115,6 +115,10 @@ main_loop (void)
if (motor_mode >= 2)
{
goto_update ();
+ if (goto_finish)
+ {
+ proto_send0 ('F');
+ }
}
motor_timer_2 = timer_read ();
/* Speed control. */
@@ -198,6 +202,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (goto_sign) goto_d = -goto_d;
goto_x = postrack_x;
goto_y = postrack_y;
+ goto_finish = 0;
break;
case c ('a', 1):
/* Angular move.
@@ -205,6 +210,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_mode = 2;
goto_mode = 1;
goto_a = args[0];
+ goto_finish = 0;
break;
case c ('g', 8):
/* Go to position move.
@@ -214,6 +220,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
goto_mode = 2;
goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
+ goto_finish = 0;
break;
case c ('f', 1):
/* Fuck the wall move.
@@ -221,6 +228,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_mode = 2;
goto_mode = 10;
goto_s = args[0];
+ goto_finish = 0;
+ break;
+ case c ('F', 1):
+ /* Setup finish flag.
+ * - b: new value. */
+ goto_finish = args[0];
break;
case c ('s', 0):
/* Set zero speed (stop). */
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
new file mode 100644
index 0000000..b7e941d
--- /dev/null
+++ b/n/asserv/src/taz.c
@@ -0,0 +1,212 @@
+/* taz.c - Automate pour Taz, robot 2005. */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * 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.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+
+/** FSM state. */
+uint8_t taz_state;
+uint8_t taz_substate;
+/** Positions. */
+#define taz_scale 0.05026548245743669181
+static const uint16_t taz_rear_16 = taz_scale * 270 / 2;
+static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256;
+static const uint16_t taz_front_16 = taz_scale * 270 / 2;
+static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256;
+static const uint16_t taz_side_16 = taz_scale * 340 / 2;
+static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256;
+static const uint16_t taz_start_y_16 = taz_scale * 450 / 2;
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* TODO:
+ * - timer de match.
+ */
+
+/** Initialise behavior. */
+static inline void
+taz_init (void)
+{
+ taz_state = 0;
+ taz_substate = 0;
+}
+
+/** Mise en place avant le départ. */
+static inline void
+taz_state_0 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ /* Attend que l'on enfonce le jack. */
+ if (PINC & _BV (3))
+ taz_substate = 1;
+ break;
+ case 1:
+ /* Attend que l'on enlève le jack. */
+ if (!(PINC & _BV (3)))
+ {
+ taz_substate = 2;
+ /* FTW. */
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = -0x10;
+ goto_finish = 0;
+ }
+ break;
+ case 2:
+ if (goto_finish)
+ {
+ taz_substate = 3;
+ /* Recalage. */
+ goto_y = taz_rear_32;
+ goto_a = 0x00400000;
+ /* On avance juste qu'à l'y de départ. */
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = 0;
+ goto_d = taz_start_y - taz_rear;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ goto_finish = 0;
+ }
+ break;
+ case 3:
+ if (goto_finish)
+ {
+ taz_substate = 4;
+ /* Direction l'axe des x. */
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = 0;
+ goto_finish = 0;
+ }
+ break;
+ case 4:
+ if (goto_finish)
+ {
+ taz_substate = 5;
+ /* FTW. */
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = -0x10;
+ goto_finish = 0;
+ }
+ break;
+ case 5:
+ if (goto_finish && (PINC & _BV (3)))
+ {
+ taz_substate = 6;
+ /* Recalage. */
+ goto_x = taz_rear_32;
+ goto_a = 0;
+ }
+ case 6:
+ if (!(PINC & _BV (3)))
+ {
+ taz_state = 1;
+ taz_substate = 0;
+ }
+ }
+}
+
+/** Avancée vers le pont. */
+static inline void
+taz_state_1 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ break;
+ }
+}
+
+/** Traversée du pont. */
+static inline void
+taz_state_2 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ break;
+ }
+}
+
+/** . */
+static inline void
+taz_state_3 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ break;
+ }
+}
+
+/** . */
+static inline void
+taz_state_4 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ break;
+ }
+}
+
+/** . */
+static inline void
+taz_state_5 (void)
+{
+ switch (taz_substate)
+ {
+ case 0:
+ break;
+ }
+}
+
+/** FSM. */
+static inline void
+taz_update (void)
+{
+ switch (taz_state)
+ {
+ case 0:
+ taz_state_0 ();
+ break;
+ case 1:
+ taz_state_1 ();
+ break;
+ case 2:
+ taz_state_2 ();
+ break;
+ case 3:
+ taz_state_3 ();
+ break;
+ case 4:
+ taz_state_4 ();
+ break;
+ case 5:
+ taz_state_5 ();
+ break;
+ }
+}