summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-20 21:29:29 +0200
committerNicolas Schodet2008-04-20 21:29:29 +0200
commitd47046315caff4ce01b3ea39126b476e56ab3703 (patch)
treea86e48bdbdf9806f71e7cb8a82102194bf7a4ac7 /digital
parent44b4f0058b31b32bfa26746f0467cf353b373d3b (diff)
* digital/asserv/src/asserv:
- added absolute auxiliary motor position command.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/Makefile2
-rw-r--r--digital/asserv/src/asserv/aux.c54
-rw-r--r--digital/asserv/src/asserv/aux.h43
-rw-r--r--digital/asserv/src/asserv/main.c14
-rw-r--r--digital/asserv/src/asserv/twi_proto.c11
5 files changed, 114 insertions, 10 deletions
diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile
index f3987bd7..4a61d08b 100644
--- a/digital/asserv/src/asserv/Makefile
+++ b/digital/asserv/src/asserv/Makefile
@@ -3,7 +3,7 @@ PROGS = asserv
AVR_PROGS = test_counter
HOST_PROGS = test_motor_model
asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pwm_mp.avr.c \
- pwm_ocr.avr.c pos.c speed.c postrack.c traj.c \
+ pwm_ocr.avr.c pos.c speed.c postrack.c traj.c aux.c \
twi_proto.c eeprom.avr.c state.c \
simu.host.c motor_model.host.c models.host.c
test_counter_SOURCES = test_counter.c timer.avr.c
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
new file mode 100644
index 00000000..880496b4
--- /dev/null
+++ b/digital/asserv/src/asserv/aux.c
@@ -0,0 +1,54 @@
+/* aux.c - Auxiliary motors commands. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 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 "aux.h"
+
+#include "state.h"
+
+#include "counter.h"
+#include "pos.h"
+#include "speed.h"
+
+/** Motor state. */
+struct aux_t aux0;
+
+/** Update positions. */
+void
+aux_pos_update (void)
+{
+ /* Easy... */
+ aux0.pos += counter_aux0_diff;
+}
+
+/** Goto position. */
+void
+aux_traj_goto_start (uint16_t pos, uint8_t seq)
+{
+ speed_aux0.use_pos = 1;
+ speed_aux0.pos_cons = pos_aux0.cur;
+ speed_aux0.pos_cons += (int16_t) (pos - aux0.pos);
+ state_start (&state_aux0, MODE_SPEED, seq);
+}
+
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
new file mode 100644
index 00000000..175933cc
--- /dev/null
+++ b/digital/asserv/src/asserv/aux.h
@@ -0,0 +1,43 @@
+#ifndef aux_h
+#define aux_h
+/* aux.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 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.
+ *
+ * }}} */
+
+/** Auxiliary motor informations. */
+struct aux_t
+{
+ /** Absolute position. */
+ int16_t pos;
+};
+
+extern struct aux_t aux0;
+
+void
+aux_pos_update (void);
+
+void
+aux_traj_goto_start (uint16_t pos, uint8_t seq);
+
+#endif /* aux_h */
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index a8d40b08..8be5373f 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -40,6 +40,7 @@
#include "speed.h"
#include "postrack.h"
#include "traj.h"
+#include "aux.h"
#include "twi_proto.h"
#include "eeprom.h"
@@ -132,6 +133,7 @@ main_loop (void)
main_timer[2] = timer_read ();
/* Compute absolute position. */
postrack_update ();
+ aux_pos_update ();
/* Compute trajectory. */
if (state_main.mode >= MODE_TRAJ)
traj_update ();
@@ -160,7 +162,7 @@ main_loop (void)
}
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
{
- proto_send1w ('Z', pos_aux0.cur);
+ proto_send1w ('Y', aux0.pos);
main_stat_aux_pos_cpt = main_stat_aux_pos;
}
if (main_stat_speed && !--main_stat_speed_cpt)
@@ -383,6 +385,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v32 (0, args[8], args[9], 0),
args[10]);
break;
+ case c ('y', 3):
+ /* Auxiliary go to position.
+ * - w: pos, i16.
+ * - b: sequence number. */
+ if (args[2] == state_aux0.sequence)
+ break;
+ aux_traj_goto_start (v8_to_v16 (args[0], args[1]), args[2]);
+ break;
case c ('a', 2):
/* Set both acknoledge.
* - b: main ack sequence number.
@@ -408,7 +418,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Position stats. */
main_stat_postrack_cpt = main_stat_postrack = args[0];
break;
- case c ('Z', 1):
+ case c ('Y', 1):
/* Auxiliary position stats. */
main_stat_aux_pos_cpt = main_stat_aux_pos = args[0];
break;
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index c7564766..ffc55061 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -38,6 +38,7 @@
#include "speed.h"
#include "postrack.h"
#include "traj.h"
+#include "aux.h"
struct twi_proto_t
{
@@ -83,8 +84,8 @@ twi_proto_update (void)
status[7] = v32_to_v8 (postrack_y, 1);
status[8] = v32_to_v8 (postrack_a, 2);
status[9] = v32_to_v8 (postrack_a, 1);
- status[10] = v16_to_v8 (pos_aux0.cons, 1);
- status[11] = v16_to_v8 (pos_aux0.cons, 0);
+ status[10] = v16_to_v8 (aux0.pos, 1);
+ status[11] = v16_to_v8 (aux0.pos, 0);
twi_sl_update (status, sizeof (status));
}
@@ -166,12 +167,8 @@ twi_proto_callback (u8 *buf, u8 size)
/* Move the arm.
* - w: new position.
* - b: speed. */
- speed_aux0.use_pos = 1;
- speed_aux0.pos_cons = pos_aux0.cons;
- speed_aux0.pos_cons += v8_to_v32 (0, 0, buf[2], buf[3]);
speed_aux0.max = buf[4];
- speed_aux0.slow = buf[4];
- state_start (&state_aux0, MODE_SPEED, 0);
+ aux_traj_goto_start (v8_to_v16 (buf[2], buf[3]), 0);
break;
case c ('p', x):
/* Set parameters. */