summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/mimot/src/dirty/Makefile2
-rw-r--r--digital/mimot/src/dirty/aux.c288
-rw-r--r--digital/mimot/src/dirty/aux.h83
-rw-r--r--digital/mimot/src/dirty/avrconfig.h4
-rw-r--r--digital/mimot/src/dirty/contacts.h6
-rw-r--r--digital/mimot/src/dirty/counter.h5
-rw-r--r--digital/mimot/src/dirty/counter_ext.avr.c103
-rw-r--r--digital/mimot/src/dirty/eeprom.avr.c104
-rw-r--r--digital/mimot/src/dirty/eeprom.h2
-rw-r--r--digital/mimot/src/dirty/main.c365
-rw-r--r--digital/mimot/src/dirty/models.host.c102
-rw-r--r--digital/mimot/src/dirty/models.host.h25
-rw-r--r--digital/mimot/src/dirty/pos.c75
-rw-r--r--digital/mimot/src/dirty/pos.h2
-rw-r--r--digital/mimot/src/dirty/postrack.c103
-rw-r--r--digital/mimot/src/dirty/postrack.h41
-rw-r--r--digital/mimot/src/dirty/pwm.avr.c5
-rw-r--r--digital/mimot/src/dirty/pwm.h18
-rw-r--r--digital/mimot/src/dirty/pwm_ocr.avr.c6
-rw-r--r--digital/mimot/src/dirty/simu.host.c219
-rw-r--r--digital/mimot/src/dirty/simu.host.h4
-rw-r--r--digital/mimot/src/dirty/speed.c27
-rw-r--r--digital/mimot/src/dirty/speed.h2
-rw-r--r--digital/mimot/src/dirty/state.c2
-rw-r--r--digital/mimot/src/dirty/state.h4
-rw-r--r--digital/mimot/src/dirty/timer.avr.c2
-rw-r--r--digital/mimot/src/dirty/traj.c349
-rw-r--r--digital/mimot/src/dirty/traj.h61
-rw-r--r--digital/mimot/src/dirty/twi_proto.c170
-rw-r--r--digital/mimot/tools/inter_mimot.py128
-rw-r--r--digital/mimot/tools/mimot/init.py42
-rw-r--r--digital/mimot/tools/mimot/mex.py76
-rw-r--r--digital/mimot/tools/mimot/mimot.py201
33 files changed, 1617 insertions, 1009 deletions
diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile
index 979aae7e..342c8616 100644
--- a/digital/mimot/src/dirty/Makefile
+++ b/digital/mimot/src/dirty/Makefile
@@ -1,7 +1,7 @@
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 \
+ pwm_ocr.avr.c pos.c speed.c postrack.c traj.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
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
deleted file mode 100644
index 65b4e3d5..00000000
--- a/digital/mimot/src/dirty/aux.c
+++ /dev/null
@@ -1,288 +0,0 @@
-/* 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 "pwm.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,
- /* Speed control, apply open loop PWM on blocking. */
- AUX_TRAJ_CLAMP,
- /* 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,
- /* Find zero by forcing into limit. */
- AUX_TRAJ_FIND_LIMIT,
- /* Wait for mechanical elasticity. */
- AUX_TRAJ_FIND_LIMIT_WAIT,
- /* 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].pwm = &pwm_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].pwm = &pwm_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 ((int32_t) (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;
-}
-
-/** Speed control, then clamp. */
-void
-aux_traj_clamp (struct aux_t *aux)
-{
- /* If blocking, stop control, clamp. */
- if (aux->speed->pos->blocked_counter
- > aux->speed->pos->blocked_counter_limit)
- {
- state_finish (aux->state);
- pos_reset (aux->speed->pos);
- aux->speed->cur = 0;
- aux->state->mode = MODE_PWM;
- pwm_set (aux->pwm, aux->clampin_pwm);
- aux->traj_mode = AUX_TRAJ_DONE;
- }
-}
-
-void
-aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm,
- uint8_t seq)
-{
- aux->traj_mode = AUX_TRAJ_CLAMP;
- aux->clampin_pwm = clampin_pwm;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
- 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;
- aux->speed->cur = 0;
- state_finish (aux->state);
- aux->pos = aux->reset_pos;
- aux->traj_mode = AUX_TRAJ_DONE;
- }
- break;
- }
-}
-
-/** Start find zero mode. */
-void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- 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);
- aux->reset_pos = reset_pos;
-}
-
-/** Find limit mode. */
-void
-aux_traj_find_limit (struct aux_t *aux)
-{
- switch (aux->traj_mode)
- {
- case AUX_TRAJ_FIND_LIMIT:
- /* If blocking, limit is found. */
- if (aux->speed->pos->blocked_counter
- > aux->speed->pos->blocked_counter_limit)
- {
- pos_reset (aux->speed->pos);
- aux->speed->cons = 0;
- aux->speed->cur = 0;
- aux->state->variant = 1;
- pwm_set (aux->pwm, 0);
- aux->traj_mode = AUX_TRAJ_FIND_LIMIT_WAIT;
- aux->wait = 3 * 225;
- }
- break;
- case AUX_TRAJ_FIND_LIMIT_WAIT:
- if (!--aux->wait)
- {
- state_finish (aux->state);
- aux->pos = aux->reset_pos;
- aux->traj_mode = AUX_TRAJ_DONE;
- }
- break;
- }
-}
-
-/** Start find limit mode. */
-void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- uint8_t seq)
-{
- aux->traj_mode = AUX_TRAJ_FIND_LIMIT;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
- aux->state->variant = 4;
- aux->reset_pos = reset_pos;
-}
-
-/** 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_CLAMP:
- aux_traj_clamp (aux);
- break;
- case AUX_TRAJ_FIND_ZERO_NOT:
- case AUX_TRAJ_FIND_ZERO:
- aux_traj_find_zero (aux);
- break;
- case AUX_TRAJ_FIND_LIMIT:
- case AUX_TRAJ_FIND_LIMIT_WAIT:
- aux_traj_find_limit (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
deleted file mode 100644
index 7ec60617..00000000
--- a/digital/mimot/src/dirty/aux.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#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;
- /** Associated PWM. */
- struct pwm_t *pwm;
- /** Absolute position. */
- int16_t pos;
- /** Trajectory mode. */
- uint8_t traj_mode;
- /** Goto position position. */
- uint32_t goto_pos;
- /** Clamping PWM. */
- int16_t clampin_pwm;
- /** 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;
- /** Reset position after zero is found. */
- int16_t reset_pos;
-};
-
-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_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm,
- uint8_t seq);
-
-void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- uint8_t seq);
-
-void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- 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
index ca181554..e3c368d6 100644
--- a/digital/mimot/src/dirty/avrconfig.h
+++ b/digital/mimot/src/dirty/avrconfig.h
@@ -28,7 +28,7 @@
/* global */
/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800,
* 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */
-#define AC_FREQ 14745600
+#define AC_FREQ 16000000
/* uart - UART module. */
/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */
@@ -84,8 +84,6 @@
#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. */
diff --git a/digital/mimot/src/dirty/contacts.h b/digital/mimot/src/dirty/contacts.h
index 366692c4..73b76bc8 100644
--- a/digital/mimot/src/dirty/contacts.h
+++ b/digital/mimot/src/dirty/contacts.h
@@ -26,7 +26,9 @@
* }}} */
/** Define contacts. */
-#define CONTACT_AUX0_ZERO_IO C, 2
-#define CONTACT_AUX1_ZERO_IO C, 3
+#define CONTACT_BACK_LEFT_IO C, 0
+#define CONTACT_BACK_RIGHT_IO C, 1
+#define CONTACT_FRONT_LEFT_IO C, 2
+#define CONTACT_FRONT_RIGHT_IO C, 3
#endif /* contacts_h */
diff --git a/digital/mimot/src/dirty/counter.h b/digital/mimot/src/dirty/counter.h
index b4b4979d..111d9f67 100644
--- a/digital/mimot/src/dirty/counter.h
+++ b/digital/mimot/src/dirty/counter.h
@@ -25,8 +25,9 @@
*
* }}} */
-extern uint16_t counter_aux[AC_ASSERV_AUX_NB];
-extern int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+extern uint16_t counter_left, counter_right;
+extern uint32_t counter_right_correction;
+extern int16_t counter_left_diff, counter_right_diff;
void
counter_init (void);
diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c
index 01afecc2..54bfa9f7 100644
--- a/digital/mimot/src/dirty/counter_ext.avr.c
+++ b/digital/mimot/src/dirty/counter_ext.avr.c
@@ -33,38 +33,44 @@
* 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.
+ *
+ * There is additionnal support for error correction on the right counter.
*/
-/** Define the first auxiliary counter address. */
-#define COUNTER_AUX0 0
-/** Define the second auxiliary counter address. */
-#define COUNTER_AUX1 1
+/** Define the left counter address. */
+#define COUNTER_LEFT 0
+/** Define the right counter address. */
+#define COUNTER_RIGHT 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 1
+/** Define to 1 to reverse the left counter. */
+#define COUNTER_LEFT_REVERSE 0
+/** Define to 1 to reverse the right counter. */
+#define COUNTER_RIGHT_REVERSE 0
-/** First auxiliary counter shift. */
-#define COUNTER_AUX0_SHIFT 1
-/** Second auxiliary counter shift. */
-#define COUNTER_AUX1_SHIFT 1
+/** Left counter shift. */
+#define COUNTER_LEFT_SHIFT 0
+/** Right counter shift. */
+#define COUNTER_RIGHT_SHIFT 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 uint16_t counter_aux_old[AC_ASSERV_AUX_NB];
+static uint16_t counter_left_old, counter_right_old;
/** New values, being updated by step update. */
-static int16_t counter_aux_new_step[AC_ASSERV_AUX_NB];
+static int16_t counter_left_new_step, counter_right_new_step;
/** Last raw step values */
-static uint8_t counter_aux_old_step[AC_ASSERV_AUX_NB];
+static uint8_t counter_left_old_step, counter_right_old_step;
/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
+uint16_t counter_left, counter_right;
+/** Overall uncorrected counter values. */
+static int32_t counter_right_raw;
+/** Correction factor (f8.24). */
+uint32_t counter_right_correction = 1L << 24;
/** Counter differences since last update.
* Maximum of 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+int16_t counter_left_diff, counter_right_diff;
#if !COUNTER_USE_XMEM
# define COUNTER_ALE_IO B, 4
@@ -119,8 +125,8 @@ counter_init (void)
DDRB |= 0x0f;
#endif
/* Begin with safe values. */
- counter_aux_old_step[0] = counter_read (COUNTER_AUX0);
- counter_aux_old_step[1] = counter_read (COUNTER_AUX1);
+ counter_left_old_step = counter_read (COUNTER_LEFT);
+ counter_right_old_step = counter_read (COUNTER_RIGHT);
}
/** Update one step. If counters are not read fast enough, they could
@@ -128,18 +134,18 @@ counter_init (void)
void
counter_update_step (void)
{
- uint8_t aux0, aux1;
+ uint8_t left, right;
int8_t diff;
/* Sample counters. */
- aux0 = counter_read (COUNTER_AUX0);
- aux1 = counter_read (COUNTER_AUX1);
+ left = counter_read (COUNTER_LEFT);
+ right = counter_read (COUNTER_RIGHT);
/* Update step counters. */
- diff = (int8_t) (aux0 - counter_aux_old_step[0]);
- counter_aux_old_step[0] = aux0;
- counter_aux_new_step[0] += diff;
- diff = (int8_t) (aux1 - counter_aux_old_step[1]);
- counter_aux_old_step[1] = aux1;
- counter_aux_new_step[1] += diff;
+ diff = (int8_t) (left - counter_left_old_step);
+ counter_left_old_step = left;
+ counter_left_new_step += diff;
+ diff = (int8_t) (right - counter_right_old_step);
+ counter_right_old_step = right;
+ counter_right_new_step += diff;
}
/** Update overall counter values and compute diffs. */
@@ -148,27 +154,32 @@ counter_update (void)
{
/* Wants fresh data. */
counter_update_step ();
- /* First auxiliary counter. */
- uint16_t aux0 = counter_aux_new_step[0];
- aux0 &= 0xffff << COUNTER_AUX0_SHIFT; /* Reset unused bits. */
-#if !COUNTER_AUX0_REVERSE
- counter_aux_diff[0] = (int16_t) (aux0 - counter_aux_old[0]);
+ /* Left counter. */
+ uint16_t left = counter_left_new_step;
+ left &= 0xffff << COUNTER_LEFT_SHIFT; /* Reset unused bits. */
+#if !COUNTER_LEFT_REVERSE
+ counter_left_diff = (int16_t) (left - counter_left_old);
#else
- counter_aux_diff[0] = (int16_t) (counter_aux_old[0] - aux0);
+ counter_left_diff = (int16_t) (counter_left_old - left);
#endif
- counter_aux_diff[0] >>= COUNTER_AUX0_SHIFT;
- counter_aux_old[0] = aux0;
- counter_aux[0] += counter_aux_diff[0];
- /* Second auxiliary counter. */
- uint16_t aux1 = counter_aux_new_step[1];
- aux1 &= 0xffff << COUNTER_AUX1_SHIFT; /* Reset unused bits. */
-#if !COUNTER_AUX1_REVERSE
- counter_aux_diff[1] = (int16_t) (aux1 - counter_aux_old[1]);
+ counter_left_diff >>= COUNTER_LEFT_SHIFT;
+ counter_left_old = left;
+ counter_left += counter_left_diff;
+ /* Right counter. */
+ uint16_t right = counter_right_new_step;
+ right &= 0xffff << COUNTER_RIGHT_SHIFT; /* Reset unused bits. */
+#if !COUNTER_RIGHT_REVERSE
+ counter_right_diff = (int16_t) (right - counter_right_old);
#else
- counter_aux_diff[1] = (int16_t) (counter_aux_old[1] - aux1);
+ counter_right_diff = (int16_t) (counter_right_old - right);
#endif
- counter_aux_diff[1] >>= COUNTER_AUX1_SHIFT;
- counter_aux_old[1] = aux1;
- counter_aux[1] += counter_aux_diff[1];
+ counter_right_diff >>= COUNTER_RIGHT_SHIFT;
+ counter_right_old = right;
+ /* Fix right counter. */
+ counter_right_raw += counter_right_diff;
+ uint16_t right_new = fixed_mul_f824 (counter_right_raw,
+ counter_right_correction);
+ counter_right_diff = (int16_t) (right_new - counter_right);
+ counter_right = right_new;
}
diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c
index a20032d1..fb11dc9c 100644
--- a/digital/mimot/src/dirty/eeprom.avr.c
+++ b/digital/mimot/src/dirty/eeprom.avr.c
@@ -33,6 +33,8 @@
#include "pwm.h"
#include "pos.h"
#include "speed.h"
+#include "postrack.h"
+#include "traj.h"
#define EEPROM_START 0
@@ -41,6 +43,26 @@
* your new format is not compatible with the old one or you will load
* garbages in parameters. */
+static uint32_t
+eeprom_read_32 (uint8_t *addr)
+{
+ uint8_t dw[4];
+ dw[0] = eeprom_read_byte (addr++);
+ dw[1] = eeprom_read_byte (addr++);
+ dw[2] = eeprom_read_byte (addr++);
+ dw[3] = eeprom_read_byte (addr++);
+ return v8_to_v32 (dw[3], dw[2], dw[1], dw[0]);
+}
+
+static void
+eeprom_write_32 (uint8_t *addr, uint32_t dw)
+{
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 0));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 1));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 2));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 3));
+}
+
/* Read parameters from eeprom. */
void
eeprom_read_params (void)
@@ -49,29 +71,34 @@ eeprom_read_params (void)
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++);
+ speed_theta.max = eeprom_read_byte (p8++);
+ speed_alpha.max = eeprom_read_byte (p8++);
+ speed_theta.slow = eeprom_read_byte (p8++);
+ speed_alpha.slow = eeprom_read_byte (p8++);
pwm_set_reverse (eeprom_read_byte (p8++));
+ counter_right_correction = eeprom_read_32 (p8); p8 += 4;
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++);
+ postrack_set_footing (eeprom_read_word (p16++));
+ speed_theta.acc = eeprom_read_word (p16++);
+ speed_alpha.acc = eeprom_read_word (p16++);
+ pos_theta.kp = eeprom_read_word (p16++);
+ pos_alpha.kp = eeprom_read_word (p16++);
+ pos_theta.ki = eeprom_read_word (p16++);
+ pos_alpha.ki = eeprom_read_word (p16++);
+ pos_theta.kd = eeprom_read_word (p16++);
+ pos_alpha.kd = eeprom_read_word (p16++);
+ pos_theta.blocked_error_limit = eeprom_read_word (p16++);
+ pos_theta.blocked_speed_limit = eeprom_read_word (p16++);
+ pos_theta.blocked_counter_limit = eeprom_read_word (p16++);
+ pos_alpha.blocked_error_limit = eeprom_read_word (p16++);
+ pos_alpha.blocked_speed_limit = eeprom_read_word (p16++);
+ pos_alpha.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++);
+ traj_eps = eeprom_read_word (p16++);
+ traj_aeps = eeprom_read_word (p16++);
+ traj_set_angle_limit (eeprom_read_word (p16++));
}
/* Write parameters to eeprom. */
@@ -81,29 +108,34 @@ 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++, speed_theta.max);
+ eeprom_write_byte (p8++, speed_alpha.max);
+ eeprom_write_byte (p8++, speed_theta.slow);
+ eeprom_write_byte (p8++, speed_alpha.slow);
eeprom_write_byte (p8++, pwm_reverse);
+ eeprom_write_32 (p8, counter_right_correction); p8 += 4;
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++, postrack_footing);
+ eeprom_write_word (p16++, speed_theta.acc);
+ eeprom_write_word (p16++, speed_alpha.acc);
+ eeprom_write_word (p16++, pos_theta.kp);
+ eeprom_write_word (p16++, pos_alpha.kp);
+ eeprom_write_word (p16++, pos_theta.ki);
+ eeprom_write_word (p16++, pos_alpha.ki);
+ eeprom_write_word (p16++, pos_theta.kd);
+ eeprom_write_word (p16++, pos_alpha.kd);
+ eeprom_write_word (p16++, pos_theta.blocked_error_limit);
+ eeprom_write_word (p16++, pos_theta.blocked_speed_limit);
+ eeprom_write_word (p16++, pos_theta.blocked_counter_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_error_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_speed_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_counter_limit);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_i_sat);
eeprom_write_word (p16++, pos_d_sat);
+ eeprom_write_word (p16++, traj_eps);
+ eeprom_write_word (p16++, traj_aeps);
+ eeprom_write_word (p16++, traj_angle_limit);
}
/* Clear eeprom parameters. */
diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h
index d061a9fd..9eb7c18e 100644
--- a/digital/mimot/src/dirty/eeprom.h
+++ b/digital/mimot/src/dirty/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x01
+#define EEPROM_KEY 0x02
void
eeprom_read_params (void);
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index 2b55259b..a04c53b6 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -37,7 +37,8 @@
#include "pwm.h"
#include "pos.h"
#include "speed.h"
-#include "aux.h"
+#include "postrack.h"
+#include "traj.h"
#include "twi_proto.h"
#include "eeprom.h"
@@ -54,8 +55,8 @@ 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;
+/** Report of position. */
+uint8_t main_stat_postrack, main_stat_postrack_cpt;
/** Statistics about speed control. */
uint8_t main_stat_speed, main_stat_speed_cpt;
@@ -72,6 +73,11 @@ uint8_t main_stat_timer, main_stat_timer_cpt;
/** Print input port. */
uint8_t main_print_pin, main_print_pin_cpt;
+#ifdef HOST
+/** Simulation data. */
+uint8_t main_simu, main_simu_cpt;
+#endif /* HOST */
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t main_timer[6];
@@ -93,8 +99,9 @@ main (int argc, char **argv)
counter_init ();
uart0_init ();
twi_proto_init ();
+ postrack_init ();
speed_init ();
- aux_init ();
+ traj_init ();
eeprom_read_params ();
proto_send0 ('z');
sei ();
@@ -119,46 +126,54 @@ main_loop (void)
pwm_update ();
main_timer[2] = timer_read ();
/* Compute absolute position. */
- aux_pos_update ();
+ postrack_update ();
/* Compute trajectory. */
- aux_traj_update ();
+ if (state_main.mode >= MODE_TRAJ)
+ 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)
+ && state_main.sequence_ack != state_main.sequence_finish
&& !--main_sequence_ack_cpt)
{
- proto_send2b ('A', state_aux[0].sequence_finish,
- state_aux[1].sequence_finish);
+ proto_send1b ('A', state_main.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]);
+ proto_send2w ('C', counter_left, counter_right);
main_stat_counter_cpt = main_stat_counter;
}
- if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
+ if (main_stat_postrack && !--main_stat_postrack_cpt)
{
- proto_send2w ('Y', aux[0].pos, aux[1].pos);
- main_stat_aux_pos_cpt = main_stat_aux_pos;
+ proto_send3d ('X', postrack_x, postrack_y, postrack_a);
+ main_stat_postrack_cpt = main_stat_postrack;
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.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);
+ proto_send4w ('P', pos_theta.e_old, pos_theta.i,
+ pos_alpha.e_old, pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
+#ifdef HOST
+ if (main_simu && !--main_simu_cpt)
+ {
+ proto_send3w ('H', (uint16_t) simu_pos_x,
+ (uint16_t) simu_pos_y,
+ (uint16_t) (simu_pos_a * 1024));
+ main_simu_cpt = main_simu;
+ }
+#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur);
+ proto_send2w ('W', pwm_left.cur, pwm_right.cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -184,19 +199,8 @@ 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))
@@ -207,109 +211,132 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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);
+ /* Set zero pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 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]));
+ case c ('w', 4):
+ /* Set pwm.
+ * - w: left pwm.
+ * - w: right pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, v8_to_v16 (args[0], args[1]));
+ pwm_set (&pwm_right, v8_to_v16 (args[2], args[3]));
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]);
+ case c ('c', 4):
+ /* Add to position consign.
+ * - w: theta consign offset.
+ * - w: alpha consign offset. */
+ state_main.mode = MODE_POS;
+ state_main.variant = 0;
+ pos_theta.cons += v8_to_v16 (args[0], args[1]);
+ pos_alpha.cons += v8_to_v16 (args[2], args[3]);
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;
+ case c ('s', 0):
+ /* Stop (set zero speed). */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = 0;
+ speed_alpha.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;
+ /* Set speed.
+ * - b: theta speed.
+ * - b: alpha speed. */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = args[0] << 8;
+ speed_alpha.cons = args[1] << 8;
+ break;
+ case c ('s', 9):
+ /* Set speed controlled position consign.
+ * - d: theta consign offset.
+ * - d: alpha consign offset.
+ * - b: sequence number. */
+ if (args[8] == state_main.sequence)
+ break;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ speed_alpha.pos_cons = pos_alpha.cons;
+ speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
+ state_start (&state_main, MODE_SPEED, args[8]);
break;
- case c ('s', 6):
- /* Set auxiliary speed controlled position consign.
- * - b: aux index.
+ case c ('l', 5):
+ /* Set linear speed controlled position consign.
* - d: consign offset.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (args[4] == state_main.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]);
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_SPEED, args[4]);
break;
- case c ('y', 4):
- /* Auxiliary go to position.
- * - b: aux index.
- * - w: pos, i16.
+ case c ('a', 5):
+ /* Set angular speed controlled position consign.
+ * - d: angle offset.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[3] == state->sequence)
+ if (args[4] == state_main.sequence)
break;
- aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
+ traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2],
+ args[3]), args[4]);
break;
- case c ('y', 5):
- /* Auxiliary clamp.
- * - b: aux index.
- * - b: speed.
- * - w: clamping PWM.
+ case c ('f', 2):
+ /* Go to the wall.
+ * - b: 0: forward, 1: backward.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[4] == state->sequence)
+ if (args[1] == state_main.sequence)
break;
- aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]),
- args[4]);
+ traj_ftw_start (args[0], args[1]);
break;
- case c ('y', 6):
- /* Auxiliary find zero.
- * - b: aux index.
- * - b: speed.
- * - b: use switch.
- * - w: reset position.
+ case c ('x', 10):
+ /* Go to position.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - b: backward (see traj.h).
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (args[9] == state_main.sequence)
break;
- if (args[2])
- aux_traj_find_zero_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]), args[5]);
- else
- aux_traj_find_limit_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]), args[5]);
+ traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ args[8], args[9]);
+ break;
+ case c ('x', 3):
+ /* Go to angle.
+ * - d: a, f0.16.
+ * - b: sequence number. */
+ if (args[2] == state_main.sequence)
+ break;
+ traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
+ 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[0]);
- state_acknowledge (&state_aux[1], args[1]);
+ case c ('x', 12):
+ /* Go to position, then angle.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - w: a, f0.16.
+ * - b: backward (see traj.h).
+ * - b: sequence number. */
+ if (args[11] == state_main.sequence)
+ break;
+ traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ v8_to_v32 (0, args[8], args[9], 0),
+ args[10], args[11]);
+ break;
+ case c ('a', 1):
+ /* Set main acknoledge.
+ * - b: main ack sequence number. */
+ state_acknowledge (&state_main, args[0]);
break;
/* Stats.
* - b: interval between stats. */
@@ -321,16 +348,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* 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];
+ case c ('X', 1):
+ /* Position stats. */
+ main_stat_postrack_cpt = main_stat_postrack = 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. */
+ /* Motor position control stats. */
main_stat_pos_cpt = main_stat_pos = args[0];
break;
case c ('W', 1):
@@ -345,6 +372,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
main_print_pin_cpt = main_print_pin = args[0];
break;
+#ifdef HOST
+ case c ('H', 1):
+ /* Simulation data. */
+ main_simu_cpt = main_simu = args[0];
+ break;
+#endif /* HOST */
default:
/* Params. */
if (cmd == 'p')
@@ -352,10 +385,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* 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]];
+ case 't':
+ pos = &pos_theta;
+ speed = &speed_theta;
+ break;
+ case 'a':
+ pos = &pos_alpha;
+ speed = &speed_alpha;
break;
default:
pos = 0;
@@ -364,6 +400,38 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
}
switch (c (args[0], size))
{
+ case c ('X', 1):
+ /* Reset position. */
+ postrack_x = 0;
+ postrack_y = 0;
+ postrack_a = 0;
+ break;
+ case c ('X', 5):
+ /* Set current x position.
+ * - d: x position. */
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('Y', 5):
+ /* Set current y position.
+ * - d: y position. */
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('A', 5):
+ /* Set current angle.
+ * - d: angle. */
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('c', 5):
+ /* Set right counter correction factor.
+ * - d: factor (f8.24). */
+ counter_right_correction = v8_to_v32 (args[1], args[2],
+ args[3], args[4]);
+ break;
+ case c ('f', 3):
+ /* Set footing.
+ * - w: footing. */
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
case c ('a', 4):
/* Set acceleration.
* - b: index.
@@ -421,9 +489,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('D', 3):
pos_d_sat = v8_to_v16 (args[1], args[2]);
break;
+ case c ('e', 5):
+ traj_eps = v8_to_v16 (args[1], args[2]);
+ traj_aeps = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('l', 3):
+ traj_set_angle_limit (v8_to_v16 (args[1], args[2]));
+ break;
case c ('w', 2):
/* Set PWM direction.
- * - b: bits: 0000[aux1][aux0][right][left]. */
+ * - b: bits: 000000[right][left]. */
pwm_set_reverse (args[1]);
break;
case c ('E', 2):
@@ -437,21 +512,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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_send1d ('c', counter_right_correction);
+ proto_send1w ('f', postrack_footing);
+ proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
+ proto_send2b ('s', speed_theta.max, speed_theta.slow);
+ proto_send2b ('s', speed_alpha.max, speed_alpha.slow);
+ proto_send3w ('b', pos_theta.blocked_error_limit,
+ pos_theta.blocked_speed_limit,
+ pos_theta.blocked_counter_limit);
+ proto_send3w ('b', pos_alpha.blocked_error_limit,
+ pos_alpha.blocked_speed_limit,
+ pos_alpha.blocked_counter_limit);
+ proto_send2w ('p', pos_theta.kp, pos_alpha.kp);
+ proto_send2w ('i', pos_theta.ki, pos_alpha.ki);
+ proto_send2w ('d', pos_theta.kd, pos_alpha.kd);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
+ proto_send2w ('e', traj_eps, traj_aeps);
+ proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', pwm_reverse);
break;
default:
@@ -459,6 +538,24 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
return;
}
}
+#ifdef HOST
+ else if (cmd == 'h')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 7):
+ /* Set simulated position.
+ * - w: x position.
+ * - w: y position.
+ * - w: angle (rad) * 1024. */
+ simu_pos_x = (double) v8_to_v16 (args[1], args[2]);
+ simu_pos_y = (double) v8_to_v16 (args[3], args[4]);
+ simu_pos_a = (double) (int16_t) v8_to_v16 (args[5], args[6])
+ / 1024;
+ break;
+ }
+ }
+#endif /* HOST */
else
{
proto_send0 ('?');
diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c
index db4152e0..0b9b3665 100644
--- a/digital/mimot/src/dirty/models.host.c
+++ b/digital/mimot/src/dirty/models.host.c
@@ -32,66 +32,43 @@
#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 =
+/* RE25G with 1:20.25 gearbox model. */
+static const struct motor_def_t re25g_model =
{
/* Motor characteristics. */
- 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
- 26.10 / 1000, /* Torque constant (N.m/A). */
+ 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 23.4 / 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. */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
-/* Robospierre rotation motor, AMAX32GHP with 1:16 gearbox model. */
-static const struct motor_def_t robospierre_rotation_amax32ghp_model =
-{
- /* Motor characteristics. */
- 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
- 25.44 / 1000, /* Torque constant (N.m/A). */
- 0, /* Bearing friction (N.m/(rad/s)). */
- 3.99, /* Terminal resistance (Ohm). */
+ 2.32, /* Terminal resistance (Ohm). */
0.24 / 1000, /* Terminal inductance (H). */
24.0, /* Maximum voltage (V). */
/* Gearbox characteristics. */
- 16, /* Gearbox ratio. */
+ 20.25, /* Gearbox ratio. */
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
- 0.200 * 0.010 * 0.010, /* Load (kg.m^2). */
- /* This is a pifometric estimation. */
+ 0.0, /* Load (kg.m^2). */
/* Hardware limits. */
-INFINITY, +INFINITY,
};
-/* 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. */
- { 256, 256 },
- /** Sensor update function. */
- simu_sensor_update_marcel,
-};
-
-/* Robospierre, APBTeam 2011. */
-static const struct robot_t robospierre_robot =
+/* TazG, Taz with RE25G motors. */
+static const struct robot_t tazg_robot =
{
- /** Auxiliary motors, NULL if not present. */
- { &marcel_clamp_f2342_model, &robospierre_rotation_amax32ghp_model },
- /** Number of steps for each auxiliary motor encoder. */
- { 256, 250 },
- /** Sensor update function. */
- simu_sensor_update_robospierre,
+ /* Main motors. */
+ &re25g_model,
+ /* Number of steps on the main motors encoders. */
+ 500,
+ /* Wheel radius (m). */
+ 0.04,
+ /* Distance between the wheels (m). */
+ 0.30,
+ /* Weight of the robot (kg). */
+ 10.0,
+ /* Distance of the gravity center from the center of motors axis (m). */
+ 0.0,
+ /* Whether the encoder is mounted on the main motor (false) or not (true). */
+ 0,
+ 0.0, 0.0, NULL
};
/* Table of models. */
@@ -100,8 +77,7 @@ static const struct
const char *name;
const struct robot_t *robot;
} models[] = {
- { "marcel", &marcel_robot },
- { "robospierre", &robospierre_robot },
+ { "tazg", &tazg_robot },
{ 0, 0 }
};
@@ -120,20 +96,24 @@ models_get (const char *name)
/** Initialise simulation models. */
void
-models_init (const struct robot_t *robot, struct motor_t aux_motor[])
+models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
+ struct motor_t *main_motor_right)
{
- int i;
- if (aux_motor)
+ if (main_motor_left)
+ {
+ main_motor_left->m = *robot->main_motor;
+ main_motor_left->m.J = robot->weight * robot->wheel_r
+ * robot->wheel_r / 2;
+ main_motor_left->h = ECHANT_PERIOD;
+ main_motor_left->d = 1000;
+ }
+ if (main_motor_right)
{
- 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;
- }
- }
+ main_motor_right->m = *robot->main_motor;
+ main_motor_right->m.J = robot->weight * robot->wheel_r
+ * robot->wheel_r / 2;
+ main_motor_right->h = ECHANT_PERIOD;
+ main_motor_right->d = 1000;
}
}
diff --git a/digital/mimot/src/dirty/models.host.h b/digital/mimot/src/dirty/models.host.h
index 0e53aaea..e1bf3427 100644
--- a/digital/mimot/src/dirty/models.host.h
+++ b/digital/mimot/src/dirty/models.host.h
@@ -31,10 +31,24 @@
* 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];
+ /** Main motors. */
+ const struct motor_def_t *main_motor;
+ /** Number of steps on the main motors encoders. */
+ int main_encoder_steps;
+ /** Wheel radius (m). */
+ double wheel_r;
+ /** Distance between the wheels (m). */
+ double footing;
+ /** Weight of the robot (kg). */
+ double weight;
+ /** Distance of the gravity center from the center of motors axis (m). */
+ double gravity_center_distance;
+ /** Whether the encoder is mounted on the main motor (false) or not (true). */
+ int encoder_separated;
+ /** Encoder wheel radius (m). */
+ double encoder_wheel_r;
+ /** Distance between the encoders wheels (m). */
+ double encoder_footing;
/** Sensor update function. */
void (*sensor_update) (void);
};
@@ -45,6 +59,7 @@ models_get (const char *name);
/** Initialise simulation models. */
void
-models_init (const struct robot_t *robot, struct motor_t aux_motor[]);
+models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
+ struct motor_t *main_motor_right);
#endif /* models_host_h */
diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c
index 28684414..6304bb5c 100644
--- a/digital/mimot/src/dirty/pos.c
+++ b/digital/mimot/src/dirty/pos.c
@@ -40,8 +40,8 @@
* 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];
+/** Theta/alpha control states. */
+struct pos_t pos_theta, pos_alpha;
/** Error saturation. */
int32_t pos_e_sat = 1023;
@@ -84,43 +84,59 @@ pos_compute_pid (int32_t e, struct pos_t *pos)
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)
+/** Update PWM for a polar motor system. */
+void
+pos_update_polar (struct state_t *state,
+ struct pos_t *pos_theta, struct pos_t *pos_alpha,
+ int16_t counter_left_diff, int16_t counter_right_diff,
+ struct pwm_t *pwm_left, struct pwm_t *pwm_right)
{
if (state->mode >= MODE_POS)
{
- int16_t pid;
- int32_t error;
- /* Update current shaft position. */
- pos->cur += counter_diff;
+ int16_t pid_theta, pid_alpha;
+ int32_t error_theta, error_alpha;
+ int16_t cur_speed_theta, cur_speed_alpha;
+ /* Update current shaft positions. */
+ cur_speed_theta = counter_left_diff + counter_right_diff;
+ cur_speed_alpha = counter_right_diff - counter_left_diff;
+ pos_theta->cur += cur_speed_theta;
+ pos_alpha->cur += cur_speed_alpha;
+ if (state->variant & 1)
+ pos_reset (pos_theta);
+ if (state->variant & 2)
+ pos_reset (pos_alpha);
/* 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++;
+ error_theta = pos_theta->cons - pos_theta->cur;
+ error_alpha = pos_alpha->cons - pos_alpha->cur;
+ /* Test for blocking. */
+ if (UTILS_ABS (error_theta) > pos_theta->blocked_error_limit
+ && UTILS_ABS (cur_speed_theta) < pos_theta->blocked_speed_limit)
+ pos_theta->blocked_counter++;
else
- pos->blocked_counter = 0;
- if (state->variant & 1)
- {
- pos_reset (pos);
- }
- else if (!(state->variant & 4)
- && pos->blocked_counter > pos->blocked_counter_limit)
+ pos_theta->blocked_counter = 0;
+ if (UTILS_ABS (error_alpha) > pos_alpha->blocked_error_limit
+ && UTILS_ABS (cur_speed_alpha) < pos_alpha->blocked_speed_limit)
+ pos_alpha->blocked_counter++;
+ else
+ pos_alpha->blocked_counter = 0;
+ if (pos_theta->blocked_counter > pos_theta->blocked_counter_limit
+ || pos_alpha->blocked_counter > pos_alpha->blocked_counter_limit)
{
/* Blocked. */
- pos_reset (pos);
+ pos_reset (pos_theta);
+ pos_reset (pos_alpha);
state_blocked (state);
- pwm_set (pwm, 0);
+ pwm_set (pwm_left, 0);
+ pwm_set (pwm_right, 0);
}
else
{
/* Compute PID. */
- pid = pos_compute_pid (error, pos);
+ pid_theta = pos_compute_pid (error_theta, pos_theta);
+ pid_alpha = pos_compute_pid (error_alpha, pos_alpha);
/* Update PWM. */
- pwm_set (pwm, pid);
+ pwm_set (pwm_left, pid_theta - pid_alpha);
+ pwm_set (pwm_right, pid_theta + pid_alpha);
}
}
}
@@ -129,10 +145,9 @@ pos_update_single (struct state_t *state, struct pos_t *pos,
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]);
+ pos_update_polar (&state_main, &pos_theta, &pos_alpha,
+ counter_left_diff, counter_right_diff,
+ &pwm_left, &pwm_right);
}
/** Reset position control state. To be called when the position control is
diff --git a/digital/mimot/src/dirty/pos.h b/digital/mimot/src/dirty/pos.h
index 514b091c..df6935b3 100644
--- a/digital/mimot/src/dirty/pos.h
+++ b/digital/mimot/src/dirty/pos.h
@@ -48,7 +48,7 @@ struct pos_t
uint8_t blocked_counter;
};
-extern struct pos_t pos_aux[AC_ASSERV_AUX_NB];
+extern struct pos_t pos_theta, pos_alpha;
extern int32_t pos_e_sat, pos_i_sat, pos_d_sat;
diff --git a/digital/mimot/src/dirty/postrack.c b/digital/mimot/src/dirty/postrack.c
new file mode 100644
index 00000000..a3b2e8cb
--- /dev/null
+++ b/digital/mimot/src/dirty/postrack.c
@@ -0,0 +1,103 @@
+/* postrack.c - Compute current position from counters. */
+/* 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 "postrack.h"
+
+#include "modules/math/fixed/fixed.h"
+#include "modules/math/math.h"
+
+#include "counter.h"
+
+/** Current position, f24.8. */
+int32_t postrack_x, postrack_y;
+/** Current angle, f8.24. */
+int32_t postrack_a;
+/** Distance between the weels, u16. */
+uint16_t postrack_footing;
+/** Precomputed footing factor, f8.24.
+ * postrack_footing_factor = (1/2pi * 256) / postrack_footing
+ * Explanations:
+ * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we
+ * must divide by 2pi to convert unit (Arc=Angle * Radius only works for
+ * radians).
+ * - dd (see postrack_update) is in f10.16 format, we multiply by 256 to have
+ * a angle in f8.24 format.
+ * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */
+static uint32_t postrack_footing_factor;
+
+/** Initialise the position tracker. */
+void
+postrack_init (void)
+{
+ /* Prevent division by 0 by providing a default large value. */
+ postrack_set_footing (0x1000);
+}
+
+/** Update the current position. */
+void
+postrack_update (void)
+{
+ int32_t d, dd, da, na, dsc;
+ d = counter_right_diff + counter_left_diff; /* 10b */
+ d <<= 16; /* 10.16b */
+ if (counter_right_diff == counter_left_diff)
+ {
+ /* Line. */
+ postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8;
+ postrack_y += fixed_mul_f824 (d, fixed_sin_f824 (postrack_a)) >> 8;
+ }
+ else
+ {
+ /* Arc. */
+ dd = counter_right_diff - counter_left_diff; /* 10b */
+ dd <<= 16; /* 10.16b */
+ da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */
+ /* New angle. */
+ na = postrack_a + da;
+ /* Compute da in radians. */
+ da = fixed_mul_f824 (da, 2 * M_PI * (1L << 24));
+ /* X increment. */
+ dsc = fixed_sin_f824 (na) - fixed_sin_f824 (postrack_a); /* 8.24b */
+ dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */
+ postrack_x += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */
+ /* Y increment. */
+ dsc = fixed_cos_f824 (postrack_a) - fixed_cos_f824 (na); /* 8.24b */
+ dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */
+ postrack_y += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */
+ /* Angle update. */
+ postrack_a = na;
+ postrack_a &= 0x00ffffff;
+ }
+}
+
+/** Change the footing value. */
+void
+postrack_set_footing (uint16_t footing)
+{
+ postrack_footing = footing;
+ postrack_footing_factor =
+ (uint32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing;
+}
+
diff --git a/digital/mimot/src/dirty/postrack.h b/digital/mimot/src/dirty/postrack.h
new file mode 100644
index 00000000..26fc4ee3
--- /dev/null
+++ b/digital/mimot/src/dirty/postrack.h
@@ -0,0 +1,41 @@
+#ifndef postrack_h
+#define postrack_h
+/* postrack.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 int32_t postrack_x, postrack_y;
+extern int32_t postrack_a;
+extern uint16_t postrack_footing;
+
+void
+postrack_init (void);
+
+void
+postrack_update (void);
+
+void
+postrack_set_footing (uint16_t footing);
+
+#endif /* postrack_h */
diff --git a/digital/mimot/src/dirty/pwm.avr.c b/digital/mimot/src/dirty/pwm.avr.c
index 37ec34b0..dc43343f 100644
--- a/digital/mimot/src/dirty/pwm.avr.c
+++ b/digital/mimot/src/dirty/pwm.avr.c
@@ -27,9 +27,8 @@
#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)
-};
+struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
+struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
/** PWM reverse directions. */
uint8_t pwm_reverse;
diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h
index b4f12d0d..32000bb7 100644
--- a/digital/mimot/src/dirty/pwm.h
+++ b/digital/mimot/src/dirty/pwm.h
@@ -42,34 +42,34 @@ struct pwm_t
int16_t min;
};
-extern struct pwm_t pwm_aux[AC_ASSERV_AUX_NB];
+extern struct pwm_t pwm_left, pwm_right;
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 PWM_VALUE_pwm_left pwm_left.cur
+#define PWM_VALUE_pwm_right pwm_right.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 PWM_MAX_FOR_pwm_left PWM_MAX
+#define PWM_MAX_FOR_pwm_right 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 PWM_MIN_FOR_pwm_left 0x10
+#define PWM_MIN_FOR_pwm_right 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)
+#define PWM_REVERSE_BIT_pwm_left _BV (0)
+#define PWM_REVERSE_BIT_pwm_right _BV (1)
/** State init macro. */
#define PWM_INIT_FOR(x) \
diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c
index a1d98c25..804d7dcd 100644
--- a/digital/mimot/src/dirty/pwm_ocr.avr.c
+++ b/digital/mimot/src/dirty/pwm_ocr.avr.c
@@ -30,8 +30,8 @@
#include "io.h"
/** Assign PWM outputs. */
-#define PWM1 pwm_aux0
-#define PWM2 pwm_aux1
+#define PWM1 pwm_left
+#define PWM2 pwm_right
#define PWM1_OCR OCR1A
#define PWM1_OCR_BIT 5
@@ -54,7 +54,7 @@ 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. */
+ * Fpwm = f_IO / (2 * prescaler * (1 + TOP)) = 7812.5 Hz. */
#if PWM1or2
TCCR1A =
regv (COM1A1, COM1A0, COM1B1, COM1B0, FOC1A, FOC1B, WGM11, WGM10,
diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c
index d27ffe5f..6aec1788 100644
--- a/digital/mimot/src/dirty/simu.host.c
+++ b/digital/mimot/src/dirty/simu.host.c
@@ -39,7 +39,6 @@
#include <string.h>
#include "pwm.h"
-#include "aux.h"
#include "contacts.h"
@@ -50,15 +49,18 @@
uint8_t PORTB, PORTC, PORTD, PINC;
/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
+uint16_t counter_left, counter_right;
/** Counter differences since last update.
* Maximum of 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+int16_t counter_left_diff, counter_right_diff;
+/** Overall uncorrected counter values. */
+static int32_t counter_right_raw;
+/** Correction factor (f8.24). */
+uint32_t counter_right_correction = 1L << 24;
/** PWM control states. */
-struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
- PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
-};
+struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
+struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
/** PWM reverse directions. */
uint8_t pwm_reverse;
@@ -66,52 +68,25 @@ uint8_t pwm_reverse;
const struct robot_t *simu_robot;
/** Motor models. */
-struct motor_t simu_aux_model[AC_ASSERV_AUX_NB];
+struct motor_t simu_left_model, simu_right_model;
+
+/** Computed simulated position (mm, rad). */
+double simu_pos_x, simu_pos_y, simu_pos_a;
/** Full counter values. */
-uint32_t simu_counter_aux[AC_ASSERV_AUX_NB];
+uint32_t simu_counter_left, simu_counter_right;
+double simu_counter_left_th, simu_counter_right_th;
/** Use mex. */
int simu_mex;
/** Mex message types. */
-uint8_t simu_mex_aux;
-uint8_t simu_mex_limits;
+uint8_t simu_mex_position;
+uint8_t simu_mex_pwm;
/** Counter to limit the interval between information is sent. */
int simu_send_cpt;
-static void
-simu_handle_limits__update_limit (double *th_limit, int32_t limit_int,
- double i_G, int sign)
-{
- if (limit_int == -1)
- /* No change. */
- ;
- else if (limit_int == -2)
- /* INFINITY. */
- *th_limit = sign > 0 ? INFINITY : -INFINITY;
- else
- *th_limit = (double) limit_int / 1024.0 * i_G;
-}
-
-static void
-simu_handle_limits (void *user, mex_msg_t *msg)
-{
- int i;
- int32_t limit_min_int, limit_max_int;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- {
- mex_msg_pop (msg, "ll", &limit_min_int, &limit_max_int);
- simu_handle_limits__update_limit (&simu_aux_model[i].m.th_min,
- limit_min_int,
- simu_aux_model[i].m.i_G, -1);
- simu_handle_limits__update_limit (&simu_aux_model[i].m.th_max,
- limit_max_int,
- simu_aux_model[i].m.i_G, +1);
- }
-}
-
/** Initialise simulation. */
static void
simu_init (void)
@@ -128,9 +103,8 @@ simu_init (void)
simu_send_cpt = simu_mex;
mex_node_connect ();
mex_instance = host_get_instance ("mimot0", 0);
- simu_mex_aux = mex_node_reservef ("%s:aux", mex_instance);
- simu_mex_limits = mex_node_reservef ("%s:limits", mex_instance);
- mex_node_register (simu_mex_limits, simu_handle_limits, 0);
+ simu_mex_position = mex_node_reservef ("%s:position", mex_instance);
+ simu_mex_pwm = mex_node_reservef ("%s:pwm", mex_instance);
}
if (argc != 1)
{
@@ -143,59 +117,94 @@ simu_init (void)
fprintf (stderr, "unknown model name: %s\n", argv[0]);
exit (1);
}
- models_init (simu_robot, simu_aux_model);
+ models_init (simu_robot, &simu_left_model, &simu_right_model);
+ simu_pos_x = simu_pos_y = simu_pos_a = 0;
}
-/** Update sensors for Marcel. */
-void
-simu_sensor_update_marcel (void)
-{
-}
-
-/** Update sensors for Robospierre. */
-void
-simu_sensor_update_robospierre (void)
+/** Update simulation position. */
+static void
+simu_pos_update (double dl, double dr, double footing)
{
- PINC = 0xf0;
- if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G)
- PINC |= IO_BV (CONTACT_AUX0_ZERO_IO);
+ double d = 0.5 * (dl + dr);
+ double da = (dr - dl) / footing;
+ double na = simu_pos_a + da;
+ if (da < 0.0001 && da > -0.0001)
+ {
+ /* Avoid a division by zero when angle is too small. */
+ double a = simu_pos_a + da * 0.5;
+ simu_pos_x += d * cos (a);
+ simu_pos_y += d * sin (a);
+ }
+ else
+ {
+ /* Radius of turn is d / da. */
+ simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da;
+ simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da;
+ }
+ simu_pos_a = na;
}
/** Do a simulation step. */
static void
simu_step (void)
{
- int i;
- double old_aux_th[AC_ASSERV_AUX_NB];
+ double old_left_th, old_right_th;
/* 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));
+ simu_left_model.u = simu_left_model.m.u_max
+ * ((double) pwm_left.cur / (PWM_MAX + 1));
+ simu_right_model.u = simu_right_model.m.u_max
+ * ((double) pwm_right.cur / (PWM_MAX + 1));
/* Make one step. */
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ old_left_th = simu_left_model.th;
+ old_right_th = simu_right_model.th;
+ motor_model_step (&simu_left_model);
+ motor_model_step (&simu_right_model);
+ /* Modify counters. */
+ uint32_t counter_left_new;
+ uint32_t counter_right_new;
+ if (!simu_robot->encoder_separated)
{
- old_aux_th[i] = simu_aux_model[i].th;
- if (simu_robot->aux_motor[i])
- motor_model_step (&simu_aux_model[i]);
+ counter_left_new = simu_left_model.th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
+ counter_right_new = simu_right_model.th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
}
- /* Update auxiliary counter. */
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ else
{
- 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;
- }
+ /* Thanks Thalès. */
+ double left_diff = (simu_left_model.th - old_left_th)
+ / simu_left_model.m.i_G * simu_robot->wheel_r;
+ double right_diff = (simu_right_model.th - old_right_th)
+ / simu_right_model.m.i_G * simu_robot->wheel_r;
+ double sum = left_diff + right_diff;
+ double diff = (left_diff - right_diff)
+ * (simu_robot->encoder_footing / simu_robot->footing);
+ double left_enc_diff = 0.5 * (sum + diff);
+ double right_enc_diff = 0.5 * (sum - diff);
+ simu_counter_left_th += left_enc_diff / simu_robot->encoder_wheel_r;
+ simu_counter_right_th += right_enc_diff / simu_robot->encoder_wheel_r;
+ counter_left_new = simu_counter_left_th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
+ counter_right_new = simu_counter_right_th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
}
+ /* Update an integer counter. */
+ counter_left_diff = counter_left_new - simu_counter_left;
+ counter_left += counter_left_diff;
+ simu_counter_left = counter_left_new;
+ counter_right_diff = counter_right_new - simu_counter_right;
+ counter_right_raw += counter_right_diff;
+ uint16_t right_new = fixed_mul_f824 (counter_right_raw,
+ counter_right_correction);
+ counter_right_diff = (int16_t) (right_new - counter_right);
+ counter_right = right_new;
+ simu_counter_right = counter_right_new;
+ /* Update position. */
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ / simu_left_model.m.i_G * simu_robot->wheel_r * 1000,
+ (simu_right_model.th - old_right_th)
+ / simu_right_model.m.i_G * simu_robot->wheel_r * 1000,
+ simu_robot->footing * 1000);
/* Update sensors. */
if (simu_robot->sensor_update)
simu_robot->sensor_update ();
@@ -206,31 +215,37 @@ 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++)
+ /* Send position. */
+ static int16_t simu_pos_x_sent, simu_pos_y_sent;
+ static int32_t simu_pos_a_sent;
+ int16_t simu_pos_x_to_send = simu_pos_x;
+ int16_t simu_pos_y_to_send = simu_pos_y;
+ int32_t simu_pos_a_to_send = 1024.0 * simu_pos_a;
+ if (first
+ || simu_pos_x_to_send != simu_pos_x_sent
+ || simu_pos_y_to_send != simu_pos_y_sent
+ || simu_pos_a_to_send != simu_pos_a_sent)
{
- 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;
+ m = mex_msg_new (simu_mex_position);
+ mex_msg_push (m, "hhl", simu_pos_x_to_send, simu_pos_y_to_send,
+ simu_pos_a_to_send);
+ mex_node_send (m);
+ simu_pos_x_sent = simu_pos_x_to_send;
+ simu_pos_y_sent = simu_pos_y_to_send;
+ simu_pos_a_sent = simu_pos_a_to_send;
}
- if (first || simu_aux_model_changed)
+ /* Send PWM. */
+ static int16_t pwm_left_sent, pwm_right_sent;
+ if (first
+ || pwm_left_sent == pwm_left.cur
+ || pwm_right_sent == pwm_right.cur)
{
- m = mex_msg_new (simu_mex_aux);
- 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];
- }
+ m = mex_msg_new (simu_mex_pwm);
+ mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur);
mex_node_send (m);
+ pwm_left_sent = pwm_left.cur;
+ pwm_right_sent = pwm_right.cur;
}
/* First send done. */
first = 0;
diff --git a/digital/mimot/src/dirty/simu.host.h b/digital/mimot/src/dirty/simu.host.h
index 87a728b6..83f88314 100644
--- a/digital/mimot/src/dirty/simu.host.h
+++ b/digital/mimot/src/dirty/simu.host.h
@@ -26,6 +26,7 @@
* }}} */
extern uint8_t PORTB, PORTC, PORTD, PINC;
+extern double simu_pos_x, simu_pos_y, simu_pos_a;
void
timer_init (void);
@@ -45,7 +46,4 @@ simu_sensor_update_aquajim (void);
void
simu_sensor_update_marcel (void);
-void
-simu_sensor_update_robospierre (void);
-
#endif /* simu_host_h */
diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c
index 80ca3d6b..8aa3d54b 100644
--- a/digital/mimot/src/dirty/speed.c
+++ b/digital/mimot/src/dirty/speed.c
@@ -37,16 +37,15 @@
* shaft position.
*/
-/** Auxiliaries speed control states. */
-struct speed_t speed_aux[AC_ASSERV_AUX_NB];
+/** Theta/alpha speed control states. */
+struct speed_t speed_theta, speed_alpha;
/** 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];
+ speed_theta.pos = &pos_theta;
+ speed_alpha.pos = &pos_alpha;
}
/** Update current speed according to a speed consign. */
@@ -112,18 +111,24 @@ speed_update_by (struct speed_t *speed)
speed->pos->cons += speed->cur >> 8;
}
-/** Update shaft position consign for one motor system. */
+/** Update shaft position consign for two motors system. */
static void
-speed_update_single (struct state_t *state, struct speed_t *speed)
+speed_update_double (struct state_t *state, struct speed_t *speed0,
+ struct speed_t *speed1)
{
if (state->mode >= MODE_SPEED)
{
/* Update speed. */
- speed_update_by (speed);
+ speed_update_by (speed0);
+ speed_update_by (speed1);
/* Check for completion. */
if (state->mode == MODE_SPEED
- && speed->use_pos && speed->cur == 0)
+ && (speed0->use_pos || speed1->use_pos)
+ && (!speed0->use_pos || speed0->cur == 0)
+ && (!speed1->use_pos || speed1->cur == 0))
+ {
state_finish (state);
+ }
}
}
@@ -131,8 +136,6 @@ speed_update_single (struct state_t *state, struct speed_t *speed)
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]);
+ speed_update_double (&state_main, &speed_theta, &speed_alpha);
}
diff --git a/digital/mimot/src/dirty/speed.h b/digital/mimot/src/dirty/speed.h
index 10a0da2b..5d4fd855 100644
--- a/digital/mimot/src/dirty/speed.h
+++ b/digital/mimot/src/dirty/speed.h
@@ -46,7 +46,7 @@ struct speed_t
uint8_t use_pos;
};
-extern struct speed_t speed_aux[AC_ASSERV_AUX_NB];
+extern struct speed_t speed_theta, speed_alpha;
void
speed_init (void);
diff --git a/digital/mimot/src/dirty/state.c b/digital/mimot/src/dirty/state.c
index c6c90409..11ad1a4a 100644
--- a/digital/mimot/src/dirty/state.c
+++ b/digital/mimot/src/dirty/state.c
@@ -25,5 +25,5 @@
#include "common.h"
#include "state.h"
-struct state_t state_aux[AC_ASSERV_AUX_NB];
+struct state_t state_main;
diff --git a/digital/mimot/src/dirty/state.h b/digital/mimot/src/dirty/state.h
index 4916e691..4c1a28ed 100644
--- a/digital/mimot/src/dirty/state.h
+++ b/digital/mimot/src/dirty/state.h
@@ -77,8 +77,8 @@ struct state_t
uint8_t blocked;
};
-/** Auxiliary motor states. */
-extern struct state_t state_aux[AC_ASSERV_AUX_NB];
+/** Main motors state. */
+extern struct state_t state_main;
/** Start a new command execution. */
static inline void
diff --git a/digital/mimot/src/dirty/timer.avr.c b/digital/mimot/src/dirty/timer.avr.c
index 3f956137..eded2f66 100644
--- a/digital/mimot/src/dirty/timer.avr.c
+++ b/digital/mimot/src/dirty/timer.avr.c
@@ -45,7 +45,7 @@ timer_init (void)
/* Fov = F_io / (prescaler * (TOP + 1))
* TOP = 0xff
* prescaler = 256
- * Tov = 1 / Fov = 4.444 ms */
+ * Tov = 1 / Fov = 4.096 ms */
}
/** Wait for timer overflow. */
diff --git a/digital/mimot/src/dirty/traj.c b/digital/mimot/src/dirty/traj.c
new file mode 100644
index 00000000..a12b6f5b
--- /dev/null
+++ b/digital/mimot/src/dirty/traj.c
@@ -0,0 +1,349 @@
+/* traj.c - Trajectories. */
+/* 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 "traj.h"
+
+#include "modules/math/fixed/fixed.h"
+#include "modules/math/math.h"
+#include "modules/utils/utils.h"
+#include "io.h"
+
+#include <math.h>
+
+#include "state.h"
+
+#include "pos.h"
+#include "speed.h"
+#include "postrack.h"
+
+#include "contacts.h"
+
+#ifdef HOST
+# include "simu.host.h"
+#endif
+
+/* This file provides high level trajectories commands. */
+
+/** Angle after which a backward movement is considered. */
+#define TRAJ_GOTO_BACKWARD_THRESHOLD ((int32_t) (0.55 * M_PI * (1L << 24)))
+
+/** The famous number. */
+#define PI_F824 ((int32_t) (M_PI * (1L << 24)))
+
+/** Traj mode enum. */
+enum
+{
+ /* Go to the wall. */
+ TRAJ_FTW,
+ /* Go to position. */
+ TRAJ_GOTO,
+ /* Go to angle. */
+ TRAJ_GOTO_ANGLE,
+ /* Go to position, then angle. */
+ TRAJ_GOTO_XYA,
+ /* Everything done. */
+ TRAJ_DONE,
+};
+
+/** Traj mode. */
+uint8_t traj_mode;
+
+/** Epsilon, distance considered to be small enough. */
+int16_t traj_eps = 500;
+
+/** Angle epsilon, angle considered to be small enough (f0.16). */
+int16_t traj_aeps = 0x0100;
+
+/** Angle at which to start going forward (f0.16). */
+uint16_t traj_angle_limit = 0x2000;
+
+/** Angle at which to start going forward (rad, f8.24). */
+int32_t traj_angle_limit_rad;
+
+/** Go backward. */
+static uint8_t traj_backward;
+
+/** Go to position. */
+static uint32_t traj_goto_x, traj_goto_y;
+
+/** Go to angle. */
+static uint32_t traj_goto_a;
+
+/** Initialise computed factors. */
+void
+traj_init (void)
+{
+ traj_set_angle_limit (traj_angle_limit);
+}
+
+/** Angle offset. Directly handled to speed layer. */
+void
+traj_angle_offset_start (int32_t angle, uint8_t seq)
+{
+ int32_t a = fixed_mul_f824 (angle, 2 * PI_F824);
+ uint32_t f = postrack_footing;
+ int32_t arc = fixed_mul_f824 (f, a);
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ speed_alpha.pos_cons += arc;
+ state_start (&state_main, MODE_SPEED, seq);
+}
+
+/** Go to the wall mode. */
+static void
+traj_ftw (void)
+{
+ uint8_t left, right;
+ int16_t speed;
+ speed = speed_theta.slow;
+ speed *= 256;
+ if (!traj_backward)
+ {
+ left = !IO_GET (CONTACT_FRONT_LEFT_IO);
+ right = !IO_GET (CONTACT_FRONT_RIGHT_IO);
+ }
+ else
+ {
+ speed = -speed;
+ left = !IO_GET (CONTACT_BACK_LEFT_IO);
+ right = !IO_GET (CONTACT_BACK_RIGHT_IO);
+ }
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = speed;
+ speed_alpha.cons = 0;
+ state_main.variant = 0;
+ if (!left && !right)
+ {
+ /* Backward. */
+#ifndef HOST
+ /* No angular control. */
+ state_main.variant = 2;
+#endif
+ }
+ else if (!left || !right)
+ {
+#ifndef HOST
+ /* No angular control. */
+ state_main.variant = 2;
+#else
+ /* On host, we must do the job. */
+ speed_theta.cons = speed / 4;
+ if (left)
+ speed_alpha.cons = speed / 2;
+ else
+ speed_alpha.cons = -speed / 2;
+#endif
+ }
+ else
+ {
+ /* Stay here. */
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cur;
+ speed_alpha.pos_cons = pos_alpha.cur;
+ speed_theta.cur = 0;
+ speed_alpha.cur = 0;
+ state_finish (&state_main);
+ traj_mode = TRAJ_DONE;
+ }
+}
+
+/** Start go to the wall mode. */
+void
+traj_ftw_start (uint8_t backward, uint8_t seq)
+{
+ traj_mode = TRAJ_FTW;
+ traj_backward = backward;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
+/** Go to position mode. */
+static void
+traj_goto (void)
+{
+ int32_t dx = traj_goto_x - postrack_x;
+ int32_t dy = traj_goto_y - postrack_y;
+ if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8
+ && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8)
+ {
+ /* Near enough, stop, let speed terminate the movement. */
+ state_main.mode = MODE_SPEED;
+ traj_mode = TRAJ_DONE;
+ }
+ else
+ {
+ /* Projection of destination in robot base. */
+ int32_t c = fixed_cos_f824 (postrack_a);
+ int32_t s = fixed_sin_f824 (postrack_a);
+ int32_t dt = fixed_mul_f824 (dx, c) + fixed_mul_f824 (dy, s);
+ int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s);
+ /* Compute arc length. */
+ int32_t arad = atan2 (da, dt) * (1L << 24);
+ if (traj_backward & TRAJ_BACKWARD)
+ {
+ if (arad > 0)
+ arad = - PI_F824 + arad;
+ else
+ arad = PI_F824 + arad;
+ }
+ if (traj_backward & TRAJ_REVERT_OK)
+ {
+ if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD)
+ arad = - PI_F824 + arad;
+ else if (arad < - TRAJ_GOTO_BACKWARD_THRESHOLD)
+ arad = PI_F824 + arad;
+ }
+ int32_t arc = fixed_mul_f824 (arad, postrack_footing);
+ /* Compute consign. */
+ speed_alpha.pos_cons = pos_alpha.cur;
+ speed_alpha.pos_cons += arc;
+ if (UTILS_ABS (arad) < traj_angle_limit_rad)
+ {
+ speed_theta.pos_cons = pos_theta.cur;
+ speed_theta.pos_cons += dt >> 8;
+ }
+ else
+ {
+ speed_theta.pos_cons = pos_theta.cons;
+ }
+ }
+}
+
+/** Start go to position mode (x, y: f24.8). */
+void
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq)
+{
+ traj_mode = TRAJ_GOTO;
+ traj_goto_x = x;
+ traj_goto_y = y;
+ traj_backward = backward;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
+/** Go to angle mode. */
+static void
+traj_goto_angle (void)
+{
+ /* There is some tricky parts to handle rotation direction. */
+ int16_t da = (uint16_t) (traj_goto_a >> 8) - (uint16_t) (postrack_a >> 8);
+ if (UTILS_ABS (da) < traj_aeps)
+ {
+ /* Near enough, stop, let speed terminate the movement. */
+ state_main.mode = MODE_SPEED;
+ traj_mode = TRAJ_DONE;
+ }
+ else
+ {
+ /* Compute arc length. */
+ int32_t arad = fixed_mul_f824 (((int32_t) da) << 8,
+ 2 * PI_F824);
+ int32_t arc = fixed_mul_f824 (arad, postrack_footing);
+ /* Compute consign. */
+ speed_alpha.pos_cons = pos_alpha.cur;
+ speed_alpha.pos_cons += arc;
+ }
+}
+
+/** Start go to angle mode (a: f8.24). */
+void
+traj_goto_angle_start (uint32_t a, uint8_t seq)
+{
+ traj_mode = TRAJ_GOTO_ANGLE;
+ traj_goto_a = a;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
+/** Go to position, then angle mode. */
+static void
+traj_goto_xya (void)
+{
+ int32_t dx = traj_goto_x - postrack_x;
+ int32_t dy = traj_goto_y - postrack_y;
+ if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8
+ && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8)
+ {
+ /* Near enough, now do a go to angle. */
+ traj_mode = TRAJ_GOTO_ANGLE;
+ traj_goto_angle ();
+ }
+ else
+ {
+ traj_goto ();
+ }
+}
+
+/** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */
+void
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
+ uint8_t seq)
+{
+ traj_mode = TRAJ_GOTO_XYA;
+ traj_goto_x = x;
+ traj_goto_y = y;
+ traj_goto_a = a;
+ traj_backward = backward;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
+/* Compute new speed according the defined trajectory. */
+void
+traj_update (void)
+{
+ switch (traj_mode)
+ {
+ case TRAJ_FTW:
+ traj_ftw ();
+ break;
+ case TRAJ_GOTO:
+ traj_goto ();
+ break;
+ case TRAJ_GOTO_ANGLE:
+ traj_goto_angle ();
+ break;
+ case TRAJ_GOTO_XYA:
+ traj_goto_xya ();
+ break;
+ case TRAJ_DONE:
+ break;
+ }
+}
+
+/* Set angle limit. */
+void
+traj_set_angle_limit (uint16_t a)
+{
+ traj_angle_limit = a;
+ traj_angle_limit_rad = (uint32_t) a * (uint32_t) ((1 << 8) * M_PI * 2);
+}
+
diff --git a/digital/mimot/src/dirty/traj.h b/digital/mimot/src/dirty/traj.h
new file mode 100644
index 00000000..268ccd1c
--- /dev/null
+++ b/digital/mimot/src/dirty/traj.h
@@ -0,0 +1,61 @@
+#ifndef traj_h
+#define traj_h
+/* traj.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 TRAJ_BACKWARD 1
+#define TRAJ_REVERT_OK 2
+
+extern uint8_t traj_mode;
+extern int16_t traj_eps;
+extern int16_t traj_aeps;
+extern uint16_t traj_angle_limit;
+
+void
+traj_init (void);
+
+void
+traj_update (void);
+
+void
+traj_angle_offset_start (int32_t angle, uint8_t seq);
+
+void
+traj_ftw_start (uint8_t backward, uint8_t seq);
+
+void
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq);
+
+void
+traj_goto_angle_start (uint32_t a, uint8_t seq);
+
+void
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
+ uint8_t seq);
+
+void
+traj_set_angle_limit (uint16_t a);
+
+#endif /* traj_h */
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 2f935fda..1de7d30d 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -36,7 +36,8 @@
#include "pwm.h"
#include "pos.h"
#include "speed.h"
-#include "aux.h"
+#include "postrack.h"
+#include "traj.h"
#ifdef HOST
# include "simu.host.h"
@@ -73,19 +74,23 @@ twi_proto_update (void)
while ((read_data = twi_slave_poll (buf, sizeof (buf))))
twi_proto_callback (buf, read_data);
/* Update status. */
- u8 status_with_crc[8];
+ u8 status_with_crc[12];
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);
+ | (speed_theta.cur < 0 ? (1 << 3) : 0)
+ | (speed_theta.cur > 0 ? (1 << 2) : 0)
+ | (state_main.blocked << 1)
+ | (state_main.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);
+ status[3] = v32_to_v8 (postrack_x, 3);
+ status[4] = v32_to_v8 (postrack_x, 2);
+ status[5] = v32_to_v8 (postrack_x, 1);
+ status[6] = v32_to_v8 (postrack_y, 3);
+ status[7] = v32_to_v8 (postrack_y, 2);
+ status[8] = v32_to_v8 (postrack_y, 1);
+ status[9] = v32_to_v8 (postrack_a, 2);
+ status[10] = v32_to_v8 (postrack_a, 1);
/* Compute CRC. */
status_with_crc[0] = crc_compute (&status_with_crc[1],
sizeof (status_with_crc) - 1);
@@ -115,60 +120,68 @@ twi_proto_callback (u8 *buf, u8 size)
/* 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);
+ case c ('w', 0):
+ /* Set zero pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 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);
+ case c ('s', 0):
+ /* Stop (set zero speed). */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
break;
- case c ('B', 5):
- /* Find the zero position.
- * - b: aux index.
- * - b: speed.
- * - b: use switch.
- * - w: reset position. */
- if (buf[2] < AC_ASSERV_AUX_NB)
- {
- if (buf[4])
- aux_traj_find_zero_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]), 0);
- else
- aux_traj_find_limit_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]), 0);
- }
+ case c ('l', 3):
+ /* Set linear speed controlled position consign.
+ * - 3b: theta consign offset. */
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ if (buf[2] & 0x80)
+ speed_theta.pos_cons += v8_to_v32 (0xff, buf[2], buf[3], buf[4]);
else
- buf[0] = 0;
+ speed_theta.pos_cons += v8_to_v32 (0, buf[2], buf[3], buf[4]);
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_SPEED, 0);
break;
- case c ('l', 4):
- /* Clamp.
- * - b: aux index.
- * - b: speed.
- * - w: claming PWM. */
- if (buf[2] < AC_ASSERV_AUX_NB)
- aux_traj_clamp_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[4], buf[5]), 0);
- else
- buf[0] = 0;
+ case c ('a', 2):
+ /* Set angular speed controlled position consign.
+ * - w: angle offset. */
+ traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0);
break;
- case c ('w', 1):
- /* Free motor.
- * - b: aux index. */
- if (buf[2] < AC_ASSERV_AUX_NB)
- {
- pos_reset (&pos_aux[buf[2]]);
- state_aux[buf[2]].mode = MODE_PWM;
- pwm_set (&pwm_aux[buf[2]], 0);
- }
- else
- buf[0] = 0;
+ case c ('f', 1):
+ /* Go to the wall.
+ * - b: 0: forward, 1: backward. */
+ traj_ftw_start (buf[2], 0);
+ break;
+ case c ('x', 7):
+ /* Go to position.
+ * - 3b: x position.
+ * - 3b: y position.
+ * - b: backward (see traj.h). */
+ traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ buf[8], 0);
+ break;
+ case c ('y', 2):
+ /* Go to angle.
+ * - w: angle. */
+ traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
+ break;
+ case c ('X', 9):
+ /* Go to position, then angle.
+ * - 3b: x position.
+ * - 3b: y position.
+ * - w: angle.
+ * - b: backward (see traj.h). */
+ traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ v8_to_v32 (0, buf[8], buf[9], 0),
+ buf[10], 0);
break;
case c ('p', x):
/* Set parameters. */
@@ -193,15 +206,44 @@ twi_proto_params (u8 *buf, u8 size)
size--;
switch (*buf++)
{
+ case 'X':
+ /* Set current X position.
+ * - 3b: X position. */
+ if (size < 3)
+ return 1;
+ postrack_x = v8_to_v32 (buf[0], buf[1], buf[2], 0);
+ eat = 3;
+ break;
case 'Y':
- /* Set current aux position.
- * - b: aux index.
- * - w: position. */
- if (buf[0] >= AC_ASSERV_AUX_NB || size < 3)
+ /* Set current Y position.
+ * - 3b: Y position. */
+ if (size < 3)
return 1;
- aux[buf[0]].pos = v8_to_v16 (buf[1], buf[2]);
+ postrack_y = v8_to_v32 (buf[0], buf[1], buf[2], 0);
eat = 3;
break;
+ case 'A':
+ /* Set current angle.
+ * - w: angle. */
+ if (size < 2)
+ return 1;
+ postrack_a = v8_to_v32 (0, buf[0], buf[1], 0);
+ eat = 2;
+ break;
+ case 's':
+ /* Set maximum and slow speed.
+ * - b: theta max.
+ * - b: alpha max.
+ * - b: theta slow.
+ * - b: alpha slow. */
+ if (size < 4)
+ return 1;
+ speed_theta.max = buf[0];
+ speed_alpha.max = buf[1];
+ speed_theta.slow = buf[2];
+ speed_alpha.slow = buf[3];
+ eat = 4;
+ break;
default:
return 1;
}
diff --git a/digital/mimot/tools/inter_mimot.py b/digital/mimot/tools/inter_mimot.py
new file mode 100644
index 00000000..414b61df
--- /dev/null
+++ b/digital/mimot/tools/inter_mimot.py
@@ -0,0 +1,128 @@
+# inter_mimot - Mimot interface. {{{
+#
+# Copyright (C) 2011 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.
+#
+# }}}
+"""Inter, communicating with the mimot board."""
+import math
+
+import mimot
+import mimot.init
+from utils.init_proto import init_proto
+
+from inter.inter import Inter
+from Tkinter import *
+
+class InterMimot (Inter):
+ """Inter, communicating with the mimot board."""
+
+ def __init__ (self):
+ # Asserv.
+ self.a = init_proto ('giboulee', mimot.Proto, mimot.init)
+ self.a.async = True
+ # Inter.
+ Inter.__init__ (self)
+ self.tk.createfilehandler (self.a, READABLE, self.read)
+ self.timeout ()
+ # Query position.
+ self.a.register_pos ()
+ self.a.position.register (self.pos)
+
+ def createWidgets (self):
+ Inter.createWidgets (self)
+ self.ArggButton = Button (self.rightFrame, text = 'Argggg!',
+ command = self.emergency_stop)
+ self.ArggButton.pack ()
+ self.actionVar = StringVar ()
+ self.actionVar.set ('goto')
+ self.actionSetPosButton = Radiobutton (self.rightFrame,
+ text = 'set pos', value = 'set_pos',
+ variable = self.actionVar)
+ self.actionSetPosButton.pack ()
+ self.actionGotoButton = Radiobutton (self.rightFrame,
+ text = 'goto', value = 'goto',
+ variable = self.actionVar)
+ self.actionGotoButton.pack ()
+ self.backwardVar = IntVar ()
+ self.backwardButton = Checkbutton (self.rightFrame,
+ text = 'backward', variable = self.backwardVar)
+ self.backwardButton.pack ()
+ self.revertokVar = IntVar ()
+ self.revertokButton = Checkbutton (self.rightFrame,
+ text = 'revert ok', variable = self.revertokVar)
+ self.revertokButton.pack ()
+ self.ftwButton = Button (self.rightFrame, text = 'FTW',
+ command = self.ftw)
+ self.ftwButton.pack ()
+
+ self.tableview.configure (cursor = 'crosshair')
+ self.tableview.bind ('<1>', self.button1)
+ self.tableview.bind ('<3>', self.button3)
+
+ def read (self, file, mask):
+ """Handle mimot events."""
+ self.a.proto.read ()
+ self.a.proto.sync ()
+
+ def timeout (self):
+ self.a.proto.sync ()
+ self.after (100, self.timeout)
+
+ def pos (self):
+ p = self.a.position
+ self.tableview.robot.pos = p.pos
+ self.tableview.robot.angle = p.angle
+ self.tableview.robot.update ()
+ self.update ()
+
+ def button1 (self, ev):
+ x, y = self.tableview.screen_coord ((ev.x, ev.y))
+ action = self.actionVar.get ()
+ if action == 'set_pos':
+ self.a.set_pos (x, y)
+ elif action == 'goto':
+ self.a.goto (x, y, self.backwardVar.get (),
+ self.revertokVar.get ())
+ else:
+ assert 0
+
+ def button3 (self, ev):
+ x, y = self.tableview.screen_coord ((ev.x, ev.y))
+ a = math.atan2 (y - self.tableview.robot.pos[1],
+ x - self.tableview.robot.pos[0])
+ action = self.actionVar.get ()
+ if action == 'set_pos':
+ self.a.set_pos (a = a)
+ elif action == 'goto':
+ self.a.goto_angle (a)
+
+ def emergency_stop (self):
+ self.a.free ()
+
+ def ftw (self):
+ self.a.ftw (self.backwardVar.get ())
+
+if __name__ == '__main__':
+ app = InterMimot ()
+ try:
+ app.mainloop ()
+ finally:
+ app.a.close ()
diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py
index 49b16cc1..1900de67 100644
--- a/digital/mimot/tools/mimot/init.py
+++ b/digital/mimot/tools/mimot/init.py
@@ -1,27 +1,31 @@
"""Default parameters for asserv."""
-target_marcel = dict (
- a0kp = 4,
- a0a = 16, a0sm = 0x60, a0ss = 0x10,
- a0be = 256, a0bs = 0x18, a0bc = 5,
- a1kp = 4,
- a1a = 16, a1sm = 0x60, a1ss = 0x10,
- a1be = 256, a1bs = 0x18, a1bc = 5,
+host_marcel = dict (
+ scale = 0.0395840674352314, f = 0xdd1,
+ tkp = 1, tkd = 16,
+ ta = 0.75, tsm = 0x20, tss = 0x10,
+ akp = 2, akd = 16,
+ aa = 0.25, asm = 0x20, ass = 0x10,
E = 0x3ff, D = 0x1ff,
- w = 0x03,
+ l = 0x1000,
)
-target_robospierre = dict (
- a0kp = 8, a0kd = 1,
- a0a = 2, a0sm = 0x60, a0ss = 0x10,
- a0be = 32, a0bs = 0x08, a0bc = 125,
- a1kp = 4,
- a1a = 0.5, a1sm = 0x30, a1ss = 0x08,
- a1be = 64, a1bs = 0x08, a1bc = 5,
+host = {
+ 'tazg': host_marcel,
+ 'giboulee': host_marcel,
+ 'marcel': host_marcel,
+ }
+target_marcel = dict (
+ scale = 0.0415178942124, f = 0xcef,
+ c = float (0x00ffbabf) / (1 << 24),
+ tkp = 1, tkd = 16,
+ ta = 0.75, tsm = 0x20, tss = 0x10,
+ akp = 2, akd = 16,
+ aa = 0.25, asm = 0x20, ass = 0x10,
E = 0x3ff, D = 0x1ff,
- w = 0x03,
+ l = 0x1000,
+ w = 0x00,
)
target = {
+ 'tazg': target_marcel,
+ 'giboulee': target_marcel,
'marcel': target_marcel,
- 'robospierre': target_robospierre,
}
-
-host = target
diff --git a/digital/mimot/tools/mimot/mex.py b/digital/mimot/tools/mimot/mex.py
index 5724189e..f39d3ebf 100644
--- a/digital/mimot/tools/mimot/mex.py
+++ b/digital/mimot/tools/mimot/mex.py
@@ -29,75 +29,43 @@ import simu.mex.msg
class Mex:
"""Handle communications with simulated mimot."""
- class Aux (Observable):
- """Auxiliary motor angle.
+ class Position (Observable):
+ """Robot position.
+ - pos: (x, y) millimeters.
- angle: radian.
"""
- def __init__ (self):
+ def __init__ (self, node, instance):
Observable.__init__ (self)
+ self.pos = None
self.angle = None
+ node.register (instance + ':position', self.__handle)
- class Pack:
- """Handle reception of several Aux for one message."""
+ def __handle (self, msg):
+ x, y, a = msg.pop ('hhl')
+ self.pos = (x, y)
+ self.angle = float (a) / 1024
+ self.notify ()
- def __init__ (self, node, instance, list):
- self.__list = list
- node.register (instance + ':aux', self.__handle)
+ class PWM (Observable):
+ """Motor PWM.
- def __handle (self, msg):
- angles = msg.pop ('%dl' % len (self.__list))
- for aux, angle in zip (self.__list, angles):
- aux.angle = float (angle) / 1024
- aux.notify ()
-
- class Limits (Observable):
- """Motor limits.
-
- - min, max: limits in radian.
+ - pwm: current PWM values (hardware unit).
"""
- def __init__ (self, pack, index):
+ def __init__ (self, node, instance):
Observable.__init__ (self)
- self.pack = pack
- self.index = index
- self.min = None
- self.max = None
- self.register (self.__notified)
-
- def __notified (self):
- self.pack.set (self.index, self.min, self.max)
-
- class Pack:
- """Handle emission of several limits for one message."""
-
- def __init__ (self, node, instance):
- self.node = node
- self.mtype = node.reserve (instance + ':limits')
- self.limits = [ None, None, None, None ]
-
- def set (self, index, min, max):
- self.limits[index * 2] = min
- self.limits[index * 2 + 1] = max
- self.__send ()
+ self.pwm = None
+ node.register (instance + ':pwm', self.__handle)
- def __send (self):
- m = simu.mex.msg.Msg (self.mtype)
- for l in self.limits:
- if l is None:
- li = -1
- else:
- li = int (l * 1024)
- m.push ('l', li)
- self.node.send (m)
+ def __handle (self, msg):
+ self.pwm = msg.pop ('hh')
+ self.notify ()
def __init__ (self, node, instance = 'mimot0'):
- self.aux = (self.Aux (), self.Aux ())
- self.__aux_pack = self.Aux.Pack (node, instance, self.aux)
- self.__limits_pack = self.Limits.Pack (node, instance)
- for index, aux in enumerate (self.aux):
- aux.limits = self.Limits (self.__limits_pack, index)
+ self.position = self.Position (node, instance)
+ self.pwm = self.PWM (node, instance)
diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py
index 013a85ff..665947f5 100644
--- a/digital/mimot/tools/mimot/mimot.py
+++ b/digital/mimot/tools/mimot/mimot.py
@@ -44,40 +44,62 @@ class Proto:
# be in the same order than in mimot program.
stats_order = 'CSPW'
stats_items = {
- 'a0c': ('C', 0),
- 'a1c': ('C', 1),
- 'a0s': ('S', 0),
- 'a1s': ('S', 1),
- 'a0e': ('P', 0),
- 'a0i': ('P', 1),
- 'a1e': ('P', 2),
- 'a1i': ('P', 3),
- 'a0w': ('W', 0),
- 'a1w': ('W', 1),
+ 'lc': ('C', 0),
+ 'rc': ('C', 1),
+ 'ts': ('S', 0),
+ 'as': ('S', 1),
+ 'te': ('P', 0),
+ 'ti': ('P', 1),
+ 'ae': ('P', 2),
+ 'ai': ('P', 3),
+ 'lw': ('W', 0),
+ 'rw': ('W', 1),
}
- _index = dict (a0 = 0, a1 = 1)
+ _index = dict (t = ord ('t'), a = ord ('a'))
+
+ class Position (Observable):
+ """An observable position. To be used with register_pos.
+
+ - pos: (x, y) millimeters.
+ - angle: radian.
+
+ """
+
+ def __init__ (self):
+ Observable.__init__ (self)
+ self.pos = None
+ self.angle = None
+
+ def handle (self, x, y, a):
+ """Update position and notify observers."""
+ self.pos = (x, y)
+ self.angle = a
+ self.notify ()
def __init__ (self, file, time = time.time, **param):
"""Initialise communication and send parameters to asserv."""
self.proto = proto.Proto (file, time, 0.1)
self.async = False
- self.aseq = [ 0, 0 ]
- self.aseq_ack = [ 0, 0 ]
- self.proto.register ('A', 'BB', self.__handle_ack)
+ self.mseq = 0
+ self.mseq_ack = 0
+ self.proto.register ('A', 'B', self.__handle_ack)
def make_handle (s):
return lambda *args: self.__handle_stats (s, *args)
for (s, f) in self.stats_format.iteritems ():
self.proto.register (s, f, make_handle (s))
self.stats_enabled = None
self.param = dict (
- a0kp = 0, a0ki = 0, a0kd = 0,
- a0a = 1, a0sm = 0, a0ss = 0,
- a0be = 2048, a0bs = 0x10, a0bc = 20,
- a1kp = 0, a1ki = 0, a1kd = 0,
- a1a = 1, a1sm = 0, a1ss = 0,
- a1be = 2048, a1bs = 0x10, a1bc = 20,
+ scale = 1,
+ tkp = 0, tki = 0, tkd = 0,
+ ta = 1, tsm = 0, tss = 0,
+ tbe = 2048, tbs = 0x10, tbc = 20,
+ akp = 0, aki = 0, akd = 0,
+ aa = 1, asm = 0, ass = 0,
+ abe = 2048, abs = 0x10, abc = 20,
E = 1023, I = 1023, D = 1023,
+ c = 1, f = 0x1000,
+ l = 0x2000,
w = 0x00,
)
self.param.update (param)
@@ -135,39 +157,114 @@ class Proto:
def consign (self, w, c):
"""Consign offset."""
- self.proto.send ('c', 'Bh', self._index[w], c)
+ if w == 't':
+ self.proto.send ('c', 'hh', c, 0)
+ elif w == 'a':
+ self.proto.send ('c', 'hh', 0, c)
+ else:
+ raise IndexError
def speed (self, w, s):
"""Speed consign."""
- self.proto.send ('s', 'Bb', self._index[w], s)
+ if w == 't':
+ self.proto.send ('s', 'bb', s, 0)
+ elif w == 'a':
+ self.proto.send ('s', 'bb', 0, s)
+ else:
+ raise IndexError
def speed_pos (self, w, offset):
"""Speed controlled position consign."""
- i = self._index[w]
- self.aseq[i] += 1
- self.proto.send ('s', 'BlB', i, offset, self.aseq[i])
+ if w == 't':
+ self.mseq += 1
+ self.proto.send ('s', 'llB', self._dist (offset), 0, self.mseq)
+ elif w == 'a':
+ self.mseq += 1
+ self.proto.send ('s', 'llB', 0, self.dist (offset), self.mseq)
+ else:
+ raise IndexError
+ self.wait (self.finished, auto = True)
+
+ def speed_angle (self, w, angle):
+ """Speed controlled angle consign."""
+ if w == 'a':
+ self.mseq += 1
+ self.proto.send ('s', 'llB',
+ 0, int (round (angle * self.param['f'])), self.mseq)
+ else:
+ raise IndexError
+ self.wait (self.finished, auto = True)
+
+ def set_pos (self, x = None, y = None, a = None):
+ """Set current position."""
+ if x is not None:
+ self.proto.send ('p', 'cl', 'X', self._dist_f248 (x))
+ if y is not None:
+ self.proto.send ('p', 'cl', 'Y', self._dist_f248 (y))
+ if a is not None:
+ self.proto.send ('p', 'cl', 'A', self._angle_f824 (a))
+
+ def goto (self, x, y, backward = False, revert_ok = False):
+ """Go to position."""
+ self.mseq += 1
+ b = 0
+ if backward:
+ b |= 1
+ if revert_ok:
+ b |= 2
+ self.proto.send ('x', 'llBB',
+ self._dist_f248 (x), self._dist_f248 (y), b, self.mseq)
self.wait (self.finished, auto = True)
- def goto_pos (self, w, pos):
- """Go to absolute position."""
- i = self._index[w]
- self.aseq[i] += 1
- self.proto.send ('y', 'BhB', i, pos, self.aseq[i])
+ def goto_angle (self, a):
+ """Go to angle."""
+ self.mseq += 1
+ self.proto.send ('x', 'HB', self._angle_f16 (a), self.mseq)
self.wait (self.finished, auto = True)
- def clamp (self, w, s, pwm):
- """Clamp (speed control, then open loop PWM)."""
- i = self._index[w]
- self.aseq[i] += 1
- self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i])
+ def goto_xya (self, x, y, a, backward = False, revert_ok = False):
+ """Go to position, then angle."""
+ self.mseq += 1
+ b = 0
+ if backward:
+ b |= 1
+ if revert_ok:
+ b |= 2
+ self.proto.send ('x', 'llHBB',
+ self._dist_f248 (x), self._dist_f248 (y),
+ self._angle_f16 (a), b, self.mseq)
self.wait (self.finished, auto = True)
+ def ftw (self, backward = True):
+ """Go to the wall."""
+ self.mseq += 1
+ self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq)
+ self.wait (self.finished, auto = True)
+
+ def set_simu_pos (self, x, y, a):
+ """Set simulated position."""
+ self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)),
+ int (round (a * 1024)))
+
+ def register_pos (self, func = None, interval = 225 / 4):
+ """Will call func each time a position is received. If no function is
+ provided, use the Position observable object."""
+ if func is None:
+ self.position = self.Position ()
+ self.pos_func = self.position.handle
+ else:
+ self.pos_func = func
+ self.proto.register ('X', 'lll', self.__handle_pos)
+ self.proto.send ('X', 'B', interval)
+
def send_param (self):
"""Send all parameters."""
p = self.param
def f88 (x):
return int (round (x * (1 << 8)))
- for m in ('a0', 'a1'):
+ def f824 (x):
+ return int (round (x * (1 << 24)))
+ for m in ('t', 'a'):
index = self._index [m]
self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp']))
self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki']))
@@ -179,6 +276,9 @@ class Proto:
self.proto.send ('p', 'cH', 'E', p['E'])
self.proto.send ('p', 'cH', 'I', p['I'])
self.proto.send ('p', 'cH', 'D', p['D'])
+ self.proto.send ('p', 'cL', 'c', f824 (p['c']))
+ self.proto.send ('p', 'cH', 'f', p['f'])
+ self.proto.send ('p', 'cH', 'l', p['l'])
self.proto.send ('p', 'cB', 'w', p['w'])
def write_eeprom (self):
@@ -196,11 +296,17 @@ class Proto:
self.stats_line = [ ]
self.stats_count += 1
- def __handle_ack (self, a0seq, a1seq):
+ def __handle_ack (self, mseq):
"""Record current acknowledge level and acknowledge reception."""
- self.aseq_ack[0] = a0seq & 0x7f
- self.aseq_ack[1] = a1seq & 0x7f
- self.proto.send ('a', 'BB', a0seq, a1seq)
+ self.mseq_ack = mseq & 0x7f
+ self.proto.send ('a', 'B', mseq)
+
+ def __handle_pos (self, x, y, a):
+ """Handle position report."""
+ x = x / 256 * self.param['scale']
+ y = y / 256 * self.param['scale']
+ a = a * 2 * math.pi / (1 << 24)
+ self.pos_func (x, y, a)
def wait (self, cond = None, auto = False):
"""Wait for a condition to become true, or for a number of recorded
@@ -216,8 +322,7 @@ class Proto:
def finished (self):
"""Return True if movement commands have been acknowledged."""
- return (self.aseq[0] == self.aseq_ack[0]
- and self.aseq[1] == self.aseq_ack[1])
+ return self.mseq == self.mseq_ack
def free (self):
"""Coast motors."""
@@ -239,3 +344,15 @@ class Proto:
"""Return fileno for select() calls."""
return self.proto.fileno ()
+ def _dist (self, d):
+ return int (round (d / self.param['scale']))
+
+ def _dist_f248 (self, d):
+ return int (round ((1 << 8) * d / self.param['scale']))
+
+ def _angle_f16 (self, a):
+ return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff
+
+ def _angle_f824 (self, a):
+ return int (round ((1 << 24) * a / (2 * math.pi)))
+