summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-29 23:03:56 +0200
committerNicolas Schodet2010-04-29 23:03:56 +0200
commitdf7d0d94ecc69fa9aadd2ab90db89b5ef1a74030 (patch)
tree7af42d2bd652fdf0f4fbd308d3df06d8a51299c5
parentd467bcadf9322a48fa7519907430fa5fb089329e (diff)
digital/mimot/src/dirty: add copy/pasted firmware for mimot, refs #123
This is a quick and dirty firmware copied from asserv. This will be improved once #124 is closed.
-rw-r--r--digital/mimot/src/dirty/Makefile16
-rw-r--r--digital/mimot/src/dirty/aux.c196
-rw-r--r--digital/mimot/src/dirty/aux.h68
-rw-r--r--digital/mimot/src/dirty/avrconfig.h104
-rw-r--r--digital/mimot/src/dirty/contacts.h32
-rw-r--r--digital/mimot/src/dirty/counter.h37
-rw-r--r--digital/mimot/src/dirty/counter_ext.avr.c142
-rw-r--r--digital/mimot/src/dirty/eeprom.avr.c116
-rw-r--r--digital/mimot/src/dirty/eeprom.h40
-rw-r--r--digital/mimot/src/dirty/main.c453
-rw-r--r--digital/mimot/src/dirty/models.host.c104
-rw-r--r--digital/mimot/src/dirty/models.host.h50
-rw-r--r--digital/mimot/src/dirty/motor_model.host.c88
-rw-r--r--digital/mimot/src/dirty/motor_model.host.h65
-rw-r--r--digital/mimot/src/dirty/pos.c145
-rw-r--r--digital/mimot/src/dirty/pos.h61
-rw-r--r--digital/mimot/src/dirty/pwm.avr.c57
-rw-r--r--digital/mimot/src/dirty/pwm.h98
-rw-r--r--digital/mimot/src/dirty/pwm_config.h41
-rw-r--r--digital/mimot/src/dirty/pwm_ocr.avr.c161
-rw-r--r--digital/mimot/src/dirty/pwm_ocr.avr.h37
-rw-r--r--digital/mimot/src/dirty/simu.host.c261
-rw-r--r--digital/mimot/src/dirty/simu.host.h48
-rw-r--r--digital/mimot/src/dirty/speed.c130
-rw-r--r--digital/mimot/src/dirty/speed.h57
-rw-r--r--digital/mimot/src/dirty/state.c29
-rw-r--r--digital/mimot/src/dirty/state.h124
-rw-r--r--digital/mimot/src/dirty/timer.avr.c51
-rw-r--r--digital/mimot/src/dirty/timer.h41
-rw-r--r--digital/mimot/src/dirty/twi_proto.c173
-rw-r--r--digital/mimot/src/dirty/twi_proto.h34
31 files changed, 3059 insertions, 0 deletions
diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile
new file mode 100644
index 00000000..979aae7e
--- /dev/null
+++ b/digital/mimot/src/dirty/Makefile
@@ -0,0 +1,16 @@
+BASE = ../../../avr
+PROGS = dirty
+dirty_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c \
+ pwm_ocr.avr.c pos.c speed.c aux.c \
+ twi_proto.c eeprom.avr.c state.c \
+ simu.host.c motor_model.host.c models.host.c
+MODULES = proto uart utils math/fixed twi
+CONFIGFILE = avrconfig.h
+# atmega8, atmega8535, atmega128...
+AVR_MCU = atmega32
+# -O2 : speed
+# -Os : size
+OPTIMIZE = -Os
+HOST_LIBS = -lm
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
new file mode 100644
index 00000000..d262cba0
--- /dev/null
+++ b/digital/mimot/src/dirty/aux.c
@@ -0,0 +1,196 @@
+/* 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 "modules/utils/utils.h"
+#include "io.h"
+
+#include "state.h"
+
+#include "counter.h"
+#include "pos.h"
+#include "speed.h"
+
+#include "contacts.h"
+
+#ifdef HOST
+# include "simu.host.h"
+#endif
+
+/** Motors states. */
+struct aux_t aux[AC_ASSERV_AUX_NB];
+
+/** Trajectory modes. */
+enum
+{
+ /* Goto position, with blocking detection. */
+ AUX_TRAJ_GOTO,
+ /* Goto position, try to unblock. */
+ AUX_TRAJ_GOTO_UNBLOCK,
+ /* Find zero mode, turn until zero is not seen. */
+ AUX_TRAJ_FIND_ZERO_NOT,
+ /* Find zero mode, turn until zero is seen. */
+ AUX_TRAJ_FIND_ZERO,
+ /* Everything done. */
+ AUX_TRAJ_DONE,
+};
+
+/** Initialise motors states. */
+void
+aux_init (void)
+{
+ aux[0].state = &state_aux[0];
+ aux[0].speed = &speed_aux[0];
+ aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO);
+ aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO);
+ aux[0].handle_blocking = 0;
+ aux[1].state = &state_aux[1];
+ aux[1].speed = &speed_aux[1];
+ aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO);
+ aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO);
+ aux[1].handle_blocking = 0;
+}
+
+/** Update positions. */
+void
+aux_pos_update (void)
+{
+ uint8_t i;
+ /* Easy... */
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ aux[i].pos += counter_aux_diff[i];
+}
+
+/** Goto position. */
+void
+aux_traj_goto (struct aux_t *aux)
+{
+ switch (aux->traj_mode)
+ {
+ case AUX_TRAJ_GOTO:
+ if (aux->speed->pos->blocked_counter
+ > aux->speed->pos->blocked_counter_limit)
+ {
+ aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK;
+ aux->speed->pos_cons = aux->speed->pos->cur;
+ aux->speed->pos_cons -= 250;
+ aux->wait = 225 / 2;
+ }
+ else if (UTILS_ABS (aux->speed->pos_cons - aux->speed->pos->cur)
+ < 300)
+ {
+ aux->traj_mode = AUX_TRAJ_DONE;
+ aux->state->variant = 0;
+ state_finish (aux->state);
+ }
+ break;
+ case AUX_TRAJ_GOTO_UNBLOCK:
+ if (!--aux->wait)
+ {
+ aux->traj_mode = AUX_TRAJ_GOTO;
+ aux->speed->pos_cons = aux->goto_pos;
+ }
+ break;
+ }
+}
+
+void
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq)
+{
+ aux->traj_mode = AUX_TRAJ_GOTO;
+ aux->speed->use_pos = 1;
+ aux->speed->pos_cons = aux->speed->pos->cur;
+ aux->speed->pos_cons += (int16_t) (pos - aux->pos);
+ aux->goto_pos = aux->speed->pos_cons;
+ state_start (aux->state, MODE_TRAJ, seq);
+ if (aux->handle_blocking)
+ aux->state->variant = 4;
+}
+
+/** Find zero mode. */
+void
+aux_traj_find_zero (struct aux_t *aux)
+{
+ uint8_t zero = *aux->zero_pin & aux->zero_bv;
+ switch (aux->traj_mode)
+ {
+ case AUX_TRAJ_FIND_ZERO_NOT:
+ if (zero)
+ aux->traj_mode = AUX_TRAJ_FIND_ZERO;
+ break;
+ case AUX_TRAJ_FIND_ZERO:
+ if (!zero)
+ {
+ aux->speed->cons = 0;
+ state_finish (aux->state);
+ aux->pos = 0;
+ aux->traj_mode = AUX_TRAJ_DONE;
+ }
+ break;
+ }
+}
+
+/** Start find zero mode. */
+void
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+{
+ aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT;
+ aux->speed->use_pos = 0;
+ aux->speed->cons = speed << 8;
+ state_start (aux->state, MODE_TRAJ, seq);
+}
+
+/** Update trajectories for one motor. */
+static void
+aux_traj_update_single (struct aux_t *aux)
+{
+ if (aux->state->mode >= MODE_TRAJ)
+ {
+ switch (aux->traj_mode)
+ {
+ case AUX_TRAJ_GOTO:
+ case AUX_TRAJ_GOTO_UNBLOCK:
+ aux_traj_goto (aux);
+ break;
+ case AUX_TRAJ_FIND_ZERO_NOT:
+ case AUX_TRAJ_FIND_ZERO:
+ aux_traj_find_zero (aux);
+ break;
+ case AUX_TRAJ_DONE:
+ break;
+ }
+ }
+}
+
+/** Update trajectories. */
+void
+aux_traj_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ aux_traj_update_single (&aux[i]);
+}
+
diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
new file mode 100644
index 00000000..65456ec9
--- /dev/null
+++ b/digital/mimot/src/dirty/aux.h
@@ -0,0 +1,68 @@
+#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
+{
+ /** Associated state. */
+ struct state_t *state;
+ /** Controlled speed. */
+ struct speed_t *speed;
+ /** Absolute position. */
+ int16_t pos;
+ /** Trajectory mode. */
+ uint8_t traj_mode;
+ /** Goto position position. */
+ uint32_t goto_pos;
+ /** Wait counter. */
+ uint16_t wait;
+ /** Top zero port input register. */
+ volatile uint8_t *zero_pin;
+ /** Top zero port bit value. */
+ uint8_t zero_bv;
+ /** Handle blocking by aux instead of pos. */
+ uint8_t handle_blocking;
+};
+
+extern struct aux_t aux[AC_ASSERV_AUX_NB];
+
+void
+aux_init (void);
+
+void
+aux_pos_update (void);
+
+void
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq);
+
+void
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+
+void
+aux_traj_update (void);
+
+#endif /* aux_h */
diff --git a/digital/mimot/src/dirty/avrconfig.h b/digital/mimot/src/dirty/avrconfig.h
new file mode 100644
index 00000000..c7cc73f3
--- /dev/null
+++ b/digital/mimot/src/dirty/avrconfig.h
@@ -0,0 +1,104 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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.
+ *
+ * }}} */
+
+/* global */
+/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800,
+ * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */
+#define AC_FREQ 14745600
+
+/* uart - UART module. */
+/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */
+#define AC_UART0_PORT 0
+/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800,
+ * 115200, 230400, 250000, 500000, 1000000. */
+#define AC_UART0_BAUDRATE 38400
+/** Send mode:
+ * - POLLING: no interrupts.
+ * - RING: interrupts, ring buffer. */
+#define AC_UART0_SEND_MODE RING
+/** Recv mode, same as send mode. */
+#define AC_UART0_RECV_MODE RING
+/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */
+#define AC_UART0_CHAR_SIZE 8
+/** Parity : ODD, EVEN, NONE. */
+#define AC_UART0_PARITY EVEN
+/** Stop bits : 1, 2. */
+#define AC_UART0_STOP_BITS 1
+/** Send buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_SEND_BUFFER_SIZE 32
+/** Recv buffer size, should be power of 2 for RING mode. */
+#define AC_UART0_RECV_BUFFER_SIZE 32
+/** If the send buffer is full when putc:
+ * - DROP: drop the new byte.
+ * - WAIT: wait until there is room in the send buffer. */
+#define AC_UART0_SEND_BUFFER_FULL DROP
+/** In HOST compilation:
+ * - STDIO: use stdin/out.
+ * - PTS: use pseudo terminal. */
+#define AC_UART0_HOST_DRIVER STDIO
+/** Same thing for secondary port. */
+#define AC_UART1_PORT -1
+#define AC_UART1_BAUDRATE 115200
+#define AC_UART1_SEND_MODE RING
+#define AC_UART1_RECV_MODE RING
+#define AC_UART1_CHAR_SIZE 8
+#define AC_UART1_PARITY EVEN
+#define AC_UART1_STOP_BITS 1
+#define AC_UART1_SEND_BUFFER_SIZE 32
+#define AC_UART1_RECV_BUFFER_SIZE 32
+#define AC_UART1_SEND_BUFFER_FULL WAIT
+#define AC_UART1_HOST_DRIVER PTS
+
+/* proto - Protocol module. */
+/** Maximum argument size. */
+#define AC_PROTO_ARGS_MAX_SIZE 12
+/** Callback function name. */
+#define AC_PROTO_CALLBACK proto_callback
+/** Putchar function name. */
+#define AC_PROTO_PUTC uart0_putc
+/** Support for quote parameter. */
+#define AC_PROTO_QUOTE 1
+
+/* asserv. */
+/** Number of auxiliary motors. */
+#define AC_ASSERV_AUX_NB 2
+/** Use external counters. */
+#define AC_ASSERV_COUNTER_EXTERNAL 1
+/** TWI address. */
+#define AC_ASSERV_TWI_ADDRESS 6
+
+/* twi - TWI module. */
+/** Activate master part. */
+#define AC_TWI_MASTER_ENABLE 0
+/** Activate slave part. */
+#define AC_TWI_SLAVE_ENABLE 1
+/** Slave recv buffer size. */
+#define AC_TWI_SL_RECV_BUFFER_SIZE 16
+/** Slave send buffer size. */
+#define AC_TWI_SL_SEND_BUFFER_SIZE 16
+
+#endif /* avrconfig_h */
diff --git a/digital/mimot/src/dirty/contacts.h b/digital/mimot/src/dirty/contacts.h
new file mode 100644
index 00000000..1de0c57d
--- /dev/null
+++ b/digital/mimot/src/dirty/contacts.h
@@ -0,0 +1,32 @@
+#ifndef contacts_h
+#define contacts_h
+/* contacts.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2009 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.
+ *
+ * }}} */
+
+/** Define contacts. */
+#define CONTACT_AUX0_ZERO_IO C, 0
+#define CONTACT_AUX1_ZERO_IO C, 1
+
+#endif /* contacts_h */
diff --git a/digital/mimot/src/dirty/counter.h b/digital/mimot/src/dirty/counter.h
new file mode 100644
index 00000000..39d596c5
--- /dev/null
+++ b/digital/mimot/src/dirty/counter.h
@@ -0,0 +1,37 @@
+#ifndef counter_h
+#define counter_h
+/* counter.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.
+ *
+ * }}} */
+
+extern uint16_t counter_aux[AC_ASSERV_AUX_NB];
+extern int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+
+void
+counter_init (void);
+
+void
+counter_update (void);
+
+#endif /* counter_h */
diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c
new file mode 100644
index 00000000..1bbe9a38
--- /dev/null
+++ b/digital/mimot/src/dirty/counter_ext.avr.c
@@ -0,0 +1,142 @@
+/* counter_ext.avr.c - External counter support. */
+/* 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 "counter.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+#include "io.h"
+
+/**
+ * This file add support for an external counter like the hdlcounter or
+ * avrcounter project. This can be better in order not to loose steps and
+ * support more counters.
+ */
+
+/** Define the first auxiliary counter address. */
+#define COUNTER_AUX0 0
+/** Define the second auxiliary counter address. */
+#define COUNTER_AUX1 1
+
+/** Define to 1 to reverse the first auxiliary counter. */
+#define COUNTER_AUX0_REVERSE 0
+/** Define to 1 to reverse the second auxiliary counter. */
+#define COUNTER_AUX1_REVERSE 0
+
+/** Define to 1 to use the AVR External Memory system, or 0 to use hand made
+ * signals. */
+#define COUNTER_USE_XMEM 0
+
+/** Last values. */
+static uint8_t counter_aux_old[AC_ASSERV_AUX_NB];
+/** Overall counter values. */
+uint16_t counter_aux[AC_ASSERV_AUX_NB];
+/** Counter differences since last update.
+ * Maximum of 9 significant bits, sign included. */
+int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+
+#if !COUNTER_USE_XMEM
+# define COUNTER_ALE_IO B, 4
+# define COUNTER_RD_IO D, 6
+#endif
+
+/** Read an external counter. */
+static inline uint8_t
+counter_read (uint8_t n)
+{
+#if COUNTER_USE_XMEM
+ uint8_t * const ext = (void *) 0x1100;
+ return ext[n];
+#else
+ uint8_t v;
+ PORTA = (PORTA & 0xf0) | (n & 0x0f);
+ IO_CLR (COUNTER_ALE_IO);
+ PORTA &= 0xf0;
+ DDRA &= 0xf0;
+ DDRB &= 0xf0;
+ IO_CLR (COUNTER_RD_IO);
+ utils_nop ();
+ utils_nop ();
+ v = (PINA & 0x0f) | (PINB & 0x0f) << 4;
+ IO_SET (COUNTER_RD_IO);
+ IO_SET (COUNTER_ALE_IO);
+ DDRA |= 0x0f;
+ DDRB |= 0x0f;
+ return v;
+#endif
+}
+
+/** Initialize the counters. */
+void
+counter_init (void)
+{
+#if COUNTER_USE_XMEM
+ /* Long wait-states. */
+ XMCRA = _BV (SRW11);
+ /* Do not use port C for address. */
+ XMCRB = _BV (XMM2) | _BV (XMM1) | _BV (XMM0);
+ /* Long wait-states and enable. */
+ MCUCR |= _BV (SRE) | _BV (SRW10);
+#else
+ IO_SET (COUNTER_ALE_IO);
+ IO_SET (COUNTER_RD_IO);
+ IO_OUTPUT (COUNTER_ALE_IO);
+ IO_OUTPUT (COUNTER_RD_IO);
+ PORTA &= 0xf0;
+ PORTB &= 0xf0;
+ DDRA |= 0x0f;
+ DDRB |= 0x0f;
+#endif
+ /* Begin with safe values. */
+ counter_aux_old[0] = counter_read (COUNTER_AUX0);
+ counter_aux_old[1] = counter_read (COUNTER_AUX1);
+}
+
+/** Update overall counter values and compute diffs. */
+void
+counter_update (void)
+{
+ uint8_t aux0, aux1;
+ /* Sample counters. */
+ aux0 = counter_read (COUNTER_AUX0);
+ aux1 = counter_read (COUNTER_AUX1);
+ /* First auxiliary counter. */
+#if !COUNTER_AUX0_REVERSE
+ counter_aux_diff[0] = (int8_t) (aux0 - counter_aux_old[0]);
+#else
+ counter_aux_diff[0] = (int8_t) (counter_aux_old[0] - aux0);
+#endif
+ counter_aux_old[0] = aux0;
+ counter_aux[0] += counter_aux_diff[0];
+ /* Second auxiliary counter. */
+#if !COUNTER_AUX1_REVERSE
+ counter_aux_diff[1] = (int8_t) (aux1 - counter_aux_old[1]);
+#else
+ counter_aux_diff[1] = (int8_t) (counter_aux_old[1] - aux1);
+#endif
+ counter_aux_old[1] = aux1;
+ counter_aux[1] += counter_aux_diff[1];
+}
+
diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c
new file mode 100644
index 00000000..a20032d1
--- /dev/null
+++ b/digital/mimot/src/dirty/eeprom.avr.c
@@ -0,0 +1,116 @@
+/* eeprom.avr.c - Save parameters to internal EEPROM. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "eeprom.h"
+
+#include "modules/utils/byte.h"
+
+#include <avr/eeprom.h>
+
+#include "counter.h"
+#include "pwm.h"
+#include "pos.h"
+#include "speed.h"
+
+#define EEPROM_START 0
+
+/* WARNING:
+ * If you change EEPROM format, be sure to change the EEPROM_KEY in header if
+ * your new format is not compatible with the old one or you will load
+ * garbages in parameters. */
+
+/* Read parameters from eeprom. */
+void
+eeprom_read_params (void)
+{
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
+ uint16_t *p16;
+ if (eeprom_read_byte (p8++) != EEPROM_KEY)
+ return;
+ speed_aux[0].max = eeprom_read_byte (p8++);
+ speed_aux[1].max = eeprom_read_byte (p8++);
+ speed_aux[0].slow = eeprom_read_byte (p8++);
+ speed_aux[1].slow = eeprom_read_byte (p8++);
+ pwm_set_reverse (eeprom_read_byte (p8++));
+ p16 = (uint16_t *) p8;
+ speed_aux[0].acc = eeprom_read_word (p16++);
+ speed_aux[1].acc = eeprom_read_word (p16++);
+ pos_aux[0].kp = eeprom_read_word (p16++);
+ pos_aux[1].kp = eeprom_read_word (p16++);
+ pos_aux[0].ki = eeprom_read_word (p16++);
+ pos_aux[1].ki = eeprom_read_word (p16++);
+ pos_aux[0].kd = eeprom_read_word (p16++);
+ pos_aux[1].kd = eeprom_read_word (p16++);
+ pos_aux[0].blocked_error_limit = eeprom_read_word (p16++);
+ pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++);
+ pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_error_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++);
+ pos_e_sat = eeprom_read_word (p16++);
+ pos_i_sat = eeprom_read_word (p16++);
+ pos_d_sat = eeprom_read_word (p16++);
+}
+
+/* Write parameters to eeprom. */
+void
+eeprom_write_params (void)
+{
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
+ uint16_t *p16;
+ eeprom_write_byte (p8++, EEPROM_KEY);
+ eeprom_write_byte (p8++, speed_aux[0].max);
+ eeprom_write_byte (p8++, speed_aux[1].max);
+ eeprom_write_byte (p8++, speed_aux[0].slow);
+ eeprom_write_byte (p8++, speed_aux[1].slow);
+ eeprom_write_byte (p8++, pwm_reverse);
+ p16 = (uint16_t *) p8;
+ eeprom_write_word (p16++, speed_aux[0].acc);
+ eeprom_write_word (p16++, speed_aux[1].acc);
+ eeprom_write_word (p16++, pos_aux[0].kp);
+ eeprom_write_word (p16++, pos_aux[1].kp);
+ eeprom_write_word (p16++, pos_aux[0].ki);
+ eeprom_write_word (p16++, pos_aux[1].ki);
+ eeprom_write_word (p16++, pos_aux[0].kd);
+ eeprom_write_word (p16++, pos_aux[1].kd);
+ eeprom_write_word (p16++, pos_aux[0].blocked_error_limit);
+ eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit);
+ eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_error_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit);
+ eeprom_write_word (p16++, pos_e_sat);
+ eeprom_write_word (p16++, pos_i_sat);
+ eeprom_write_word (p16++, pos_d_sat);
+}
+
+/* Clear eeprom parameters. */
+void
+eeprom_clear_params (void)
+{
+ uint8_t *p = (uint8_t *) EEPROM_START;
+ eeprom_write_byte (p, 0xff);
+}
+
diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h
new file mode 100644
index 00000000..d061a9fd
--- /dev/null
+++ b/digital/mimot/src/dirty/eeprom.h
@@ -0,0 +1,40 @@
+#ifndef eeprom_h
+#define eeprom_h
+/* eeprom.h - Save parameters to internal EEPROM. */
+/* 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.
+ *
+ * }}} */
+
+/** Change the eeprom key each time you change eeprom format. */
+#define EEPROM_KEY 0x01
+
+void
+eeprom_read_params (void);
+
+void
+eeprom_write_params (void);
+
+void
+eeprom_clear_params (void);
+
+#endif /* eeprom_h */
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
new file mode 100644
index 00000000..9b674461
--- /dev/null
+++ b/digital/mimot/src/dirty/main.c
@@ -0,0 +1,453 @@
+/* main.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "modules/uart/uart.h"
+#include "modules/proto/proto.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/byte.h"
+#include "modules/math/fixed/fixed.h"
+#include "io.h"
+
+#include "state.h"
+
+#include "counter.h"
+#include "pwm.h"
+#include "pos.h"
+#include "speed.h"
+#include "aux.h"
+
+#include "twi_proto.h"
+#include "eeprom.h"
+
+#ifndef HOST
+# include "timer.h"
+#else
+# include "simu.host.h"
+#endif
+
+/** Report command completion. */
+uint8_t main_sequence_ack = 4, main_sequence_ack_cpt;
+
+/** Report of counters. */
+uint8_t main_stat_counter, main_stat_counter_cpt;
+
+/** Report of auxiliary position. */
+uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt;
+
+/** Statistics about speed control. */
+uint8_t main_stat_speed, main_stat_speed_cpt;
+
+/** Statistics about shaft position control. */
+uint8_t main_stat_pos, main_stat_pos_cpt;
+
+/** Statistics about pwm values. */
+uint8_t main_stat_pwm, main_stat_pwm_cpt;
+
+/** Report of timer. */
+uint8_t main_stat_timer, main_stat_timer_cpt;
+
+/** Print input port. */
+uint8_t main_print_pin, main_print_pin_cpt;
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t main_timer[6];
+
+static void
+main_loop (void);
+
+/** Entry point. */
+int
+main (int argc, char **argv)
+{
+ avr_init (argc, argv);
+ /* Pull-ups. */
+ PORTB = 0xe0;
+ PORTC = 0xfc;
+ PORTD = 0x80;
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ uart0_init ();
+ twi_proto_init ();
+ speed_init ();
+ aux_init ();
+ eeprom_read_params ();
+ proto_send0 ('z');
+ sei ();
+ while (1)
+ main_loop ();
+ return 0;
+}
+
+/** Main loop. */
+static void
+main_loop (void)
+{
+ main_timer[5] = timer_read ();
+ timer_wait ();
+ /* Counter update. */
+ counter_update ();
+ main_timer[0] = timer_read ();
+ /* Position control. */
+ pos_update ();
+ main_timer[1] = timer_read ();
+ /* Pwm setup. */
+ pwm_update ();
+ main_timer[2] = timer_read ();
+ /* Compute absolute position. */
+ aux_pos_update ();
+ /* Compute trajectory. */
+ aux_traj_update ();
+ /* Speed control. */
+ speed_update ();
+ main_timer[3] = timer_read ();
+ /* Stats. */
+ if (main_sequence_ack
+ && (state_aux[0].sequence_ack != state_aux[0].sequence_finish
+ || state_aux[1].sequence_ack != state_aux[1].sequence_finish)
+ && !--main_sequence_ack_cpt)
+ {
+ proto_send2b ('A', state_aux[0].sequence_finish,
+ state_aux[1].sequence_finish);
+ main_sequence_ack_cpt = main_sequence_ack;
+ }
+ if (main_stat_counter && !--main_stat_counter_cpt)
+ {
+ proto_send2w ('C', counter_aux[0], counter_aux[1]);
+ main_stat_counter_cpt = main_stat_counter;
+ }
+ if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
+ {
+ proto_send2w ('Y', aux[0].pos, aux[1].pos);
+ main_stat_aux_pos_cpt = main_stat_aux_pos;
+ }
+ if (main_stat_speed && !--main_stat_speed_cpt)
+ {
+ proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ main_stat_speed_cpt = main_stat_speed;
+ }
+ if (main_stat_pos && !--main_stat_pos_cpt)
+ {
+ proto_send4w ('P', pos_aux[0].e_old, pos_aux[0].i,
+ pos_aux[1].e_old, pos_aux[1].i);
+ main_stat_pos_cpt = main_stat_pos;
+ }
+ if (main_stat_pwm && !--main_stat_pwm_cpt)
+ {
+ proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur);
+ main_stat_pwm_cpt = main_stat_pwm;
+ }
+ if (main_stat_timer && !--main_stat_timer_cpt)
+ {
+ proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3],
+ main_timer[4], main_timer[4], main_timer[5]);
+ main_stat_timer_cpt = main_stat_timer;
+ }
+ if (main_print_pin && !--main_print_pin_cpt)
+ {
+ proto_send1b ('I', PINC & 0xfc);
+ main_print_pin_cpt = main_print_pin;
+ }
+ /* Misc. */
+ while (uart0_poll ())
+ proto_accept (uart0_getc ());
+ twi_proto_update ();
+ main_timer[4] = timer_read ();
+}
+
+/** Handle incoming messages. */
+void
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
+{
+ /* Many commands use the first argument as a selector. */
+ struct aux_t *auxp = 0;
+ struct pwm_t *pwm = 0;
+ struct pos_t *pos = 0;
+ struct speed_t *speed = 0;
+ struct state_t *state = 0;
+ if (args[0] < AC_ASSERV_AUX_NB)
+ {
+ auxp = &aux[args[0]];
+ pwm = &pwm_aux[args[0]];
+ pos = &pos_aux[args[0]];
+ speed = &speed_aux[args[0]];
+ state = &state_aux[args[0]];
+ }
+ /* Decode command. */
+#define c(cmd, size) (cmd << 8 | size)
+ switch (c (cmd, size))
+ {
+ case c ('z', 0):
+ /* Reset. */
+ utils_reset ();
+ break;
+ /* Commands. */
+ case c ('w', 0):
+ /* Set zero auxiliary pwm. */
+ pos_reset (&pos_aux[0]);
+ pos_reset (&pos_aux[1]);
+ state_aux[0].mode = MODE_PWM;
+ state_aux[1].mode = MODE_PWM;
+ pwm_set (&pwm_aux[0], 0);
+ pwm_set (&pwm_aux[1], 0);
+ break;
+ case c ('w', 3):
+ /* Set auxiliary pwm.
+ * - b: aux index.
+ * - w: pwm. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ pos_reset (pos);
+ state->mode = MODE_PWM;
+ pwm_set (pwm, v8_to_v16 (args[1], args[2]));
+ break;
+ case c ('c', 3):
+ /* Add to auxiliary position consign.
+ * - b: aux index.
+ * - w: consign offset. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ state->mode = MODE_POS;
+ state->variant = 0;
+ pos->cons += v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('s', 1):
+ /* Set auxiliary zero speed.
+ * - b: aux index. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ state->mode = MODE_SPEED;
+ state->variant = 0;
+ speed->use_pos = 0;
+ speed->cons = 0;
+ break;
+ case c ('s', 2):
+ /* Set auxiliary speed.
+ * - b: aux index.
+ * - b: speed. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ state->mode = MODE_SPEED;
+ state->variant = 0;
+ speed->use_pos = 0;
+ speed->cons = args[1] << 8;
+ break;
+ case c ('s', 6):
+ /* Set auxiliary speed controlled position consign.
+ * - b: aux index.
+ * - d: consign offset.
+ * - b: sequence number. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ if (args[5] == state->sequence)
+ break;
+ speed->use_pos = 1;
+ speed->pos_cons = pos->cons;
+ speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]);
+ state_start (state, MODE_SPEED, args[5]);
+ break;
+ case c ('y', 4):
+ /* Auxiliary go to position.
+ * - b: aux index.
+ * - w: pos, i16.
+ * - b: sequence number. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ if (args[3] == state->sequence)
+ break;
+ aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
+ break;
+ case c ('y', 3):
+ /* Auxiliary find zero.
+ * - b: aux index.
+ * - b: speed.
+ * - b: sequence number. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ if (args[2] == state->sequence)
+ break;
+ aux_traj_find_zero_start (auxp, args[1], args[2]);
+ break;
+ case c ('a', 2):
+ /* Set all acknoledge.
+ * - b: first auxiliary ack sequence number.
+ * - b: second auxiliary ack sequence number. */
+ state_acknowledge (&state_aux[0], args[1]);
+ state_acknowledge (&state_aux[1], args[2]);
+ break;
+ /* Stats.
+ * - b: interval between stats. */
+ case c ('A', 1):
+ /* Command completion report. */
+ main_sequence_ack_cpt = main_sequence_ack = args[0];
+ break;
+ case c ('C', 1):
+ /* Counter stats. */
+ main_stat_counter_cpt = main_stat_counter = args[0];
+ break;
+ case c ('Y', 1):
+ /* Auxiliary position stats. */
+ main_stat_aux_pos_cpt = main_stat_aux_pos = args[0];
+ break;
+ case c ('S', 1):
+ /* Motor speed control stats. */
+ main_stat_speed_cpt = main_stat_speed = args[0];
+ break;
+ case c ('P', 1):
+ /* Auxiliary motor position control stats. */
+ main_stat_pos_cpt = main_stat_pos = args[0];
+ break;
+ case c ('W', 1):
+ /* Pwm stats. */
+ main_stat_pwm_cpt = main_stat_pwm = args[0];
+ break;
+ case c ('T', 1):
+ /* Timing stats. */
+ main_stat_timer_cpt = main_stat_timer = args[0];
+ break;
+ case c ('I', 1):
+ /* Input port stats. */
+ main_print_pin_cpt = main_print_pin = args[0];
+ break;
+ default:
+ /* Params. */
+ if (cmd == 'p')
+ {
+ /* Many commands use the first argument as a selector. */
+ switch (args[1])
+ {
+ case 0:
+ case 1:
+ pos = &pos_aux[args[1]];
+ speed = &speed_aux[args[1]];
+ break;
+ default:
+ pos = 0;
+ speed = 0;
+ break;
+ }
+ switch (c (args[0], size))
+ {
+ case c ('a', 4):
+ /* Set acceleration.
+ * - b: index.
+ * - w: acceleration. */
+ if (!speed) { proto_send0 ('?'); return; }
+ speed->acc = v8_to_v16 (args[2], args[3]);
+ break;
+ case c ('s', 4):
+ /* Set maximum and slow speed.
+ * - b: index.
+ * - b: max speed.
+ * - b: slow speed. */
+ if (!speed) { proto_send0 ('?'); return; }
+ speed->max = args[2];
+ speed->slow = args[3];
+ break;
+ case c ('p', 4):
+ /* Set proportional coefficient.
+ * - b: index.
+ * - w: P coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->kp = v8_to_v16 (args[2], args[3]);
+ break;
+ case c ('i', 4):
+ /* Set integral coefficient.
+ * - b: index.
+ * - w: I coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->ki = v8_to_v16 (args[2], args[3]);
+ break;
+ case c ('d', 4):
+ /* Set differential coefficient.
+ * - b: index.
+ * - w: D coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->kd = v8_to_v16 (args[2], args[3]);
+ break;
+ case c ('b', 7):
+ /* Set blocking detection parameters.
+ * - b: index.
+ * - w: error limit.
+ * - w: speed limit.
+ * - b: counter limit. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->blocked_error_limit = v8_to_v16 (args[2], args[3]);
+ pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]);
+ pos->blocked_counter_limit = args[6];
+ break;
+ case c ('E', 3):
+ pos_e_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('I', 3):
+ pos_i_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('D', 3):
+ pos_d_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('w', 2):
+ /* Set PWM direction.
+ * - b: bits: 0000[aux1][aux0][right][left]. */
+ pwm_set_reverse (args[1]);
+ break;
+ case c ('E', 2):
+ /* Write to eeprom.
+ * - b: 00: clear config, 01: write config. */
+ if (args[1])
+ eeprom_write_params ();
+ else
+ eeprom_clear_params ();
+ break;
+ case c ('P', 1):
+ /* Print current settings. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc);
+ proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow);
+ proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow);
+ proto_send3w ('b', pos_aux[0].blocked_error_limit,
+ pos_aux[0].blocked_speed_limit,
+ pos_aux[0].blocked_counter_limit);
+ proto_send3w ('b', pos_aux[1].blocked_error_limit,
+ pos_aux[1].blocked_speed_limit,
+ pos_aux[1].blocked_counter_limit);
+ proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp);
+ proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki);
+ proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd);
+ proto_send1w ('E', pos_e_sat);
+ proto_send1w ('I', pos_i_sat);
+ proto_send1w ('D', pos_d_sat);
+ proto_send1b ('w', pwm_reverse);
+ break;
+ default:
+ proto_send0 ('?');
+ return;
+ }
+ }
+ else
+ {
+ proto_send0 ('?');
+ return;
+ }
+ break;
+ }
+ proto_send (cmd, size, args);
+#undef c
+}
+
diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c
new file mode 100644
index 00000000..ab7242db
--- /dev/null
+++ b/digital/mimot/src/dirty/models.host.c
@@ -0,0 +1,104 @@
+/* models.host.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "motor_model.host.h"
+#include "models.host.h"
+#include "simu.host.h"
+
+#include <math.h>
+#include <string.h>
+
+/* Marcel clamp, with a Faulhaber 2342 and 23/1 3.71:1 gearbox model. */
+static const struct motor_def_t marcel_clamp_f2342_model =
+{
+ /* Motor characteristics. */
+ 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 26.10 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 7.10, /* Terminal resistance (Ohm). */
+ 0.265 / 1000, /* Terminal inductance (H). */
+ 24.0, /* Maximum voltage (V). */
+ /* Gearbox characteristics. */
+ 3.71, /* Gearbox ratio. */
+ 0.88, /* Gearbox efficiency. */
+ /* Load characteristics. */
+ 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */
+ /* This is a pifometric estimation. */
+};
+
+/* Marcel, APBTeam 2010. */
+static const struct robot_t marcel_robot =
+{
+ /** Auxiliary motors, NULL if not present. */
+ { &marcel_clamp_f2342_model, &marcel_clamp_f2342_model },
+ /** Number of steps for each auxiliary motor encoder. */
+ { 512, 512 },
+ /** Sensor update function. */
+ simu_sensor_update_marcel,
+};
+
+/* Table of models. */
+static const struct
+{
+ const char *name;
+ const struct robot_t *robot;
+} models[] = {
+ { "marcel", &marcel_robot },
+ { 0, 0 }
+};
+
+/** Get a pointer to a model by name, or return 0. */
+const struct robot_t *
+models_get (const char *name)
+{
+ int i;
+ for (i = 0; models[i].name; i++)
+ {
+ if (strcmp (models[i].name, name) == 0)
+ return models[i].robot;
+ }
+ return 0;
+}
+
+/** Initialise simulation models. */
+void
+models_init (const struct robot_t *robot, struct motor_t aux_motor[])
+{
+ int i;
+ if (aux_motor)
+ {
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ if (robot->aux_motor[i])
+ {
+ aux_motor[i].m = *robot->aux_motor[i];
+ aux_motor[i].h = ECHANT_PERIOD;
+ aux_motor[i].d = 1000;
+ }
+ }
+ }
+}
+
diff --git a/digital/mimot/src/dirty/models.host.h b/digital/mimot/src/dirty/models.host.h
new file mode 100644
index 00000000..0e53aaea
--- /dev/null
+++ b/digital/mimot/src/dirty/models.host.h
@@ -0,0 +1,50 @@
+#ifndef models_host_h
+#define models_host_h
+/* models.host.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+#define ECHANT_PERIOD (1.0 / ((double) AC_FREQ / 256 / 256))
+
+/** Define a robot and its peripherals.
+ * Encoder characteristics are defined at gearbox output. */
+struct robot_t
+{
+ /** Auxiliary motors, NULL if not present. */
+ const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB];
+ /** Number of steps for each auxiliary motor encoder. */
+ int aux_encoder_steps[AC_ASSERV_AUX_NB];
+ /** Sensor update function. */
+ void (*sensor_update) (void);
+};
+
+/** Get a pointer to a model by name, or return 0. */
+const struct robot_t *
+models_get (const char *name);
+
+/** Initialise simulation models. */
+void
+models_init (const struct robot_t *robot, struct motor_t aux_motor[]);
+
+#endif /* models_host_h */
diff --git a/digital/mimot/src/dirty/motor_model.host.c b/digital/mimot/src/dirty/motor_model.host.c
new file mode 100644
index 00000000..37171c32
--- /dev/null
+++ b/digital/mimot/src/dirty/motor_model.host.c
@@ -0,0 +1,88 @@
+/* motor_model.c - DC motor model. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "motor_model.host.h"
+
+/**
+ * Switching to french for all those non english speaking people.
+ *
+ * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux
+ * parties, la modélisation électrique et la modélisation mécanique.
+ *
+ * On peut trouver de l'aide sur :
+ * - le site de maxon : http://www.maxonmotor.com/
+ * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html
+ * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm
+ *
+ * Voici ce qui en résulte, des belles équations différentielles :
+ * u(t) = R i(t) + L di(t)/dt + 1/Ke o(t)
+ * J do(t)/dt = Kt i(t) - Rf o(t)
+ * dth(t)/dt = o(t)
+ *
+ * Les variables sont décrites dans la structure motor_t.
+ *
+ * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en
+ * exercice.
+ *
+ * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il
+ * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive
+ * à :
+ *
+ * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t))
+ * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t))
+ * th(t+h) = th(t) + h o(t)
+ *
+ * C'est consternant de simplicité non ?
+ */
+
+/** Make a simulation step. */
+void motor_model_step (struct motor_t *m)
+{
+ double i_, o_, th_; /* New computed values. */
+ double h; /* Infinitesimal step... Well, not so infinite. */
+ int d;
+ d = m->d;
+ h = m->h / d;
+ /* Make several small steps to increase precision. */
+ for (; d; d--)
+ {
+ /* Ah, the mystical power of computation. */
+ i_ = m->i
+ + h * (1.0 / m->m.L * m->u
+ - m->m.R / m->m.L * m->i
+ - 1.0 / m->m.Ke / m->m.L * m->o);
+ o_ = m->o
+ + h * m->m.i_G * m->m.i_G * m->m.ro_G / m->m.J
+ * (m->m.Kt * m->i - m->m.Rf * m->o);
+ th_ = m->th + h * m->o;
+ /* Ok, now store this step. */
+ m->i = i_;
+ m->o = o_;
+ m->th = th_;
+ }
+ /* Damn! It's finished yet! */
+ m->t += m->h;
+}
+
diff --git a/digital/mimot/src/dirty/motor_model.host.h b/digital/mimot/src/dirty/motor_model.host.h
new file mode 100644
index 00000000..3a04e15a
--- /dev/null
+++ b/digital/mimot/src/dirty/motor_model.host.h
@@ -0,0 +1,65 @@
+#ifndef motor_model_host_h
+#define motor_model_host_h
+/* motor_model.host.h - DC motor model. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+/** Motor and load characteristics. */
+struct motor_def_t
+{
+ /* Motor characteristics. */
+ double Ke; /* Speed constant ((rad/s)/V). */
+ double Kt; /* Torque constant (N.m/A). */
+ double Rf; /* Bearing friction (N.m/(rad/s)). */
+ double R; /* Terminal resistance (Ohm). */
+ double L; /* Terminal inductance (H). */
+ double u_max;/* Maximum voltage (V). */
+ /* Gearbox characteristics. */
+ double i_G; /* Gearbox ratio. */
+ double ro_G;/* Gearbox efficiency. */
+ /* Load characteristics. */
+ double J; /* Load at gearbox output (kg.m^2). */
+};
+
+/** Motor and load characteristics and current data. Angular speed and theta
+ * are at motor output, not gearbox output. */
+struct motor_t
+{
+ /* Motor and load characteristics. */
+ struct motor_def_t m;
+ /* Simulation parameters. */
+ double h; /* Simulation time step (s). */
+ int d; /* Simulation time step division. */
+ /* Simulation current state. */
+ double t; /* Current time (not really used) (s). */
+ double u; /* Current input voltage (V). */
+ double i; /* Current current (A). */
+ double o; /* Current angular speed (o for omega) (rad/s). */
+ double th; /* Current theta (th for theta) (rad). */
+};
+
+/** Make a simulation step. */
+void motor_model_step (struct motor_t *m);
+
+#endif /* motor_model_host_h */
diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c
new file mode 100644
index 00000000..ef6475c2
--- /dev/null
+++ b/digital/mimot/src/dirty/pos.c
@@ -0,0 +1,145 @@
+/* pos.c - Position motor control. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "pos.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "counter.h"
+#include "pwm.h"
+#include "state.h"
+
+/**
+ * This file is responsible for position motor control. The consign is a
+ * position of the motor shafts, as theta/alpha. Theta is the sum of right
+ * and left position, alpha is the difference between the right and the left
+ * position.
+ * 16 bits are enough as long as there is no long blocking (see 2005 cup!).
+ */
+
+/** Auxiliaries control states. */
+struct pos_t pos_aux[AC_ASSERV_AUX_NB];
+
+/** Error saturation. */
+int32_t pos_e_sat = 1023;
+/** Integral saturation. */
+int32_t pos_i_sat = 1023;
+/** Differential saturation. */
+int32_t pos_d_sat = 1023;
+
+/** Compute a PID.
+ * How to compute maximum numbers size:
+ * Result is 24 bits (16 bits kept after shift).
+ * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff
+ * max is 12 bits (can be saturated with d_sat).
+ * If i_sat == 1023, i max is 11 bits.
+ * In the final addition, let's give 23 bits to the p part, and 22 bits to the
+ * i and d part (23b + 22b + 22b => 23b + 23b => 24b).
+ * Therefore, kp can be 23 - 11 = 12 bits (f4.8).
+ * ki can be 22 - 11 = 11 bits (f3.8).
+ * kd can be 22 - 12 = 10 bits (f2.8).
+ * How to increase this number:
+ * - lower the shift.
+ * - bound the value returned.
+ * - lower e, i & d saturation. */
+static inline int16_t
+pos_compute_pid (int32_t e, struct pos_t *pos)
+{
+ int32_t diff, pid;
+ /* Saturate error. */
+ UTILS_BOUND (e, -pos_e_sat, pos_e_sat);
+ /* Integral update. */
+ pos->i += e;
+ UTILS_BOUND (pos->i, -pos_i_sat, pos_i_sat);
+ /* Differential value. */
+ diff = e - pos->e_old;
+ UTILS_BOUND (diff, -pos_d_sat, pos_d_sat);
+ /* Compute PID. */
+ pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd;
+ /* Save result. */
+ pos->e_old = e;
+ return pid >> 8;
+}
+
+/** Update PWM for a single motor system. */
+static void
+pos_update_single (struct state_t *state, struct pos_t *pos,
+ int16_t counter_diff, struct pwm_t *pwm)
+{
+ if (state->mode >= MODE_POS)
+ {
+ int16_t pid;
+ int32_t error;
+ /* Update current shaft position. */
+ pos->cur += counter_diff;
+ /* Compute error. */
+ error = pos->cons - pos->cur;
+ /* Test or blocking. */
+ if (UTILS_ABS (error) > pos->blocked_error_limit
+ && UTILS_ABS (counter_diff) < pos->blocked_speed_limit)
+ pos->blocked_counter++;
+ else
+ pos->blocked_counter = 0;
+ if (!(state->variant & 4)
+ && pos->blocked_counter > pos->blocked_counter_limit)
+ {
+ /* Blocked. */
+ pos_reset (pos);
+ state_blocked (state);
+ pwm_set (pwm, 0);
+ }
+ else
+ {
+ /* Compute PID. */
+ pid = pos_compute_pid (error, pos);
+ /* Update PWM. */
+ pwm_set (pwm, pid);
+ }
+ }
+}
+
+/** Update PWM according to consign. */
+void
+pos_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i],
+ &pwm_aux[i]);
+}
+
+/** Reset position control state. To be called when the position control is
+ * deactivated. */
+void
+pos_reset (struct pos_t *pos)
+{
+ pos->i = 0;
+ pos->cur = 0;
+ pos->cons = 0;
+ pos->e_old = 0;
+ pos->blocked_counter = 0;
+}
+
diff --git a/digital/mimot/src/dirty/pos.h b/digital/mimot/src/dirty/pos.h
new file mode 100644
index 00000000..514b091c
--- /dev/null
+++ b/digital/mimot/src/dirty/pos.h
@@ -0,0 +1,61 @@
+#ifndef pos_h
+#define pos_h
+/* pos.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.
+ *
+ * }}} */
+
+/** Position control state. */
+struct pos_t
+{
+ /** Current position. */
+ uint32_t cur;
+ /** Consign position. */
+ uint32_t cons;
+ /** PID coefficients (f8.8, maximum depends on saturation values). */
+ uint16_t kp, ki, kd;
+ /** Current integral value. */
+ int32_t i;
+ /** Last error value. */
+ int32_t e_old;
+ /** Blocking detection: error limit. */
+ int32_t blocked_error_limit;
+ /** Blocking detection: speed limit. */
+ int16_t blocked_speed_limit;
+ /** Blocking detection: counter limit. */
+ uint8_t blocked_counter_limit;
+ /** Count the number of blocked detection. */
+ uint8_t blocked_counter;
+};
+
+extern struct pos_t pos_aux[AC_ASSERV_AUX_NB];
+
+extern int32_t pos_e_sat, pos_i_sat, pos_d_sat;
+
+void
+pos_update (void);
+
+void
+pos_reset (struct pos_t *pos);
+
+#endif /* pos_h */
diff --git a/digital/mimot/src/dirty/pwm.avr.c b/digital/mimot/src/dirty/pwm.avr.c
new file mode 100644
index 00000000..37ec34b0
--- /dev/null
+++ b/digital/mimot/src/dirty/pwm.avr.c
@@ -0,0 +1,57 @@
+/* pwm.avr.c - Handle all PWM generators. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "pwm.h"
+#include "pwm_ocr.avr.h"
+
+/** PWM control states. */
+struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
+ PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
+};
+/** PWM reverse directions. */
+uint8_t pwm_reverse;
+
+/** Initialise PWM generators. */
+void
+pwm_init (void)
+{
+ pwm_ocr_init ();
+}
+
+/** Update the hardware PWM values. */
+void
+pwm_update (void)
+{
+ pwm_ocr_update ();
+}
+
+/** Set which PWM is reversed. */
+void
+pwm_set_reverse (uint8_t reverse)
+{
+ pwm_reverse = reverse;
+ pwm_ocr_set_reverse (reverse);
+}
+
diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h
new file mode 100644
index 00000000..a751ee9e
--- /dev/null
+++ b/digital/mimot/src/dirty/pwm.h
@@ -0,0 +1,98 @@
+#ifndef pwm_h
+#define pwm_h
+/* pwm.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.
+ *
+ * }}} */
+
+/** Define the absolute maximum PWM value. */
+#define PWM_MAX 0x3ff
+
+/** PWM control state. */
+struct pwm_t
+{
+ /** Current PWM value. */
+ int16_t cur;
+ /** Maximum value. */
+ int16_t max;
+ /** Minimum value (dead zone). */
+ int16_t min;
+};
+
+extern struct pwm_t pwm_aux[AC_ASSERV_AUX_NB];
+
+extern uint8_t pwm_reverse;
+
+/** Define current PWM value for each output. */
+#define PWM_VALUE(x) PWM_VALUE_ (x)
+#define PWM_VALUE_(x) PWM_VALUE_ ## x
+#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur
+#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur
+
+/** Define maximum PWM value for each output. */
+#define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x)
+#define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x
+#define PWM_MAX_FOR_pwm_aux0 PWM_MAX
+#define PWM_MAX_FOR_pwm_aux1 PWM_MAX
+
+/** Define minimum PWM value for each output, if the value is less than the
+ * minimum, use 0. */
+#define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x)
+#define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x
+#define PWM_MIN_FOR_pwm_aux0 0x10
+#define PWM_MIN_FOR_pwm_aux1 0x10
+
+/** Define which bit controls the PWM inversion. */
+#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x)
+#define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x
+#define PWM_REVERSE_BIT_pwm_aux0 _BV (0)
+#define PWM_REVERSE_BIT_pwm_aux1 _BV (1)
+
+/** State init macro. */
+#define PWM_INIT_FOR(x) \
+ { 0, PWM_MAX_FOR (x), PWM_MIN_FOR (x) }
+
+/** Set PWM value. */
+static inline void
+pwm_set (struct pwm_t *pwm, int16_t value)
+{
+ if (value > pwm->max)
+ pwm->cur = pwm->max;
+ else if (value < -pwm->max)
+ pwm->cur = -pwm->max;
+ else if (value > -pwm->min && value < pwm->min)
+ pwm->cur = 0;
+ else
+ pwm->cur = value;
+}
+
+void
+pwm_init (void);
+
+void
+pwm_update (void);
+
+void
+pwm_set_reverse (uint8_t reverse);
+
+#endif /* pwm_h */
diff --git a/digital/mimot/src/dirty/pwm_config.h b/digital/mimot/src/dirty/pwm_config.h
new file mode 100644
index 00000000..e937002e
--- /dev/null
+++ b/digital/mimot/src/dirty/pwm_config.h
@@ -0,0 +1,41 @@
+#ifndef pwm_config_h
+#define pwm_config_h
+/* pwm_config.h - Helper for PWM configuration. */
+/* 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.
+ *
+ * }}} */
+
+/* Simplify conditionnal compilation. */
+#define PWM1or2 (defined (PWM1) || defined (PWM2))
+#ifdef PWM1
+# define PWM1c(x) x
+#else
+# define PWM1c(x) 0
+#endif
+#ifdef PWM2
+# define PWM2c(x) x
+#else
+# define PWM2c(x) 0
+#endif
+
+#endif /* pwm_config_h */
diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c
new file mode 100644
index 00000000..c4cd5279
--- /dev/null
+++ b/digital/mimot/src/dirty/pwm_ocr.avr.c
@@ -0,0 +1,161 @@
+/* pwm_ocr.avr.c - PWM using internal generator. */
+/* 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 "pwm_ocr.avr.h"
+#include "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "io.h"
+
+/** Assign PWM outputs. */
+#define PWM1 pwm_aux0
+#define PWM2 pwm_aux1
+
+#define PWM1_OCR OCR1A
+#define PWM1_OCR_BIT 5
+#define PWM1_DIR 3
+#define PWM2_OCR OCR1B
+#define PWM2_OCR_BIT 4
+#define PWM2_DIR 2
+
+#define PWM1_BRK_IO A, 4
+#define PWM2_BRK_IO A, 5
+
+#include "pwm_config.h"
+
+/** PWM reverse direction, port D. */
+static uint8_t pwm_ocr_dir_reverse_d;
+
+/** Initialise PWM generator. */
+void
+pwm_ocr_init (void)
+{
+ /* Phase Correct PWM, TOP = 0x3ff, OCnA & OCnB with positive logic.
+ * f_IO without prescaler.
+ * Fpwm = f_IO / (2 * prescaler * (1 + TOP)) = 7200 Hz. */
+#if PWM1or2
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, FOC1A, FOC1B, WGM11, WGM10,
+ 1, 0, 1, 0, 0, 0, 1, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 0, 1);
+ /* Enable PWM and direction outputs in DDRD. */
+ DDRD |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR))
+ | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR));
+ /* Will activate output at first non zero PWM. */
+# ifdef PWM1
+ IO_OUTPUT (PWM1_BRK_IO);
+# endif
+# ifdef PWM2
+ IO_OUTPUT (PWM2_BRK_IO);
+# endif
+#endif
+}
+
+/** Update the hardware PWM values. */
+void
+pwm_ocr_update (void)
+{
+#if PWM1or2
+ uint8_t dir_d;
+ /* Sample port D. */
+ dir_d = PORTD & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR)));
+# ifdef PWM1
+ uint16_t pwm1;
+ /* Set PWM1. */
+ if (PWM_VALUE (PWM1) == 0)
+ {
+ pwm1 = 0;
+ }
+ else
+ {
+ IO_SET (PWM1_BRK_IO);
+ if (PWM_VALUE (PWM1) < 0)
+ {
+ pwm1 = -PWM_VALUE (PWM1);
+ }
+ else
+ {
+ dir_d |= _BV (PWM1_DIR);
+ pwm1 = PWM_VALUE (PWM1);
+ }
+ }
+# endif /* PWM1 */
+# ifdef PWM2
+ uint16_t pwm2;
+ /* Set PWM2. */
+ if (PWM_VALUE (PWM2) == 0)
+ {
+ pwm2 = 0;
+ }
+ else
+ {
+ IO_SET (PWM2_BRK_IO);
+ if (PWM_VALUE (PWM2) < 0)
+ {
+ pwm2 = -PWM_VALUE (PWM2);
+ }
+ else
+ {
+ dir_d |= _BV (PWM2_DIR);
+ pwm2 = PWM_VALUE (PWM2);
+ }
+ }
+# endif /* PWM2 */
+#endif /* PWM1or2 */
+ /* Setup registers. */
+ /* Here, there could be a problem because OCRx are double buffered, not
+ * PORTx! */
+ /* Another problem arise if the OCR sampling is done between left and
+ * right OCR: the right PWM is one cycle late. */
+ /* A solution could be to use interrupts to update PWM or to synchronise
+ * general timer with PWM. */
+#if PWM1or2
+ dir_d ^= pwm_ocr_dir_reverse_d;
+ PORTD = dir_d;
+# ifdef PWM1
+ PWM1_OCR = pwm1;
+# endif
+# ifdef PWM2
+ PWM2_OCR = pwm2;
+# endif
+#endif /* PWM1or2 */
+}
+
+void
+pwm_ocr_set_reverse (uint8_t reverse)
+{
+ pwm_reverse = reverse;
+ pwm_ocr_dir_reverse_d = 0;
+#ifdef PWM1
+ if (reverse & PWM_REVERSE_BIT (PWM1))
+ pwm_ocr_dir_reverse_d |= _BV (PWM1_DIR);
+#endif
+#ifdef PWM2
+ if (reverse & PWM_REVERSE_BIT (PWM2))
+ pwm_ocr_dir_reverse_d |= _BV (PWM2_DIR);
+#endif
+}
+
diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.h b/digital/mimot/src/dirty/pwm_ocr.avr.h
new file mode 100644
index 00000000..cd013824
--- /dev/null
+++ b/digital/mimot/src/dirty/pwm_ocr.avr.h
@@ -0,0 +1,37 @@
+#ifndef pwm_ocr_avr_h
+#define pwm_ocr_avr_h
+/* pwm_ocr.avr.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.
+ *
+ * }}} */
+
+void
+pwm_ocr_init (void);
+
+void
+pwm_ocr_update (void);
+
+void
+pwm_ocr_set_reverse (uint8_t reverse);
+
+#endif /* pwm_ocr_avr_h */
diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c
new file mode 100644
index 00000000..1f0d6394
--- /dev/null
+++ b/digital/mimot/src/dirty/simu.host.c
@@ -0,0 +1,261 @@
+/* simu.host.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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 "simu.host.h"
+
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "io.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "pwm.h"
+#include "aux.h"
+
+#include "contacts.h"
+
+#include "motor_model.host.h"
+#include "models.host.h"
+
+/** Simulate some AVR regs. */
+uint8_t PORTB, PORTC, PORTD, PINC;
+
+/** Overall counter values. */
+uint16_t counter_aux[AC_ASSERV_AUX_NB];
+/** Counter differences since last update.
+ * Maximum of 9 significant bits, sign included. */
+int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+
+/** PWM control states. */
+struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
+ PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
+};
+/** PWM reverse directions. */
+uint8_t pwm_reverse;
+
+/* Robot model. */
+const struct robot_t *simu_robot;
+
+/** Motor models. */
+struct motor_t simu_aux_model[AC_ASSERV_AUX_NB];
+
+/** Full counter values. */
+uint32_t simu_counter_aux[AC_ASSERV_AUX_NB];
+
+/** Use mex. */
+int simu_mex;
+
+/** Counter to limit the interval between information is sent. */
+int simu_send_cpt;
+
+/** Initialise simulation. */
+static void
+simu_init (void)
+{
+ int argc;
+ char **argv;
+ host_get_program_arguments (&argc, &argv);
+ if (argc == 2 && strncmp (argv[0], "-m", 2) == 0)
+ {
+ simu_mex = atoi (argv[0] + 2);
+ if (!simu_mex) simu_mex = 1;
+ simu_send_cpt = simu_mex;
+ mex_node_connect ();
+ argc--; argv++;
+ }
+ if (argc != 1)
+ {
+ fprintf (stderr, "Syntax: dirty.host [-m[interval]] model\n");
+ exit (1);
+ }
+ simu_robot = models_get (argv[0]);
+ if (!simu_robot)
+ {
+ fprintf (stderr, "unknown model name: %s\n", argv[0]);
+ exit (1);
+ }
+ models_init (simu_robot, simu_aux_model);
+}
+
+/** Update sensors for Marcel. */
+void
+simu_sensor_update_marcel (void)
+{
+}
+
+/** Do a simulation step. */
+static void
+simu_step (void)
+{
+ int i;
+ double old_aux_th[AC_ASSERV_AUX_NB];
+ /* Convert pwm value into voltage. */
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ simu_aux_model[i].u = simu_aux_model[i].m.u_max
+ * ((double) pwm_aux[i].cur / (PWM_MAX + 1));
+ /* Make one step. */
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ old_aux_th[i] = simu_aux_model[i].th;
+ if (simu_robot->aux_motor[i])
+ motor_model_step (&simu_aux_model[i]);
+ }
+ /* Update auxiliary counter. */
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ if (simu_robot->aux_motor[i])
+ {
+ uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI)
+ * simu_robot->aux_encoder_steps[i];
+ counter_aux_diff[i] = counter_aux_new - simu_counter_aux[i];
+ counter_aux[i] += counter_aux_diff[i];
+ simu_counter_aux[i] = counter_aux_new;
+ }
+ else
+ {
+ counter_aux_diff[i] = 0;
+ counter_aux[i] = 0;
+ simu_counter_aux[i] = 0;
+ }
+ }
+ /* Update sensors. */
+ if (simu_robot->sensor_update)
+ simu_robot->sensor_update ();
+}
+
+/** Send information to the other nodes. */
+static void
+simu_send (void)
+{
+ static int first = 1;
+ int i;
+ mex_msg_t *m;
+ /* Send Aux position. */
+ static int32_t simu_aux_model_sent[AC_ASSERV_AUX_NB];
+ int32_t simu_aux_model_to_send[AC_ASSERV_AUX_NB];
+ int simu_aux_model_changed = 0;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ simu_aux_model_to_send[i] = 1024.0 * simu_aux_model[i].th
+ / simu_aux_model[i].m.i_G;
+ if (!first && simu_aux_model_to_send[i] != simu_aux_model_sent[i])
+ simu_aux_model_changed = 1;
+ }
+ if (first || simu_aux_model_changed)
+ {
+ m = mex_msg_new (0xc8);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ if (simu_robot->aux_motor[i])
+ mex_msg_push (m, "l", simu_aux_model_to_send[i]);
+ else
+ mex_msg_push (m, "l", 0);
+ simu_aux_model_sent[i] = simu_aux_model_to_send[i];
+ }
+ mex_node_send (m);
+ }
+ /* First send done. */
+ first = 0;
+}
+
+/** Initialise the timer. */
+void
+timer_init (void)
+{
+ simu_init ();
+}
+
+/** Wait for timer overflow. */
+void
+timer_wait (void)
+{
+ if (simu_mex)
+ mex_node_wait_date (mex_node_date () + 4);
+ simu_step ();
+ if (simu_mex && !--simu_send_cpt)
+ {
+ simu_send_cpt = simu_mex;
+ simu_send ();
+ }
+}
+
+/** Read timer value. Used for performance analysis. */
+uint8_t
+timer_read (void)
+{
+ return 0;
+}
+
+/** Initialize the counters. */
+void
+counter_init (void)
+{
+}
+
+/** Update overall counter values and compute diffs. */
+void
+counter_update (void)
+{
+}
+
+/** Initialise PWM generator. */
+void
+pwm_init (void)
+{
+}
+
+/** Update the hardware PWM values. */
+void
+pwm_update (void)
+{
+}
+
+void
+eeprom_read_params (void)
+{
+}
+
+void
+eeprom_write_params (void)
+{
+}
+
+void
+eeprom_clear_params (void)
+{
+}
+
+void
+pwm_set_reverse (uint8_t reverse)
+{
+ pwm_reverse = reverse;
+}
+
diff --git a/digital/mimot/src/dirty/simu.host.h b/digital/mimot/src/dirty/simu.host.h
new file mode 100644
index 00000000..6250bed1
--- /dev/null
+++ b/digital/mimot/src/dirty/simu.host.h
@@ -0,0 +1,48 @@
+#ifndef simu_host_h
+#define simu_host_h
+/* simu.host.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 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.
+ *
+ * }}} */
+
+extern uint8_t PORTB, PORTC, PORTD, PINC;
+
+void
+timer_init (void);
+
+void
+timer_wait (void);
+
+uint8_t
+timer_read (void);
+
+void
+simu_sensor_update_giboulee (void);
+
+void
+simu_sensor_update_aquajim (void);
+
+void
+simu_sensor_update_marcel (void);
+
+#endif /* simu_host_h */
diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c
new file mode 100644
index 00000000..f9d4193e
--- /dev/null
+++ b/digital/mimot/src/dirty/speed.c
@@ -0,0 +1,130 @@
+/* speed.c - Speed control. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "speed.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "pos.h"
+#include "state.h"
+
+/**
+ * This file is responsible for speed control. It changes the current shafts
+ * positions using ramps. It can be controlled by a wanted speed or wanted
+ * shaft position.
+ */
+
+/** Auxiliaries speed control states. */
+struct speed_t speed_aux[AC_ASSERV_AUX_NB];
+
+/** Initialise speed control states. */
+void
+speed_init (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_aux[i].pos = &pos_aux[i];
+}
+
+/** Update current speed according to a speed consign. */
+static void
+speed_update_by_speed (struct speed_t *speed)
+{
+ /* Update current speed. */
+ if (UTILS_ABS (speed->cons - speed->cur) < speed->acc)
+ speed->cur = speed->cons;
+ else if (speed->cons > speed->cur)
+ speed->cur += speed->acc;
+ else
+ speed->cur -= speed->acc;
+}
+
+/** Compute maximum allowed speed according to: distance left, maximum speed,
+ * current speed and acceleration. */
+static int16_t
+speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
+{
+ int16_t s;
+ /* Compute maximum speed in order to be able to brake in time.
+ * The "+ 0xff" is to ceil result.
+ * s = sqrt (2 * a * d) */
+ s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8);
+ /* Apply consign. */
+ s = UTILS_MIN (max, s);
+ /* Apply sign. */
+ if (d < 0)
+ s = -s;
+ /* Convert to f8.8 and check acceleration. */
+ s = s << 8;
+ UTILS_BOUND (s, cur - acc, cur + acc);
+ return s;
+}
+
+/** Update current speed according to a position consign. */
+static void
+speed_update_by_position (struct speed_t *speed)
+{
+ int32_t diff = speed->pos_cons - speed->pos->cons;
+ speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc,
+ speed->max);
+}
+
+/** Update shaft position consign according to its consign type. */
+static void
+speed_update_by (struct speed_t *speed)
+{
+ if (speed->use_pos)
+ speed_update_by_position (speed);
+ else
+ speed_update_by_speed (speed);
+ /* Update shaft position. */
+ speed->pos->cons += speed->cur >> 8;
+}
+
+/** Update shaft position consign for one motor system. */
+static void
+speed_update_single (struct state_t *state, struct speed_t *speed)
+{
+ if (state->mode >= MODE_SPEED)
+ {
+ /* Update speed. */
+ speed_update_by (speed);
+ /* Check for completion. */
+ if (state->mode == MODE_SPEED
+ && speed->use_pos && speed->cur == 0)
+ state_finish (state);
+ }
+}
+
+/** Update shaft position consign according to consign. */
+void
+speed_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_update_single (&state_aux[i], &speed_aux[i]);
+}
+
diff --git a/digital/mimot/src/dirty/speed.h b/digital/mimot/src/dirty/speed.h
new file mode 100644
index 00000000..10a0da2b
--- /dev/null
+++ b/digital/mimot/src/dirty/speed.h
@@ -0,0 +1,57 @@
+#ifndef speed_h
+#define speed_h
+/* speed.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.
+ *
+ * }}} */
+
+/** Speed control state. */
+struct speed_t
+{
+ /** Controlled position. */
+ struct pos_t *pos;
+ /** Current speed, f8.8. */
+ int16_t cur;
+ /** Consign speed, f8.8. */
+ int16_t cons;
+ /** Maximum speed for position consign, u8. */
+ int8_t max;
+ /** Slow speed for position consign, u8. */
+ int8_t slow;
+ /** Acceleration, f8.8. */
+ int16_t acc;
+ /** Consign position. */
+ uint32_t pos_cons;
+ /** Whether to use the consign position (1) or not (0). */
+ uint8_t use_pos;
+};
+
+extern struct speed_t speed_aux[AC_ASSERV_AUX_NB];
+
+void
+speed_init (void);
+
+void
+speed_update (void);
+
+#endif /* speed_h */
diff --git a/digital/mimot/src/dirty/state.c b/digital/mimot/src/dirty/state.c
new file mode 100644
index 00000000..c6c90409
--- /dev/null
+++ b/digital/mimot/src/dirty/state.c
@@ -0,0 +1,29 @@
+/* state.c */
+/* 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 "state.h"
+
+struct state_t state_aux[AC_ASSERV_AUX_NB];
+
diff --git a/digital/mimot/src/dirty/state.h b/digital/mimot/src/dirty/state.h
new file mode 100644
index 00000000..4916e691
--- /dev/null
+++ b/digital/mimot/src/dirty/state.h
@@ -0,0 +1,124 @@
+#ifndef state_h
+#define state_h
+/* state.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.
+ *
+ * }}} */
+
+/** There is two mechanism to acknowledge long lived command completion.
+ *
+ * The first one is dedicated to unreliable and full duplex channels like the
+ * serial port. It is based on sequence numbers.
+ *
+ * The second one is simpler, but can only be used with reliable and half
+ * duplex channels like the TWI bus. It just use a flag to remember command
+ * completion witch is reseted when a new command arrives. */
+
+/** Control mode. */
+enum state_mode_t
+{
+ /** Simple PWM setup mode. */
+ MODE_PWM,
+ /** Position control mode. */
+ MODE_POS,
+ /** Speed control mode. */
+ MODE_SPEED,
+ /** Trajectory control mode. */
+ MODE_TRAJ,
+};
+
+/** Current motor state. */
+struct state_t
+{
+ /** Current motor control mode. */
+ uint8_t mode;
+ /** Control mode variant.
+ * Used for main motors:
+ * - bit 0: disable theta position control.
+ * - bit 1: disable alpha position control.
+ * - bit 2: disable blocking detection. */
+ uint8_t variant;
+ /** Sequence number of the currently processed command, should be between
+ * 1 and 127. When a command is received on the serial port it is ignored
+ * if its sequence number is equal to the current sequence number. In
+ * this case a duplicated message is inferred. */
+ uint8_t sequence;
+ /** Sequence number of the most recently finished command. When a command
+ * is finished, the current sequence number is copied to this variable. */
+ uint8_t sequence_finish;
+ /** Sequence number of the most recently acknowledged command. Until this
+ * is not equal to the last finished command sequence number, a message is
+ * generated on the serial line. */
+ uint8_t sequence_ack;
+ /** Simpler flag based mechanism, indicates if the last received command
+ * is finished. */
+ uint8_t finished;
+ /** Wether the motor is blocked. */
+ uint8_t blocked;
+};
+
+/** Auxiliary motor states. */
+extern struct state_t state_aux[AC_ASSERV_AUX_NB];
+
+/** Start a new command execution. */
+static inline void
+state_start (struct state_t *motor, uint8_t mode, uint8_t sequence)
+{
+ motor->mode = mode;
+ motor->variant = 0;
+ motor->sequence = sequence;
+ motor->finished = 0;
+ motor->blocked = 0;
+}
+
+/** Signal the current command completion. */
+static inline void
+state_finish (struct state_t *motor)
+{
+ motor->sequence_finish = motor->sequence;
+ motor->finished = 1;
+}
+
+/** Signal the current command is blocked, disable motor control until a new
+ * command is given. */
+static inline void
+state_blocked (struct state_t *motor)
+{
+ motor->sequence_finish = motor->sequence | 0x80;
+ motor->blocked = 1;
+ motor->mode = MODE_PWM;
+}
+
+/** Acknowledge a command completion and blocked state. */
+static inline void
+state_acknowledge (struct state_t *motor, uint8_t sequence)
+{
+ motor->sequence_ack = sequence;
+ if (sequence == motor->sequence_finish)
+ {
+ motor->finished = 0;
+ motor->blocked = 0;
+ }
+}
+
+#endif /* state_h */
diff --git a/digital/mimot/src/dirty/timer.avr.c b/digital/mimot/src/dirty/timer.avr.c
new file mode 100644
index 00000000..eb313603
--- /dev/null
+++ b/digital/mimot/src/dirty/timer.avr.c
@@ -0,0 +1,51 @@
+/* timer.avr.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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 "modules/utils/utils.h"
+#include "io.h"
+
+/** Initialise the timer. */
+void
+timer_init (void)
+{
+ TCCR0 = regv (FOC0, WGM00, COM01, COM00, WGM01, CS02, CS01, CS00,
+ 0, 0, 0, 0, 0, 1, 0, 0);
+ /* Fov = F_io / (prescaler * (TOP + 1))
+ * TOP = 0xff
+ * prescaler = 256
+ * Tov = 1 / Fov = 4.444 ms */
+}
+
+/** Wait for timer overflow. */
+void
+timer_wait (void)
+{
+ while (!(TIFR & _BV (TOV0)))
+ ;
+ /* Write 1 to clear. */
+ TIFR = _BV (TOV0);
+}
+
diff --git a/digital/mimot/src/dirty/timer.h b/digital/mimot/src/dirty/timer.h
new file mode 100644
index 00000000..754a2d46
--- /dev/null
+++ b/digital/mimot/src/dirty/timer.h
@@ -0,0 +1,41 @@
+#ifndef timer_h
+#define timer_h
+/* timer.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.
+ *
+ * }}} */
+
+/** Read timer value. Used for performance analysis. */
+extern inline uint8_t
+timer_read (void)
+{
+ return TCNT0;
+}
+
+void
+timer_init (void);
+
+void
+timer_wait (void);
+
+#endif /* timer_h */
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
new file mode 100644
index 00000000..614536e6
--- /dev/null
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -0,0 +1,173 @@
+/* twi_proto.c - Implement the protocol over TWI. */
+/* 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 "twi_proto.h"
+
+#include "modules/utils/utils.h"
+#include "modules/utils/byte.h"
+#include "modules/utils/crc.h"
+#include "modules/twi/twi.h"
+#include "io.h"
+
+#include "state.h"
+
+#include "pwm.h"
+#include "pos.h"
+#include "speed.h"
+#include "aux.h"
+
+#ifdef HOST
+# include "simu.host.h"
+#endif
+
+struct twi_proto_t
+{
+ u8 seq;
+};
+
+struct twi_proto_t twi_proto;
+
+static void
+twi_proto_callback (u8 *buf, u8 size);
+
+static u8
+twi_proto_params (u8 *buf, u8 size);
+
+/** Initialise. */
+void
+twi_proto_init (void)
+{
+ twi_init (AC_ASSERV_TWI_ADDRESS);
+ twi_proto_update ();
+}
+
+/** Handle received commands and update status. */
+void
+twi_proto_update (void)
+{
+ u8 buf[AC_TWI_SL_RECV_BUFFER_SIZE];
+ u8 read_data;
+ /* Handle incoming command. */
+ while ((read_data = twi_sl_poll (buf, sizeof (buf))))
+ twi_proto_callback (buf, read_data);
+ /* Update status. */
+ u8 status_with_crc[8];
+ u8 *status = &status_with_crc[1];
+ status[0] = 0
+ | (state_aux[1].blocked << 3)
+ | (state_aux[1].finished << 2)
+ | (state_aux[0].blocked << 1)
+ | (state_aux[0].finished << 0);
+ status[1] = PINC;
+ status[2] = twi_proto.seq;
+ status[3] = v16_to_v8 (aux[0].pos, 1);
+ status[4] = v16_to_v8 (aux[0].pos, 0);
+ status[5] = v16_to_v8 (aux[1].pos, 1);
+ status[6] = v16_to_v8 (aux[1].pos, 0);
+ /* Compute CRC. */
+ status_with_crc[0] = crc_compute (&status_with_crc[1],
+ sizeof (status_with_crc) - 1);
+ twi_sl_update (status_with_crc, sizeof (status_with_crc));
+}
+
+/** Handle one command. */
+static void
+twi_proto_callback (u8 *buf, u8 size)
+{
+ /* Check CRC. */
+ if (crc_compute (buf + 1, size - 1) != buf[0])
+ return;
+ else
+ {
+ /* Remove the CRC of the buffer. */
+ buf += 1;
+ size -= 1;
+ }
+
+ if (buf[0] == twi_proto.seq)
+ return;
+#define c(cmd, size) (cmd)
+ switch (c (buf[1], 0))
+ {
+ case c ('z', 0):
+ /* Reset. */
+ utils_reset ();
+ break;
+ case c ('b', 3):
+ /* Move the aux0.
+ * - w: new position.
+ * - b: speed. */
+ speed_aux[0].max = buf[4];
+ aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0);
+ break;
+ case c ('B', 1):
+ /* Find the zero position of the aux0.
+ * - b: speed. */
+ aux_traj_find_zero_start (&aux[0], buf[2], 0);
+ break;
+ case c ('c', 3):
+ /* Move the aux1.
+ * - w: new position.
+ * - b: speed. */
+ speed_aux[1].max = buf[4];
+ aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0);
+ break;
+ case c ('C', 1):
+ /* Find the zero position of the aux1.
+ * - b: speed. */
+ aux_traj_find_zero_start (&aux[1], buf[2], 0);
+ break;
+ case c ('p', x):
+ /* Set parameters. */
+ if (twi_proto_params (&buf[2], size - 2) != 0)
+ buf[0] = 0;
+ break;
+ default:
+ buf[0] = 0;
+ break;
+ }
+ /* Acknowledge. */
+ twi_proto.seq = buf[0];
+}
+
+/* Handle a parameter list of change. */
+static u8
+twi_proto_params (u8 *buf, u8 size)
+{
+ u8 eat;
+ while (*buf && size)
+ {
+ size--;
+ switch (*buf++)
+ {
+ default:
+ return 1;
+ }
+ buf += eat;
+ size -= eat;
+ }
+ return 0;
+}
+
diff --git a/digital/mimot/src/dirty/twi_proto.h b/digital/mimot/src/dirty/twi_proto.h
new file mode 100644
index 00000000..a0746be8
--- /dev/null
+++ b/digital/mimot/src/dirty/twi_proto.h
@@ -0,0 +1,34 @@
+#ifndef twi_proto_h
+#define twi_proto_h
+/* twi_proto.h - Implement the protocol over TWI. */
+/* 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.
+ *
+ * }}} */
+
+void
+twi_proto_init (void);
+
+void
+twi_proto_update (void);
+
+#endif /* twi_proto_h */