summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2012-03-05 23:49:17 +0100
committerNicolas Schodet2012-03-06 23:16:56 +0100
commit830048da9a94cd9fb91678694dfd07d577e3070c (patch)
tree79012f8b303dbca5379b20661085022b47d04e1e
parent4895e54c666db9720abda0aa8ae901058679aee3 (diff)
digital/mimot: convert to new control system
-rw-r--r--digital/mimot/src/dirty/Makefile13
-rw-r--r--digital/mimot/src/dirty/aux.c138
-rw-r--r--digital/mimot/src/dirty/aux.h24
-rw-r--r--digital/mimot/src/dirty/avrconfig.h53
-rw-r--r--digital/mimot/src/dirty/counter_ext.avr.c174
-rw-r--r--digital/mimot/src/dirty/cs.c69
-rw-r--r--digital/mimot/src/dirty/cs.h (renamed from digital/mimot/src/dirty/counter.h)23
-rw-r--r--digital/mimot/src/dirty/eeprom.avr.c101
-rw-r--r--digital/mimot/src/dirty/eeprom.h2
-rw-r--r--digital/mimot/src/dirty/main.c204
-rw-r--r--digital/mimot/src/dirty/models.host.c63
-rw-r--r--digital/mimot/src/dirty/models.host.h11
-rw-r--r--digital/mimot/src/dirty/motor_model.host.c100
-rw-r--r--digital/mimot/src/dirty/motor_model.host.h68
-rw-r--r--digital/mimot/src/dirty/pos.c149
-rw-r--r--digital/mimot/src/dirty/pos.h61
-rw-r--r--digital/mimot/src/dirty/pwm.avr.c57
-rw-r--r--digital/mimot/src/dirty/pwm.h101
-rw-r--r--digital/mimot/src/dirty/pwm_config.h41
-rw-r--r--digital/mimot/src/dirty/pwm_ocr.avr.c161
-rw-r--r--digital/mimot/src/dirty/pwm_ocr.avr.h37
-rw-r--r--digital/mimot/src/dirty/seq.c (renamed from digital/mimot/src/dirty/state.c)8
-rw-r--r--digital/mimot/src/dirty/seq.h (renamed from digital/mimot/src/dirty/state.h)106
-rw-r--r--digital/mimot/src/dirty/simu.host.c72
-rw-r--r--digital/mimot/src/dirty/speed.c138
-rw-r--r--digital/mimot/src/dirty/speed.h57
-rw-r--r--digital/mimot/src/dirty/timer.avr.c5
-rw-r--r--digital/mimot/src/dirty/twi_proto.c32
-rw-r--r--digital/mimot/tools/mimot/init.py2
29 files changed, 479 insertions, 1591 deletions
diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile
index 979aae7e..55c3e894 100644
--- a/digital/mimot/src/dirty/Makefile
+++ b/digital/mimot/src/dirty/Makefile
@@ -1,10 +1,13 @@
BASE = ../../../avr
PROGS = dirty
-dirty_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c \
- pwm_ocr.avr.c pos.c speed.c aux.c \
- twi_proto.c eeprom.avr.c state.c \
- simu.host.c motor_model.host.c models.host.c
-MODULES = proto uart utils math/fixed twi
+dirty_SOURCES = main.c timer.avr.c \
+ aux.c cs.c \
+ twi_proto.c eeprom.avr.c seq.c \
+ simu.host.c models.host.c
+MODULES = proto uart utils math/fixed twi \
+ motor/encoder motor/encoder/ext motor/output motor/output/pwm_ocr \
+ motor/control_system motor/speed_control motor/pos_control \
+ motor/blocking_detection motor/motor_model
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
AVR_MCU = atmega32
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
index 65b4e3d5..50bebc3a 100644
--- a/digital/mimot/src/dirty/aux.c
+++ b/digital/mimot/src/dirty/aux.c
@@ -28,13 +28,6 @@
#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
@@ -47,6 +40,10 @@ struct aux_t aux[AC_ASSERV_AUX_NB];
/** Trajectory modes. */
enum
{
+ /* Everything done. */
+ AUX_TRAJ_DONE,
+ /* Detect end of speed controled position control. */
+ AUX_TRAJ_SPEED,
/* Goto position, with blocking detection. */
AUX_TRAJ_GOTO,
/* Goto position, try to unblock. */
@@ -61,23 +58,17 @@ enum
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].cs = &cs_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].cs = &cs_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;
@@ -90,7 +81,28 @@ aux_pos_update (void)
uint8_t i;
/* Easy... */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- aux[i].pos += counter_aux_diff[i];
+ aux[i].pos += aux[i].cs->encoder->diff;
+}
+
+/** Wait for zero speed mode. */
+void
+aux_traj_speed (struct aux_t *aux)
+{
+ if (aux->cs->speed.use_pos
+ && aux->cs->speed.pos_cons == aux->cs->pos.cons)
+ {
+ control_state_finished (&aux->cs->state);
+ aux->traj_mode = AUX_TRAJ_DONE;
+ }
+}
+
+/** Start speed mode. */
+void
+aux_traj_speed_start (struct aux_t *aux)
+{
+ /* Speed setup should have been done yet. */
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
+ aux->traj_mode = AUX_TRAJ_SPEED;
}
/** Goto position. */
@@ -100,43 +112,39 @@ 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)
+ if (aux->cs->blocking_detection.blocked)
{
aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK;
- aux->speed->pos_cons = aux->speed->pos->cur;
- aux->speed->pos_cons -= 250;
+ speed_control_pos_offset_from_here (&aux->cs->speed, -250);
aux->wait = 225 / 2;
}
- else if (UTILS_ABS ((int32_t) (aux->speed->pos_cons -
- aux->speed->pos->cur)) < 300)
+ else if (UTILS_ABS ((int32_t) (aux->cs->speed.pos_cons
+ - aux->cs->pos.cur)) < 300)
{
aux->traj_mode = AUX_TRAJ_DONE;
- aux->state->variant = 0;
- state_finish (aux->state);
+ control_state_finished (&aux->cs->state);
}
break;
case AUX_TRAJ_GOTO_UNBLOCK:
if (!--aux->wait)
{
aux->traj_mode = AUX_TRAJ_GOTO;
- aux->speed->pos_cons = aux->goto_pos;
+ speed_control_pos (&aux->cs->speed, aux->goto_pos);
}
break;
}
}
void
-aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq)
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos)
{
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_pos_offset_from_here (&aux->cs->speed,
+ (int16_t) (pos - aux->pos));
+ aux->goto_pos = aux->cs->speed.pos_cons;
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ aux->handle_blocking ? CS_MODE_BLOCKING_DETECTION
+ : 0);
}
/** Speed control, then clamp. */
@@ -144,28 +152,23 @@ 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)
+ if (aux->cs->blocking_detection.blocked)
{
- 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);
+ control_state_set_mode (&aux->cs->state, CS_MODE_NONE, 0);
+ control_state_finished (&aux->cs->state);
+ output_set (aux->cs->output, 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_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm)
{
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;
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_BLOCKING_DETECTION);
}
/** Find zero mode. */
@@ -182,9 +185,8 @@ aux_traj_find_zero (struct aux_t *aux)
case AUX_TRAJ_FIND_ZERO:
if (!zero)
{
- aux->speed->cons = 0;
- aux->speed->cur = 0;
- state_finish (aux->state);
+ speed_control_set_speed (&aux->cs->speed, 0);
+ control_state_finished (&aux->cs->state);
aux->pos = aux->reset_pos;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -194,14 +196,12 @@ aux_traj_find_zero (struct aux_t *aux)
/** 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_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos)
{
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;
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Find limit mode. */
@@ -212,14 +212,12 @@ aux_traj_find_limit (struct aux_t *aux)
{
case AUX_TRAJ_FIND_LIMIT:
/* If blocking, limit is found. */
- if (aux->speed->pos->blocked_counter
- > aux->speed->pos->blocked_counter_limit)
+ if (aux->cs->blocking_detection.blocked)
{
- pos_reset (aux->speed->pos);
- aux->speed->cons = 0;
- aux->speed->cur = 0;
- aux->state->variant = 1;
- pwm_set (aux->pwm, 0);
+ /* Disable all but traj control. */
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_TRAJ_CONTROL - 1);
+ output_set (aux->cs->output, 0);
aux->traj_mode = AUX_TRAJ_FIND_LIMIT_WAIT;
aux->wait = 3 * 225;
}
@@ -227,7 +225,8 @@ aux_traj_find_limit (struct aux_t *aux)
case AUX_TRAJ_FIND_LIMIT_WAIT:
if (!--aux->wait)
{
- state_finish (aux->state);
+ control_state_set_mode (&aux->cs->state, CS_MODE_NONE, 0);
+ control_state_finished (&aux->cs->state);
aux->pos = aux->reset_pos;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -237,14 +236,12 @@ aux_traj_find_limit (struct aux_t *aux)
/** 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_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos)
{
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;
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_BLOCKING_DETECTION);
aux->reset_pos = reset_pos;
}
@@ -252,10 +249,13 @@ aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
static void
aux_traj_update_single (struct aux_t *aux)
{
- if (aux->state->mode >= MODE_TRAJ)
+ if (aux->cs->state.modes & CS_MODE_TRAJ_CONTROL)
{
switch (aux->traj_mode)
{
+ case AUX_TRAJ_SPEED:
+ aux_traj_speed (aux);
+ break;
case AUX_TRAJ_GOTO:
case AUX_TRAJ_GOTO_UNBLOCK:
aux_traj_goto (aux);
diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
index 7ec60617..133b7d51 100644
--- a/digital/mimot/src/dirty/aux.h
+++ b/digital/mimot/src/dirty/aux.h
@@ -24,16 +24,13 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "cs.h"
/** Auxiliary motor informations. */
struct aux_t
{
- /** Associated state. */
- struct state_t *state;
- /** Controlled speed. */
- struct speed_t *speed;
- /** Associated PWM. */
- struct pwm_t *pwm;
+ /** Associated control system. */
+ control_system_single_t *cs;
/** Absolute position. */
int16_t pos;
/** Trajectory mode. */
@@ -63,19 +60,20 @@ void
aux_pos_update (void);
void
-aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq);
+aux_traj_speed_start (struct aux_t *aux);
void
-aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm,
- uint8_t seq);
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos);
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- uint8_t seq);
+aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm);
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos,
- uint8_t seq);
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos);
+
+void
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed,
+ int16_t reset_pos);
void
aux_traj_update (void);
diff --git a/digital/mimot/src/dirty/avrconfig.h b/digital/mimot/src/dirty/avrconfig.h
index ca181554..9637b1f1 100644
--- a/digital/mimot/src/dirty/avrconfig.h
+++ b/digital/mimot/src/dirty/avrconfig.h
@@ -115,4 +115,57 @@
/** Slave transmission buffer size. */
#define AC_TWI_SLAVE_SEND_BUFFER_SIZE 16
+/* motor/encoder - Encoder module. */
+/** Use external encoder module. */
+#define AC_ENCODER_USE_EXT 1
+
+/* motor/encoder/ext - External encoder module. */
+/** Number of encoders. */
+#define AC_ENCODER_EXT_NB 2
+/** Use external memory hardware interface. */
+#define AC_ENCODER_EXT_USE_XMEM 0
+/** If not using XMEM, address/data bus, see io_bus.h. */
+#define AC_ENCODER_EXT_AD_BUS A, 4, 0, B, 4, 0
+/** If not using XMEM, address latch enable IO. */
+#define AC_ENCODER_EXT_ALE_IO B, 4
+/** If not using XMEM, read enable IO, valid low. */
+#define AC_ENCODER_EXT_RD_IO D, 6
+/** Reverse flag for each encoder (1 to reverse direction). */
+#define AC_ENCODER_EXT_REVERSE 0, 1
+/** Right shift for all encoders to lower resolution. */
+#define AC_ENCODER_EXT_SHIFT 1
+/** For debug purpose only! */
+#define AC_ENCODER_EXT_EXPORT_READ 0
+
+/* motor/output - Output module. */
+/** Use Output Compare PWM output. */
+#define AC_OUTPUT_USE_PWM_OCR 1
+/** Use Motor Power PWM output. */
+#define AC_OUTPUT_USE_PWM_MP 0
+/** Define module and module index for each output. */
+#define AC_OUTPUT_LIST (pwm_ocr, 0), (pwm_ocr, 1)
+
+/* motor/output/pwm_ocr - Output Compare PWM output module. */
+/** For each output, define output parameters:
+ *
+ * (timer, ocr, pwm_io, dir_io[, brake_io])
+ *
+ * With:
+ * - timer: timer number (ex: 1 for TIMER1)
+ * - ocr: output compare (ex: A for output compare A)
+ * - mode: compare output mode (ex: 2, see datasheet)
+ * - pwm_io: corresponding io port (ex: B, 1)
+ * - dir_io: io port used for direction (ex: B, 2)
+ * - brake_io: optional io port used for brake (ex: B, 3)
+ */
+#define AC_OUTPUT_PWM_OCR_LIST \
+ (1, A, 2, D,5, D,3, A,4), \
+ (1, B, 2, D,4, D,2, A,5)
+/** Clock select for each used timer. */
+#define AC_OUTPUT_PWM_OCR_CS_1 0b0001
+/** Waveform Generation Mode for each used timer. */
+#define AC_OUTPUT_PWM_OCR_WGM_1 0b0011
+/** Offset added to PWM value to compensate for H-bridge weakness. */
+#define AC_OUTPUT_PWM_OCR_OFFSET 0x40
+
#endif /* avrconfig_h */
diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c
deleted file mode 100644
index 01afecc2..00000000
--- a/digital/mimot/src/dirty/counter_ext.avr.c
+++ /dev/null
@@ -1,174 +0,0 @@
-/* counter_ext.avr.c - External counter support. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "counter.h"
-
-#include "modules/utils/utils.h"
-#include "modules/math/fixed/fixed.h"
-#include "io.h"
-
-/**
- * This file add support for an external counter like the hdlcounter or
- * avrcounter project. This can be better in order not to loose steps and
- * support more counters.
- */
-
-/** Define the first auxiliary counter address. */
-#define COUNTER_AUX0 0
-/** Define the second auxiliary counter address. */
-#define COUNTER_AUX1 1
-
-/** Define to 1 to reverse the first auxiliary counter. */
-#define COUNTER_AUX0_REVERSE 0
-/** Define to 1 to reverse the second auxiliary counter. */
-#define COUNTER_AUX1_REVERSE 1
-
-/** First auxiliary counter shift. */
-#define COUNTER_AUX0_SHIFT 1
-/** Second auxiliary counter shift. */
-#define COUNTER_AUX1_SHIFT 1
-
-/** 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];
-/** New values, being updated by step update. */
-static int16_t counter_aux_new_step[AC_ASSERV_AUX_NB];
-/** Last raw step values */
-static uint8_t counter_aux_old_step[AC_ASSERV_AUX_NB];
-/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
-/** Counter differences since last update.
- * Maximum of 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
-
-#if !COUNTER_USE_XMEM
-# define COUNTER_ALE_IO B, 4
-# define COUNTER_RD_IO D, 6
-#endif
-
-/** Read an external counter. */
-static inline uint8_t
-counter_read (uint8_t n)
-{
-#if COUNTER_USE_XMEM
- uint8_t * const ext = (void *) 0x1100;
- return ext[n];
-#else
- uint8_t v;
- PORTA = (PORTA & 0xf0) | (n & 0x0f);
- IO_CLR (COUNTER_ALE_IO);
- PORTA &= 0xf0;
- DDRA &= 0xf0;
- DDRB &= 0xf0;
- IO_CLR (COUNTER_RD_IO);
- utils_nop ();
- utils_nop ();
- v = (PINA & 0x0f) | (PINB & 0x0f) << 4;
- IO_SET (COUNTER_RD_IO);
- IO_SET (COUNTER_ALE_IO);
- DDRA |= 0x0f;
- DDRB |= 0x0f;
- return v;
-#endif
-}
-
-/** Initialize the counters. */
-void
-counter_init (void)
-{
-#if COUNTER_USE_XMEM
- /* Long wait-states. */
- XMCRA = _BV (SRW11);
- /* Do not use port C for address. */
- XMCRB = _BV (XMM2) | _BV (XMM1) | _BV (XMM0);
- /* Long wait-states and enable. */
- MCUCR |= _BV (SRE) | _BV (SRW10);
-#else
- IO_SET (COUNTER_ALE_IO);
- IO_SET (COUNTER_RD_IO);
- IO_OUTPUT (COUNTER_ALE_IO);
- IO_OUTPUT (COUNTER_RD_IO);
- PORTA &= 0xf0;
- PORTB &= 0xf0;
- DDRA |= 0x0f;
- DDRB |= 0x0f;
-#endif
- /* Begin with safe values. */
- counter_aux_old_step[0] = counter_read (COUNTER_AUX0);
- counter_aux_old_step[1] = counter_read (COUNTER_AUX1);
-}
-
-/** Update one step. If counters are not read fast enough, they could
- * overflow, call this function often to update step counters. */
-void
-counter_update_step (void)
-{
- uint8_t aux0, aux1;
- int8_t diff;
- /* Sample counters. */
- aux0 = counter_read (COUNTER_AUX0);
- aux1 = counter_read (COUNTER_AUX1);
- /* 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;
-}
-
-/** Update overall counter values and compute diffs. */
-void
-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]);
-#else
- counter_aux_diff[0] = (int16_t) (counter_aux_old[0] - aux0);
-#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]);
-#else
- counter_aux_diff[1] = (int16_t) (counter_aux_old[1] - aux1);
-#endif
- counter_aux_diff[1] >>= COUNTER_AUX1_SHIFT;
- counter_aux_old[1] = aux1;
- counter_aux[1] += counter_aux_diff[1];
-}
-
diff --git a/digital/mimot/src/dirty/cs.c b/digital/mimot/src/dirty/cs.c
new file mode 100644
index 00000000..73c2cbd1
--- /dev/null
+++ b/digital/mimot/src/dirty/cs.c
@@ -0,0 +1,69 @@
+/* cs.c - Control system definition. */
+/* {{{
+ *
+ * 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.
+ *
+ * }}} */
+#include "common.h"
+#include "cs.h"
+
+encoder_t encoder_aux[AC_ASSERV_AUX_NB];
+output_t output_aux[AC_ASSERV_AUX_NB];
+
+control_system_single_t cs_aux[AC_ASSERV_AUX_NB];
+
+void
+cs_init (void)
+{
+ uint8_t i;
+ /* Initialise encoders. */
+ encoder_init (0, &encoder_aux[0]);
+ encoder_init (1, &encoder_aux[1]);
+ /* Initialise outputs. */
+ output_aux[0].max = OUTPUT_MAX;
+ output_init (0, &output_aux[0]);
+ output_aux[1].max = OUTPUT_MAX;
+ output_init (1, &output_aux[1]);
+ /* Initialise control system. */
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ control_system_single_init (&cs_aux[i]);
+ cs_aux[i].encoder = &encoder_aux[i];
+ cs_aux[i].output = &output_aux[i];
+ }
+}
+
+void
+cs_update_prepare (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ control_system_single_update_prepare (&cs_aux[i]);
+}
+
+void
+cs_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ control_system_single_update (&cs_aux[i]);
+}
+
diff --git a/digital/mimot/src/dirty/counter.h b/digital/mimot/src/dirty/cs.h
index b4b4979d..ab3c3a62 100644
--- a/digital/mimot/src/dirty/counter.h
+++ b/digital/mimot/src/dirty/cs.h
@@ -1,9 +1,9 @@
-#ifndef counter_h
-#define counter_h
-/* counter.h */
+#ifndef cs_h
+#define cs_h
+/* cs.h - Control system definition. */
/* asserv - Position & speed motor control on AVR. {{{
*
- * Copyright (C) 2008 Nicolas Schodet
+ * Copyright (C) 2011 Nicolas Schodet
*
* APBTeam:
* Web: http://apbteam.org/
@@ -24,17 +24,20 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "modules/motor/control_system/control_system.h"
-extern uint16_t counter_aux[AC_ASSERV_AUX_NB];
-extern int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+extern encoder_t encoder_aux[AC_ASSERV_AUX_NB];
+extern output_t output_aux[AC_ASSERV_AUX_NB];
+
+extern control_system_single_t cs_aux[AC_ASSERV_AUX_NB];
void
-counter_init (void);
+cs_init (void);
void
-counter_update_step (void);
+cs_update_prepare (void);
void
-counter_update (void);
+cs_update (void);
-#endif /* counter_h */
+#endif /* cs_h */
diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c
index a20032d1..409ab190 100644
--- a/digital/mimot/src/dirty/eeprom.avr.c
+++ b/digital/mimot/src/dirty/eeprom.avr.c
@@ -29,10 +29,7 @@
#include <avr/eeprom.h>
-#include "counter.h"
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
+#include "cs.h"
#define EEPROM_START 0
@@ -49,29 +46,33 @@ 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++);
- pwm_set_reverse (eeprom_read_byte (p8++));
+ cs_aux[0].speed.max = eeprom_read_byte (p8++);
+ cs_aux[1].speed.max = eeprom_read_byte (p8++);
+ cs_aux[0].speed.slow = eeprom_read_byte (p8++);
+ cs_aux[1].speed.slow = eeprom_read_byte (p8++);
+ output_set_reverse (&output_aux[0], eeprom_read_byte (p8++));
+ output_set_reverse (&output_aux[1], eeprom_read_byte (p8++));
p16 = (uint16_t *) p8;
- speed_aux[0].acc = eeprom_read_word (p16++);
- speed_aux[1].acc = eeprom_read_word (p16++);
- pos_aux[0].kp = eeprom_read_word (p16++);
- pos_aux[1].kp = eeprom_read_word (p16++);
- pos_aux[0].ki = eeprom_read_word (p16++);
- pos_aux[1].ki = eeprom_read_word (p16++);
- pos_aux[0].kd = eeprom_read_word (p16++);
- pos_aux[1].kd = eeprom_read_word (p16++);
- pos_aux[0].blocked_error_limit = eeprom_read_word (p16++);
- pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++);
- pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++);
- pos_aux[1].blocked_error_limit = eeprom_read_word (p16++);
- pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++);
- pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++);
- pos_e_sat = eeprom_read_word (p16++);
- pos_i_sat = eeprom_read_word (p16++);
- pos_d_sat = eeprom_read_word (p16++);
+ cs_aux[0].speed.acc = eeprom_read_word (p16++);
+ cs_aux[1].speed.acc = eeprom_read_word (p16++);
+ cs_aux[0].pos.kp = eeprom_read_word (p16++);
+ cs_aux[1].pos.kp = eeprom_read_word (p16++);
+ cs_aux[0].pos.ki = eeprom_read_word (p16++);
+ cs_aux[1].pos.ki = eeprom_read_word (p16++);
+ cs_aux[0].pos.kd = eeprom_read_word (p16++);
+ cs_aux[1].pos.kd = eeprom_read_word (p16++);
+ cs_aux[0].blocking_detection.error_limit = eeprom_read_word (p16++);
+ cs_aux[0].blocking_detection.speed_limit = eeprom_read_word (p16++);
+ cs_aux[0].blocking_detection.counter_limit = eeprom_read_word (p16++);
+ cs_aux[1].blocking_detection.error_limit = eeprom_read_word (p16++);
+ cs_aux[1].blocking_detection.speed_limit = eeprom_read_word (p16++);
+ cs_aux[1].blocking_detection.counter_limit = eeprom_read_word (p16++);
+ cs_aux[0].pos.e_sat = eeprom_read_word (p16++);
+ cs_aux[0].pos.i_sat = eeprom_read_word (p16++);
+ cs_aux[0].pos.d_sat = eeprom_read_word (p16++);
+ cs_aux[1].pos.e_sat = eeprom_read_word (p16++);
+ cs_aux[1].pos.i_sat = eeprom_read_word (p16++);
+ cs_aux[1].pos.d_sat = eeprom_read_word (p16++);
}
/* Write parameters to eeprom. */
@@ -81,29 +82,33 @@ eeprom_write_params (void)
uint8_t *p8 = (uint8_t *) EEPROM_START;
uint16_t *p16;
eeprom_write_byte (p8++, EEPROM_KEY);
- eeprom_write_byte (p8++, speed_aux[0].max);
- eeprom_write_byte (p8++, speed_aux[1].max);
- eeprom_write_byte (p8++, speed_aux[0].slow);
- eeprom_write_byte (p8++, speed_aux[1].slow);
- eeprom_write_byte (p8++, pwm_reverse);
+ eeprom_write_byte (p8++, cs_aux[0].speed.max);
+ eeprom_write_byte (p8++, cs_aux[1].speed.max);
+ eeprom_write_byte (p8++, cs_aux[0].speed.slow);
+ eeprom_write_byte (p8++, cs_aux[1].speed.slow);
+ eeprom_write_byte (p8++, output_aux[0].reverse);
+ eeprom_write_byte (p8++, output_aux[1].reverse);
p16 = (uint16_t *) p8;
- eeprom_write_word (p16++, speed_aux[0].acc);
- eeprom_write_word (p16++, speed_aux[1].acc);
- eeprom_write_word (p16++, pos_aux[0].kp);
- eeprom_write_word (p16++, pos_aux[1].kp);
- eeprom_write_word (p16++, pos_aux[0].ki);
- eeprom_write_word (p16++, pos_aux[1].ki);
- eeprom_write_word (p16++, pos_aux[0].kd);
- eeprom_write_word (p16++, pos_aux[1].kd);
- eeprom_write_word (p16++, pos_aux[0].blocked_error_limit);
- eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit);
- eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit);
- eeprom_write_word (p16++, pos_aux[1].blocked_error_limit);
- eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit);
- eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit);
- eeprom_write_word (p16++, pos_e_sat);
- eeprom_write_word (p16++, pos_i_sat);
- eeprom_write_word (p16++, pos_d_sat);
+ eeprom_write_word (p16++, cs_aux[0].speed.acc);
+ eeprom_write_word (p16++, cs_aux[1].speed.acc);
+ eeprom_write_word (p16++, cs_aux[0].pos.kp);
+ eeprom_write_word (p16++, cs_aux[1].pos.kp);
+ eeprom_write_word (p16++, cs_aux[0].pos.ki);
+ eeprom_write_word (p16++, cs_aux[1].pos.ki);
+ eeprom_write_word (p16++, cs_aux[0].pos.kd);
+ eeprom_write_word (p16++, cs_aux[1].pos.kd);
+ eeprom_write_word (p16++, cs_aux[0].blocking_detection.error_limit);
+ eeprom_write_word (p16++, cs_aux[0].blocking_detection.speed_limit);
+ eeprom_write_word (p16++, cs_aux[0].blocking_detection.counter_limit);
+ eeprom_write_word (p16++, cs_aux[1].blocking_detection.error_limit);
+ eeprom_write_word (p16++, cs_aux[1].blocking_detection.speed_limit);
+ eeprom_write_word (p16++, cs_aux[1].blocking_detection.counter_limit);
+ eeprom_write_word (p16++, cs_aux[0].pos.e_sat);
+ eeprom_write_word (p16++, cs_aux[0].pos.i_sat);
+ eeprom_write_word (p16++, cs_aux[0].pos.d_sat);
+ eeprom_write_word (p16++, cs_aux[1].pos.e_sat);
+ eeprom_write_word (p16++, cs_aux[1].pos.i_sat);
+ eeprom_write_word (p16++, cs_aux[1].pos.d_sat);
}
/* 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..ed703945 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -31,14 +31,11 @@
#include "modules/math/fixed/fixed.h"
#include "io.h"
-#include "state.h"
-
-#include "counter.h"
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
+#include "cs.h"
#include "aux.h"
+#include "seq.h"
+
#include "twi_proto.h"
#include "eeprom.h"
@@ -88,12 +85,10 @@ main (int argc, char **argv)
PORTB = 0xe0;
PORTC = 0xfc;
PORTD = 0x80;
- pwm_init ();
timer_init ();
- counter_init ();
uart0_init ();
twi_proto_init ();
- speed_init ();
+ cs_init ();
aux_init ();
eeprom_read_params ();
proto_send0 ('z');
@@ -107,37 +102,42 @@ main (int argc, char **argv)
static void
main_loop (void)
{
+ main_timer[3] = timer_read ();
+ /* Compute absolute position. */
+ aux_pos_update ();
+ main_timer[4] = timer_read ();
+ /* Compute trajectory. */
+ aux_traj_update ();
+ /* Prepare control system. */
+ cs_update_prepare ();
main_timer[5] = timer_read ();
+ /* Wait for next cycle. */
timer_wait ();
- /* Counter update. */
- counter_update ();
+ /* Encoder update. */
+ encoder_update ();
main_timer[0] = timer_read ();
- /* Position control. */
- pos_update ();
+ /* Control system update. */
+ cs_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
- pwm_update ();
+ output_update ();
main_timer[2] = timer_read ();
- /* Compute absolute position. */
- aux_pos_update ();
- /* Compute trajectory. */
- aux_traj_update ();
- /* Speed control. */
- speed_update ();
- main_timer[3] = timer_read ();
+ /* Sequences. */
+ seq_update (&seq_aux[0], &cs_aux[0].state);
+ seq_update (&seq_aux[1], &cs_aux[1].state);
/* 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)
+ && (seq_aux[0].ack != seq_aux[0].finish
+ || seq_aux[1].ack != seq_aux[1].finish)
&& !--main_sequence_ack_cpt)
{
- proto_send2b ('A', state_aux[0].sequence_finish,
- state_aux[1].sequence_finish);
+ //XXX here
+ proto_send2b ('A', seq_aux[0].finish, seq_aux[1].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', encoder_aux[0].cur, encoder_aux[1].cur);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
@@ -147,18 +147,19 @@ main_loop (void)
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ proto_send2b ('S', cs_aux[0].speed.cur >> 8,
+ cs_aux[1].speed.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', cs_aux[0].pos.last_error, cs_aux[0].pos.i,
+ cs_aux[1].pos.last_error, cs_aux[1].pos.i);
main_stat_pos_cpt = main_stat_pos;
}
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur);
+ proto_send2w ('W', output_aux[0].cur, output_aux[1].cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -176,7 +177,6 @@ main_loop (void)
while (uart0_poll ())
proto_accept (uart0_getc ());
twi_proto_update ();
- main_timer[4] = timer_read ();
}
/** Handle incoming messages. */
@@ -185,17 +185,20 @@ 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;
+ pos_control_t *pos = 0;
+ speed_control_t *speed = 0;
+ control_state_t *state = 0;
+ blocking_detection_t *bd = 0;
+ output_t *output = 0;
+ seq_t *seq = 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]];
+ pos = &cs_aux[args[0]].pos;
+ speed = &cs_aux[args[0]].speed;
+ state = &cs_aux[args[0]].state;
+ output = &output_aux[args[0]];
+ seq = &seq_aux[args[0]];
}
/* Decode command. */
#define c(cmd, size) (cmd << 8 | size)
@@ -208,49 +211,41 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* 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);
+ output_set (&output_aux[0], 0);
+ output_set (&output_aux[1], 0);
+ control_state_set_mode (&cs_aux[0].state, CS_MODE_NONE, 0);
+ control_state_set_mode (&cs_aux[1].state, CS_MODE_NONE, 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]));
+ output_set (output, v8_to_v16 (args[1], args[2]));
+ control_state_set_mode (state, CS_MODE_NONE, 0);
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]);
+ control_state_set_mode (state, CS_MODE_POS_CONTROL, 0);
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;
+ speed_control_set_speed (speed, 0);
+ control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 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;
+ speed_control_set_speed (speed, args[1]);
+ control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('s', 6):
/* Set auxiliary speed controlled position consign.
@@ -258,12 +253,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: consign offset.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (!seq_start (seq, args[5]))
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_control_pos_offset (speed, v8_to_v32 (args[1], args[2], args[3],
+ args[4]));
+ aux_traj_speed_start (auxp);
+ break;
break;
case c ('y', 4):
/* Auxiliary go to position.
@@ -271,9 +266,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: pos, i16.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[3] == state->sequence)
+ if (!seq_start (seq, args[3]))
break;
- aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
+ aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]));
break;
case c ('y', 5):
/* Auxiliary clamp.
@@ -282,10 +277,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: clamping PWM.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[4] == state->sequence)
+ if (!seq_start (seq, args[4]))
break;
- aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]),
- args[4]);
+ aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]));
break;
case c ('y', 6):
/* Auxiliary find zero.
@@ -295,21 +289,21 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: reset position.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (!seq_start (seq, args[5]))
break;
if (args[2])
aux_traj_find_zero_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]), args[5]);
+ v8_to_v16 (args[3], args[4]));
else
aux_traj_find_limit_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]), args[5]);
+ v8_to_v16 (args[3], args[4]));
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]);
+ seq_acknowledge (&seq_aux[0], args[0]);
+ seq_acknowledge (&seq_aux[1], args[1]);
break;
/* Stats.
* - b: interval between stats. */
@@ -354,12 +348,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
case 0:
case 1:
- pos = &pos_aux[args[1]];
- speed = &speed_aux[args[1]];
+ pos = &cs_aux[args[1]].pos;
+ speed = &cs_aux[args[1]].speed;
+ bd = &cs_aux[args[1]].blocking_detection;
break;
default:
pos = 0;
speed = 0;
+ bd = 0;
break;
}
switch (c (args[0], size))
@@ -407,24 +403,31 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: error limit.
* - w: speed limit.
* - b: counter limit. */
- if (!pos) { proto_send0 ('?'); return; }
- pos->blocked_error_limit = v8_to_v16 (args[2], args[3]);
- pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]);
- pos->blocked_counter_limit = args[6];
+ if (!bd) { proto_send0 ('?'); return; }
+ bd->error_limit = v8_to_v16 (args[2], args[3]);
+ bd->speed_limit = v8_to_v16 (args[4], args[5]);
+ bd->counter_limit = args[6];
break;
case c ('E', 3):
- pos_e_sat = v8_to_v16 (args[1], args[2]);
+ cs_aux[0].pos.e_sat =
+ cs_aux[1].pos.e_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('I', 3):
- pos_i_sat = v8_to_v16 (args[1], args[2]);
+ cs_aux[0].pos.i_sat =
+ cs_aux[1].pos.i_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('D', 3):
- pos_d_sat = v8_to_v16 (args[1], args[2]);
+ cs_aux[0].pos.d_sat =
+ cs_aux[1].pos.d_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('w', 2):
/* Set PWM direction.
- * - b: bits: 0000[aux1][aux0][right][left]. */
- pwm_set_reverse (args[1]);
+ * - b: bits: 000000[aux1][aux0]. */
+ output_set_reverse (&output_aux[0], (args[1] & 1) ? 1 : 0);
+ output_set_reverse (&output_aux[1], (args[1] & 2) ? 1 : 0);
break;
case c ('E', 2):
/* Write to eeprom.
@@ -437,22 +440,23 @@ 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_send1w ('E', pos_e_sat);
- proto_send1w ('I', pos_i_sat);
- proto_send1w ('D', pos_d_sat);
- proto_send1b ('w', pwm_reverse);
+ proto_send1b ('w', (output_aux[0].reverse ? 1 : 0)
+ | (output_aux[1].reverse ? 2 : 0));
+ break;
+ case c ('P', 2):
+ /* Print current settings for selected control.
+ * - b: index. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send1w ('a', speed->acc);
+ proto_send2b ('s', speed->max, speed->slow);
+ proto_send3w ('b', bd->error_limit, bd->speed_limit,
+ bd->counter_limit);
+ proto_send1w ('p', pos->kp);
+ proto_send1w ('i', pos->ki);
+ proto_send1w ('d', pos->kd);
+ proto_send1w ('E', pos->e_sat);
+ proto_send1w ('I', pos->i_sat);
+ proto_send1w ('D', pos->d_sat);
break;
default:
proto_send0 ('?');
diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c
index db4152e0..bdd19e62 100644
--- a/digital/mimot/src/dirty/models.host.c
+++ b/digital/mimot/src/dirty/models.host.c
@@ -25,73 +25,45 @@
#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */
#include "common.h"
-#include "motor_model.host.h"
#include "models.host.h"
#include "simu.host.h"
#include <math.h>
#include <string.h>
-/* Marcel clamp, with a Faulhaber 2342 and 23/1 3.71:1 gearbox model. */
-static const struct motor_def_t marcel_clamp_f2342_model =
-{
- /* Motor characteristics. */
- 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
- 26.10 / 1000, /* Torque constant (N.m/A). */
- 0, /* Bearing friction (N.m/(rad/s)). */
- 7.10, /* Terminal resistance (Ohm). */
- 0.265 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 3.71, /* Gearbox ratio. */
- 0.88, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */
- /* This is a pifometric estimation. */
- /* 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). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 16, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.200 * 0.010 * 0.010, /* Load (kg.m^2). */
- /* This is a pifometric estimation. */
- /* 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 },
+ { &motor_model_def_faulhaber_2342_x3_71,
+ &motor_model_def_faulhaber_2342_x3_71 },
+ /** Motors voltage (V). */
+ 24.0,
/** Number of steps for each auxiliary motor encoder. */
{ 256, 256 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 0.100 * 0.005 * 0.005, 0.100 * 0.005 * 0.005 },
/** Sensor update function. */
simu_sensor_update_marcel,
+ /** Initialisation function. */
+ NULL,
};
/* Robospierre, APBTeam 2011. */
static const struct robot_t robospierre_robot =
{
/** Auxiliary motors, NULL if not present. */
- { &marcel_clamp_f2342_model, &robospierre_rotation_amax32ghp_model },
+ { &motor_model_def_faulhaber_2342_x3_71, &motor_model_def_amax32ghp_x16 },
+ /** Motors voltage (V). */
+ 24.0,
/** Number of steps for each auxiliary motor encoder. */
{ 256, 250 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 0.100 * 0.005 * 0.005, 0.200 * 0.010 * 0.010 },
/** Sensor update function. */
simu_sensor_update_robospierre,
+ /** Initialisation function. */
+ NULL,
};
/* Table of models. */
@@ -120,7 +92,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, motor_model_t aux_motor[])
{
int i;
if (aux_motor)
@@ -130,10 +102,13 @@ models_init (const struct robot_t *robot, struct motor_t aux_motor[])
if (robot->aux_motor[i])
{
aux_motor[i].m = *robot->aux_motor[i];
+ aux_motor[i].m.J = robot->aux_load[i];
aux_motor[i].h = ECHANT_PERIOD;
aux_motor[i].d = 1000;
}
}
}
+ if (robot->init)
+ robot->init (robot, aux_motor);
}
diff --git a/digital/mimot/src/dirty/models.host.h b/digital/mimot/src/dirty/models.host.h
index 0e53aaea..8733f05b 100644
--- a/digital/mimot/src/dirty/models.host.h
+++ b/digital/mimot/src/dirty/models.host.h
@@ -24,6 +24,7 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "modules/motor/motor_model/motor_model_defs.host.h"
#define ECHANT_PERIOD (1.0 / ((double) AC_FREQ / 256 / 256))
@@ -32,11 +33,17 @@
struct robot_t
{
/** Auxiliary motors, NULL if not present. */
- const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB];
+ const motor_model_def_t *aux_motor[AC_ASSERV_AUX_NB];
+ /** Motors voltage (V). */
+ double u_max;
/** Number of steps for each auxiliary motor encoder. */
int aux_encoder_steps[AC_ASSERV_AUX_NB];
+ /** Load for auxiliary motors (kg.m^2). */
+ double aux_load[AC_ASSERV_AUX_NB];
/** Sensor update function. */
void (*sensor_update) (void);
+ /** Initialisation function. */
+ void (*init) (const struct robot_t *robot, motor_model_t aux_motor[]);
};
/** Get a pointer to a model by name, or return 0. */
@@ -45,6 +52,6 @@ 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, motor_model_t aux_motor[]);
#endif /* models_host_h */
diff --git a/digital/mimot/src/dirty/motor_model.host.c b/digital/mimot/src/dirty/motor_model.host.c
deleted file mode 100644
index f87e7db6..00000000
--- a/digital/mimot/src/dirty/motor_model.host.c
+++ /dev/null
@@ -1,100 +0,0 @@
-/* motor_model.c - DC motor model. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2006 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "motor_model.host.h"
-
-/**
- * Switching to french for all those non english speaking people.
- *
- * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux
- * parties, la modélisation électrique et la modélisation mécanique.
- *
- * On peut trouver de l'aide sur :
- * - le site de maxon : http://www.maxonmotor.com/
- * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html
- * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm
- *
- * Voici ce qui en résulte, des belles équations différentielles :
- * u(t) = R i(t) + L di(t)/dt + 1/Ke o(t)
- * J do(t)/dt = Kt i(t) - Rf o(t)
- * dth(t)/dt = o(t)
- *
- * Les variables sont décrites dans la structure motor_t.
- *
- * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en
- * exercice.
- *
- * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il
- * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive
- * à :
- *
- * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t))
- * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t))
- * th(t+h) = th(t) + h o(t)
- *
- * C'est consternant de simplicité non ?
- */
-
-/** Make a simulation step. */
-void motor_model_step (struct motor_t *m)
-{
- double i_, o_, th_; /* New computed values. */
- double h; /* Infinitesimal step... Well, not so infinite. */
- int d;
- d = m->d;
- h = m->h / d;
- /* Make several small steps to increase precision. */
- for (; d; d--)
- {
- /* Ah, the mystical power of computation. */
- i_ = m->i
- + h * (1.0 / m->m.L * m->u
- - m->m.R / m->m.L * m->i
- - 1.0 / m->m.Ke / m->m.L * m->o);
- o_ = m->o
- + h * m->m.i_G * m->m.i_G * m->m.ro_G / m->m.J
- * (m->m.Kt * m->i - m->m.Rf * m->o);
- th_ = m->th + h * m->o;
- /* Test for limits overflow, I have no proof it works right for the
- * moment, only suspicions. */
- if (th_ < m->m.th_min)
- {
- th_ = m->m.th_min;
- o_ = 0.0;
- }
- else if (th_ > m->m.th_max)
- {
- th_ = m->m.th_max;
- o_ = 0.0;
- }
- /* Ok, now store this step. */
- m->i = i_;
- m->o = o_;
- m->th = th_;
- }
- /* Damn! It's finished yet! */
- m->t += m->h;
-}
-
diff --git a/digital/mimot/src/dirty/motor_model.host.h b/digital/mimot/src/dirty/motor_model.host.h
deleted file mode 100644
index 283b6fd6..00000000
--- a/digital/mimot/src/dirty/motor_model.host.h
+++ /dev/null
@@ -1,68 +0,0 @@
-#ifndef motor_model_host_h
-#define motor_model_host_h
-/* motor_model.host.h - DC motor model. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2006 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/** Motor and load characteristics. */
-struct motor_def_t
-{
- /* Motor characteristics. */
- double Ke; /* Speed constant ((rad/s)/V). */
- double Kt; /* Torque constant (N.m/A). */
- double Rf; /* Bearing friction (N.m/(rad/s)). */
- double R; /* Terminal resistance (Ohm). */
- double L; /* Terminal inductance (H). */
- double u_max;/* Maximum voltage (V). */
- /* Gearbox characteristics. */
- double i_G; /* Gearbox ratio. */
- double ro_G;/* Gearbox efficiency. */
- /* Load characteristics. */
- double J; /* Load at gearbox output (kg.m^2). */
- /* Hardware limits (use +/-INFINITY for none). */
- double th_min; /* Minimum theta value. */
- double th_max; /* Maximum theta value. */
-};
-
-/** Motor and load characteristics and current data. Angular speed and theta
- * are at motor output, not gearbox output. */
-struct motor_t
-{
- /* Motor and load characteristics. */
- struct motor_def_t m;
- /* Simulation parameters. */
- double h; /* Simulation time step (s). */
- int d; /* Simulation time step division. */
- /* Simulation current state. */
- double t; /* Current time (not really used) (s). */
- double u; /* Current input voltage (V). */
- double i; /* Current current (A). */
- double o; /* Current angular speed (o for omega) (rad/s). */
- double th; /* Current theta (th for theta) (rad). */
-};
-
-/** Make a simulation step. */
-void motor_model_step (struct motor_t *m);
-
-#endif /* motor_model_host_h */
diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c
deleted file mode 100644
index 28684414..00000000
--- a/digital/mimot/src/dirty/pos.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/* pos.c - Position motor control. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2005 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "pos.h"
-
-#include "modules/utils/utils.h"
-#include "modules/math/fixed/fixed.h"
-
-#include "counter.h"
-#include "pwm.h"
-#include "state.h"
-
-/**
- * This file is responsible for position motor control. The consign is a
- * position of the motor shafts, as theta/alpha. Theta is the sum of right
- * and left position, alpha is the difference between the right and the left
- * position.
- * 16 bits are enough as long as there is no long blocking (see 2005 cup!).
- */
-
-/** Auxiliaries control states. */
-struct pos_t pos_aux[AC_ASSERV_AUX_NB];
-
-/** Error saturation. */
-int32_t pos_e_sat = 1023;
-/** Integral saturation. */
-int32_t pos_i_sat = 1023;
-/** Differential saturation. */
-int32_t pos_d_sat = 1023;
-
-/** Compute a PID.
- * How to compute maximum numbers size:
- * Result is 24 bits (16 bits kept after shift).
- * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff
- * max is 12 bits (can be saturated with d_sat).
- * If i_sat == 1023, i max is 11 bits.
- * In the final addition, let's give 23 bits to the p part, and 22 bits to the
- * i and d part (23b + 22b + 22b => 23b + 23b => 24b).
- * Therefore, kp can be 23 - 11 = 12 bits (f4.8).
- * ki can be 22 - 11 = 11 bits (f3.8).
- * kd can be 22 - 12 = 10 bits (f2.8).
- * How to increase this number:
- * - lower the shift.
- * - bound the value returned.
- * - lower e, i & d saturation. */
-static inline int16_t
-pos_compute_pid (int32_t e, struct pos_t *pos)
-{
- int32_t diff, pid;
- /* Saturate error. */
- UTILS_BOUND (e, -pos_e_sat, pos_e_sat);
- /* Integral update. */
- pos->i += e;
- UTILS_BOUND (pos->i, -pos_i_sat, pos_i_sat);
- /* Differential value. */
- diff = e - pos->e_old;
- UTILS_BOUND (diff, -pos_d_sat, pos_d_sat);
- /* Compute PID. */
- pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd;
- /* Save result. */
- pos->e_old = e;
- return pid >> 8;
-}
-
-/** Update PWM for a single motor system. */
-static void
-pos_update_single (struct state_t *state, struct pos_t *pos,
- int16_t counter_diff, struct pwm_t *pwm)
-{
- if (state->mode >= MODE_POS)
- {
- int16_t pid;
- int32_t error;
- /* Update current shaft position. */
- pos->cur += counter_diff;
- /* Compute error. */
- error = pos->cons - pos->cur;
- /* Test or blocking. */
- if (UTILS_ABS (error) > pos->blocked_error_limit
- && UTILS_ABS (counter_diff) < pos->blocked_speed_limit)
- pos->blocked_counter++;
- else
- pos->blocked_counter = 0;
- if (state->variant & 1)
- {
- pos_reset (pos);
- }
- else if (!(state->variant & 4)
- && pos->blocked_counter > pos->blocked_counter_limit)
- {
- /* Blocked. */
- pos_reset (pos);
- state_blocked (state);
- pwm_set (pwm, 0);
- }
- else
- {
- /* Compute PID. */
- pid = pos_compute_pid (error, pos);
- /* Update PWM. */
- pwm_set (pwm, pid);
- }
- }
-}
-
-/** Update PWM according to consign. */
-void
-pos_update (void)
-{
- uint8_t i;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i],
- &pwm_aux[i]);
-}
-
-/** Reset position control state. To be called when the position control is
- * deactivated. */
-void
-pos_reset (struct pos_t *pos)
-{
- pos->i = 0;
- pos->cur = 0;
- pos->cons = 0;
- pos->e_old = 0;
- pos->blocked_counter = 0;
-}
-
diff --git a/digital/mimot/src/dirty/pos.h b/digital/mimot/src/dirty/pos.h
deleted file mode 100644
index 514b091c..00000000
--- a/digital/mimot/src/dirty/pos.h
+++ /dev/null
@@ -1,61 +0,0 @@
-#ifndef pos_h
-#define pos_h
-/* pos.h */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/** Position control state. */
-struct pos_t
-{
- /** Current position. */
- uint32_t cur;
- /** Consign position. */
- uint32_t cons;
- /** PID coefficients (f8.8, maximum depends on saturation values). */
- uint16_t kp, ki, kd;
- /** Current integral value. */
- int32_t i;
- /** Last error value. */
- int32_t e_old;
- /** Blocking detection: error limit. */
- int32_t blocked_error_limit;
- /** Blocking detection: speed limit. */
- int16_t blocked_speed_limit;
- /** Blocking detection: counter limit. */
- uint8_t blocked_counter_limit;
- /** Count the number of blocked detection. */
- uint8_t blocked_counter;
-};
-
-extern struct pos_t pos_aux[AC_ASSERV_AUX_NB];
-
-extern int32_t pos_e_sat, pos_i_sat, pos_d_sat;
-
-void
-pos_update (void);
-
-void
-pos_reset (struct pos_t *pos);
-
-#endif /* pos_h */
diff --git a/digital/mimot/src/dirty/pwm.avr.c b/digital/mimot/src/dirty/pwm.avr.c
deleted file mode 100644
index 37ec34b0..00000000
--- a/digital/mimot/src/dirty/pwm.avr.c
+++ /dev/null
@@ -1,57 +0,0 @@
-/* pwm.avr.c - Handle all PWM generators. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2005 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "pwm.h"
-#include "pwm_ocr.avr.h"
-
-/** PWM control states. */
-struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
- PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
-};
-/** PWM reverse directions. */
-uint8_t pwm_reverse;
-
-/** Initialise PWM generators. */
-void
-pwm_init (void)
-{
- pwm_ocr_init ();
-}
-
-/** Update the hardware PWM values. */
-void
-pwm_update (void)
-{
- pwm_ocr_update ();
-}
-
-/** Set which PWM is reversed. */
-void
-pwm_set_reverse (uint8_t reverse)
-{
- pwm_reverse = reverse;
- pwm_ocr_set_reverse (reverse);
-}
-
diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h
deleted file mode 100644
index b4f12d0d..00000000
--- a/digital/mimot/src/dirty/pwm.h
+++ /dev/null
@@ -1,101 +0,0 @@
-#ifndef pwm_h
-#define pwm_h
-/* pwm.h */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/** Offset to compensate for H-bridge dead zone at low PWM values. */
-#define PWM_OFFSET 0x40
-
-/** Define the absolute maximum PWM value. */
-#define PWM_MAX (0x3ff - PWM_OFFSET)
-
-/** PWM control state. */
-struct pwm_t
-{
- /** Current PWM value. */
- int16_t cur;
- /** Maximum value. */
- int16_t max;
- /** Minimum value (dead zone). */
- int16_t min;
-};
-
-extern struct pwm_t pwm_aux[AC_ASSERV_AUX_NB];
-
-extern uint8_t pwm_reverse;
-
-/** Define current PWM value for each output. */
-#define PWM_VALUE(x) PWM_VALUE_ (x)
-#define PWM_VALUE_(x) PWM_VALUE_ ## x
-#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur
-#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur
-
-/** Define maximum PWM value for each output. */
-#define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x)
-#define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x
-#define PWM_MAX_FOR_pwm_aux0 PWM_MAX
-#define PWM_MAX_FOR_pwm_aux1 PWM_MAX
-
-/** Define minimum PWM value for each output, if the value is less than the
- * minimum, use 0. */
-#define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x)
-#define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x
-#define PWM_MIN_FOR_pwm_aux0 0x10
-#define PWM_MIN_FOR_pwm_aux1 0x10
-
-/** Define which bit controls the PWM inversion. */
-#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x)
-#define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x
-#define PWM_REVERSE_BIT_pwm_aux0 _BV (0)
-#define PWM_REVERSE_BIT_pwm_aux1 _BV (1)
-
-/** State init macro. */
-#define PWM_INIT_FOR(x) \
- { 0, PWM_MAX_FOR (x), PWM_MIN_FOR (x) }
-
-/** Set PWM value. */
-static inline void
-pwm_set (struct pwm_t *pwm, int16_t value)
-{
- if (value > pwm->max)
- pwm->cur = pwm->max;
- else if (value < -pwm->max)
- pwm->cur = -pwm->max;
- else if (value > -pwm->min && value < pwm->min)
- pwm->cur = 0;
- else
- pwm->cur = value;
-}
-
-void
-pwm_init (void);
-
-void
-pwm_update (void);
-
-void
-pwm_set_reverse (uint8_t reverse);
-
-#endif /* pwm_h */
diff --git a/digital/mimot/src/dirty/pwm_config.h b/digital/mimot/src/dirty/pwm_config.h
deleted file mode 100644
index e937002e..00000000
--- a/digital/mimot/src/dirty/pwm_config.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef pwm_config_h
-#define pwm_config_h
-/* pwm_config.h - Helper for PWM configuration. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/* Simplify conditionnal compilation. */
-#define PWM1or2 (defined (PWM1) || defined (PWM2))
-#ifdef PWM1
-# define PWM1c(x) x
-#else
-# define PWM1c(x) 0
-#endif
-#ifdef PWM2
-# define PWM2c(x) x
-#else
-# define PWM2c(x) 0
-#endif
-
-#endif /* pwm_config_h */
diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c
deleted file mode 100644
index a1d98c25..00000000
--- a/digital/mimot/src/dirty/pwm_ocr.avr.c
+++ /dev/null
@@ -1,161 +0,0 @@
-/* pwm_ocr.avr.c - PWM using internal generator. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "pwm_ocr.avr.h"
-#include "pwm.h"
-
-#include "modules/utils/utils.h"
-#include "io.h"
-
-/** Assign PWM outputs. */
-#define PWM1 pwm_aux0
-#define PWM2 pwm_aux1
-
-#define PWM1_OCR OCR1A
-#define PWM1_OCR_BIT 5
-#define PWM1_DIR 3
-#define PWM2_OCR OCR1B
-#define PWM2_OCR_BIT 4
-#define PWM2_DIR 2
-
-#define PWM1_BRK_IO A, 4
-#define PWM2_BRK_IO A, 5
-
-#include "pwm_config.h"
-
-/** PWM reverse direction, port D. */
-static uint8_t pwm_ocr_dir_reverse_d;
-
-/** Initialise PWM generator. */
-void
-pwm_ocr_init (void)
-{
- /* Phase Correct PWM, TOP = 0x3ff, OCnA & OCnB with positive logic.
- * f_IO without prescaler.
- * Fpwm = f_IO / (2 * prescaler * (1 + TOP)) = 7200 Hz. */
-#if PWM1or2
- TCCR1A =
- regv (COM1A1, COM1A0, COM1B1, COM1B0, FOC1A, FOC1B, WGM11, WGM10,
- 1, 0, 1, 0, 0, 0, 1, 1);
- TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
- 0, 0, 0, 0, 0, 0, 0, 1);
- /* Enable PWM and direction outputs in DDRD. */
- DDRD |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR))
- | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR));
- /* Will activate output at first non zero PWM. */
-# ifdef PWM1
- IO_OUTPUT (PWM1_BRK_IO);
-# endif
-# ifdef PWM2
- IO_OUTPUT (PWM2_BRK_IO);
-# endif
-#endif
-}
-
-/** Update the hardware PWM values. */
-void
-pwm_ocr_update (void)
-{
-#if PWM1or2
- uint8_t dir_d;
- /* Sample port D. */
- dir_d = PORTD & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR)));
-# ifdef PWM1
- uint16_t pwm1;
- /* Set PWM1. */
- if (PWM_VALUE (PWM1) == 0)
- {
- pwm1 = 0;
- }
- else
- {
- IO_SET (PWM1_BRK_IO);
- if (PWM_VALUE (PWM1) < 0)
- {
- pwm1 = -PWM_VALUE (PWM1) + PWM_OFFSET;
- }
- else
- {
- dir_d |= _BV (PWM1_DIR);
- pwm1 = PWM_VALUE (PWM1) + PWM_OFFSET;
- }
- }
-# endif /* PWM1 */
-# ifdef PWM2
- uint16_t pwm2;
- /* Set PWM2. */
- if (PWM_VALUE (PWM2) == 0)
- {
- pwm2 = 0;
- }
- else
- {
- IO_SET (PWM2_BRK_IO);
- if (PWM_VALUE (PWM2) < 0)
- {
- pwm2 = -PWM_VALUE (PWM2) + PWM_OFFSET;
- }
- else
- {
- dir_d |= _BV (PWM2_DIR);
- pwm2 = PWM_VALUE (PWM2) + PWM_OFFSET;
- }
- }
-# endif /* PWM2 */
-#endif /* PWM1or2 */
- /* Setup registers. */
- /* Here, there could be a problem because OCRx are double buffered, not
- * PORTx! */
- /* Another problem arise if the OCR sampling is done between left and
- * right OCR: the right PWM is one cycle late. */
- /* A solution could be to use interrupts to update PWM or to synchronise
- * general timer with PWM. */
-#if PWM1or2
- dir_d ^= pwm_ocr_dir_reverse_d;
- PORTD = dir_d;
-# ifdef PWM1
- PWM1_OCR = pwm1;
-# endif
-# ifdef PWM2
- PWM2_OCR = pwm2;
-# endif
-#endif /* PWM1or2 */
-}
-
-void
-pwm_ocr_set_reverse (uint8_t reverse)
-{
- pwm_reverse = reverse;
- pwm_ocr_dir_reverse_d = 0;
-#ifdef PWM1
- if (reverse & PWM_REVERSE_BIT (PWM1))
- pwm_ocr_dir_reverse_d |= _BV (PWM1_DIR);
-#endif
-#ifdef PWM2
- if (reverse & PWM_REVERSE_BIT (PWM2))
- pwm_ocr_dir_reverse_d |= _BV (PWM2_DIR);
-#endif
-}
-
diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.h b/digital/mimot/src/dirty/pwm_ocr.avr.h
deleted file mode 100644
index cd013824..00000000
--- a/digital/mimot/src/dirty/pwm_ocr.avr.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef pwm_ocr_avr_h
-#define pwm_ocr_avr_h
-/* pwm_ocr.avr.h */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-void
-pwm_ocr_init (void);
-
-void
-pwm_ocr_update (void);
-
-void
-pwm_ocr_set_reverse (uint8_t reverse);
-
-#endif /* pwm_ocr_avr_h */
diff --git a/digital/mimot/src/dirty/state.c b/digital/mimot/src/dirty/seq.c
index c6c90409..0d264917 100644
--- a/digital/mimot/src/dirty/state.c
+++ b/digital/mimot/src/dirty/seq.c
@@ -1,7 +1,7 @@
-/* state.c */
+/* seq.c */
/* asserv - Position & speed motor control on AVR. {{{
*
- * Copyright (C) 2008 Nicolas Schodet
+ * Copyright (C) 2011 Nicolas Schodet
*
* APBTeam:
* Web: http://apbteam.org/
@@ -23,7 +23,7 @@
*
* }}} */
#include "common.h"
-#include "state.h"
+#include "seq.h"
-struct state_t state_aux[AC_ASSERV_AUX_NB];
+seq_t seq_aux[AC_ASSERV_AUX_NB];
diff --git a/digital/mimot/src/dirty/state.h b/digital/mimot/src/dirty/seq.h
index 4916e691..625cc040 100644
--- a/digital/mimot/src/dirty/state.h
+++ b/digital/mimot/src/dirty/seq.h
@@ -1,9 +1,9 @@
-#ifndef state_h
-#define state_h
-/* state.h */
+#ifndef seq_h
+#define seq_h
+/* seq.h */
/* asserv - Position & speed motor control on AVR. {{{
*
- * Copyright (C) 2008 Nicolas Schodet
+ * Copyright (C) 2011 Nicolas Schodet
*
* APBTeam:
* Web: http://apbteam.org/
@@ -24,101 +24,69 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "modules/motor/control_state/control_state.h"
/** There is two mechanism to acknowledge long lived command completion.
*
* The first one is dedicated to unreliable and full duplex channels like the
- * serial port. It is based on sequence numbers.
+ * serial port. It is based on sequence numbers. This file implements this
+ * system until it can be integrated in proto module.
*
* The second one is simpler, but can only be used with reliable and half
* duplex channels like the TWI bus. It just use a flag to remember command
- * completion witch is reseted when a new command arrives. */
+ * completion witch is reseted when a new command arrives. This one is
+ * directly implemented in control system. */
-/** Control mode. */
-enum state_mode_t
+/** Current sequence state. */
+struct seq_t
{
- /** Simple PWM setup mode. */
- MODE_PWM,
- /** Position control mode. */
- MODE_POS,
- /** Speed control mode. */
- MODE_SPEED,
- /** Trajectory control mode. */
- MODE_TRAJ,
-};
-
-/** Current motor state. */
-struct state_t
-{
- /** Current motor control mode. */
- uint8_t mode;
- /** Control mode variant.
- * Used for main motors:
- * - bit 0: disable theta position control.
- * - bit 1: disable alpha position control.
- * - bit 2: disable blocking detection. */
- uint8_t variant;
/** Sequence number of the currently processed command, should be between
* 1 and 127. When a command is received on the serial port it is ignored
* if its sequence number is equal to the current sequence number. In
* this case a duplicated message is inferred. */
- uint8_t sequence;
+ uint8_t cur;
/** Sequence number of the most recently finished command. When a command
* is finished, the current sequence number is copied to this variable. */
- uint8_t sequence_finish;
+ uint8_t finish;
/** Sequence number of the most recently acknowledged command. Until this
* is not equal to the last finished command sequence number, a message is
* generated on the serial line. */
- uint8_t sequence_ack;
- /** Simpler flag based mechanism, indicates if the last received command
- * is finished. */
- uint8_t finished;
- /** Wether the motor is blocked. */
- uint8_t blocked;
+ uint8_t ack;
};
+typedef struct seq_t seq_t;
/** Auxiliary motor states. */
-extern struct state_t state_aux[AC_ASSERV_AUX_NB];
-
-/** Start a new command execution. */
-static inline void
-state_start (struct state_t *motor, uint8_t mode, uint8_t sequence)
-{
- motor->mode = mode;
- motor->variant = 0;
- motor->sequence = sequence;
- motor->finished = 0;
- motor->blocked = 0;
-}
+extern seq_t seq_aux[AC_ASSERV_AUX_NB];
-/** Signal the current command completion. */
-static inline void
-state_finish (struct state_t *motor)
+/** Start a new command execution, return non zero if this is a new
+ * sequence. */
+static inline uint8_t
+seq_start (seq_t *seq, uint8_t new)
{
- motor->sequence_finish = motor->sequence;
- motor->finished = 1;
+ if (new == seq->cur)
+ return 0;
+ else
+ {
+ seq->cur = new;
+ return 1;
+ }
}
-/** Signal the current command is blocked, disable motor control until a new
- * command is given. */
+/** Acknowledge a command completion and blocked state. */
static inline void
-state_blocked (struct state_t *motor)
+seq_acknowledge (seq_t *seq, uint8_t ack)
{
- motor->sequence_finish = motor->sequence | 0x80;
- motor->blocked = 1;
- motor->mode = MODE_PWM;
+ seq->ack = ack;
}
-/** Acknowledge a command completion and blocked state. */
+/** Update sequence state according to control state. */
static inline void
-state_acknowledge (struct state_t *motor, uint8_t sequence)
+seq_update (seq_t *seq, control_state_t *state)
{
- motor->sequence_ack = sequence;
- if (sequence == motor->sequence_finish)
- {
- motor->finished = 0;
- motor->blocked = 0;
- }
+ if (control_state_is_blocked (state))
+ seq->finish = seq->cur | 0x80;
+ else if (control_state_is_finished (state))
+ seq->finish = seq->cur;
}
-#endif /* state_h */
+#endif /* seq_h */
diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c
index d27ffe5f..c4ac5026 100644
--- a/digital/mimot/src/dirty/simu.host.c
+++ b/digital/mimot/src/dirty/simu.host.c
@@ -38,38 +38,24 @@
#include <stdio.h>
#include <string.h>
-#include "pwm.h"
+#include "cs.h"
#include "aux.h"
#include "contacts.h"
-#include "motor_model.host.h"
#include "models.host.h"
/** Simulate some AVR regs. */
uint8_t PORTB, PORTC, PORTD, PINC;
-/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
-/** Counter differences since last update.
- * Maximum of 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
-
-/** PWM control states. */
-struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
- PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
-};
-/** PWM reverse directions. */
-uint8_t pwm_reverse;
-
/* Robot model. */
const struct robot_t *simu_robot;
/** Motor models. */
-struct motor_t simu_aux_model[AC_ASSERV_AUX_NB];
+motor_model_t simu_aux_model[AC_ASSERV_AUX_NB];
-/** Full counter values. */
-uint32_t simu_counter_aux[AC_ASSERV_AUX_NB];
+/** Full encoder values. */
+uint32_t simu_encoder_aux[AC_ASSERV_AUX_NB];
/** Use mex. */
int simu_mex;
@@ -169,8 +155,8 @@ simu_step (void)
double old_aux_th[AC_ASSERV_AUX_NB];
/* Convert pwm value into voltage. */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- simu_aux_model[i].u = simu_aux_model[i].m.u_max
- * ((double) pwm_aux[i].cur / (PWM_MAX + 1));
+ simu_aux_model[i].u = simu_robot->u_max
+ * ((double) output_aux[i].cur / (OUTPUT_MAX + 1));
/* Make one step. */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
{
@@ -178,22 +164,22 @@ simu_step (void)
if (simu_robot->aux_motor[i])
motor_model_step (&simu_aux_model[i]);
}
- /* Update auxiliary counter. */
+ /* Update auxiliary encoder. */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
{
if (simu_robot->aux_motor[i])
{
- uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI)
+ uint32_t encoder_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;
+ encoder_aux[i].diff = encoder_aux_new - simu_encoder_aux[i];
+ encoder_aux[i].cur += encoder_aux[i].diff;
+ simu_encoder_aux[i] = encoder_aux_new;
}
else
{
- counter_aux_diff[i] = 0;
- counter_aux[i] = 0;
- simu_counter_aux[i] = 0;
+ encoder_aux[i].diff = 0;
+ encoder_aux[i].cur = 0;
+ simu_encoder_aux[i] = 0;
}
}
/* Update sensors. */
@@ -264,30 +250,6 @@ timer_read (void)
return 0;
}
-/** Initialize the counters. */
-void
-counter_init (void)
-{
-}
-
-/** Update overall counter values and compute diffs. */
-void
-counter_update (void)
-{
-}
-
-/** Initialise PWM generator. */
-void
-pwm_init (void)
-{
-}
-
-/** Update the hardware PWM values. */
-void
-pwm_update (void)
-{
-}
-
void
eeprom_read_params (void)
{
@@ -303,9 +265,3 @@ eeprom_clear_params (void)
{
}
-void
-pwm_set_reverse (uint8_t reverse)
-{
- pwm_reverse = reverse;
-}
-
diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c
deleted file mode 100644
index 80ca3d6b..00000000
--- a/digital/mimot/src/dirty/speed.c
+++ /dev/null
@@ -1,138 +0,0 @@
-/* speed.c - Speed control. */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2005 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "speed.h"
-
-#include "modules/utils/utils.h"
-#include "modules/math/fixed/fixed.h"
-
-#include "pos.h"
-#include "state.h"
-
-/**
- * This file is responsible for speed control. It changes the current shafts
- * positions using ramps. It can be controlled by a wanted speed or wanted
- * shaft position.
- */
-
-/** Auxiliaries speed control states. */
-struct speed_t speed_aux[AC_ASSERV_AUX_NB];
-
-/** Initialise speed control states. */
-void
-speed_init (void)
-{
- uint8_t i;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- speed_aux[i].pos = &pos_aux[i];
-}
-
-/** Update current speed according to a speed consign. */
-static void
-speed_update_by_speed (struct speed_t *speed)
-{
- /* Update current speed (be careful of overflow!). */
- if (speed->cons > speed->cur)
- {
- if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc)
- speed->cur = speed->cons;
- else
- speed->cur += speed->acc;
- }
- else
- {
- if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc)
- speed->cur = speed->cons;
- else
- speed->cur -= speed->acc;
- }
-}
-
-/** Compute maximum allowed speed according to: distance left, maximum speed,
- * current speed and acceleration. */
-static int16_t
-speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
-{
- int16_t s;
- /* Compute maximum speed in order to be able to brake in time.
- * The "+ 0xff" is to ceil result.
- * s = sqrt (2 * a * d) */
- s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8);
- /* Apply consign. */
- s = UTILS_MIN (max, s);
- /* Apply sign. */
- if (d < 0)
- s = -s;
- /* Convert to f8.8 and check acceleration. */
- s = s << 8;
- UTILS_BOUND (s, cur - acc, cur + acc);
- return s;
-}
-
-/** Update current speed according to a position consign. */
-static void
-speed_update_by_position (struct speed_t *speed)
-{
- int32_t diff = speed->pos_cons - speed->pos->cons;
- speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc,
- speed->max);
-}
-
-/** Update shaft position consign according to its consign type. */
-static void
-speed_update_by (struct speed_t *speed)
-{
- if (speed->use_pos)
- speed_update_by_position (speed);
- else
- speed_update_by_speed (speed);
- /* Update shaft position. */
- speed->pos->cons += speed->cur >> 8;
-}
-
-/** Update shaft position consign for one motor system. */
-static void
-speed_update_single (struct state_t *state, struct speed_t *speed)
-{
- if (state->mode >= MODE_SPEED)
- {
- /* Update speed. */
- speed_update_by (speed);
- /* Check for completion. */
- if (state->mode == MODE_SPEED
- && speed->use_pos && speed->cur == 0)
- state_finish (state);
- }
-}
-
-/** Update shaft position consign according to consign. */
-void
-speed_update (void)
-{
- uint8_t i;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- speed_update_single (&state_aux[i], &speed_aux[i]);
-}
-
diff --git a/digital/mimot/src/dirty/speed.h b/digital/mimot/src/dirty/speed.h
deleted file mode 100644
index 10a0da2b..00000000
--- a/digital/mimot/src/dirty/speed.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#ifndef speed_h
-#define speed_h
-/* speed.h */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/** Speed control state. */
-struct speed_t
-{
- /** Controlled position. */
- struct pos_t *pos;
- /** Current speed, f8.8. */
- int16_t cur;
- /** Consign speed, f8.8. */
- int16_t cons;
- /** Maximum speed for position consign, u8. */
- int8_t max;
- /** Slow speed for position consign, u8. */
- int8_t slow;
- /** Acceleration, f8.8. */
- int16_t acc;
- /** Consign position. */
- uint32_t pos_cons;
- /** Whether to use the consign position (1) or not (0). */
- uint8_t use_pos;
-};
-
-extern struct speed_t speed_aux[AC_ASSERV_AUX_NB];
-
-void
-speed_init (void);
-
-void
-speed_update (void);
-
-#endif /* speed_h */
diff --git a/digital/mimot/src/dirty/timer.avr.c b/digital/mimot/src/dirty/timer.avr.c
index 3f956137..561fc0ac 100644
--- a/digital/mimot/src/dirty/timer.avr.c
+++ b/digital/mimot/src/dirty/timer.avr.c
@@ -25,10 +25,9 @@
#include "common.h"
#include "modules/utils/utils.h"
+#include "modules/motor/encoder/encoder.h"
#include "io.h"
-#include "counter.h"
-
/** Top timer value. */
#define TIMER_TOP 255
/** Number of steps during wait. */
@@ -58,7 +57,7 @@ timer_wait (void)
{
while (TCNT0 < i * TIMER_STEP)
;
- counter_update_step ();
+ encoder_update_step ();
}
/* Wait overflow. */
while (!(TIFR & _BV (TOV0)))
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 2f935fda..e5ce353d 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -31,11 +31,6 @@
#include "modules/twi/twi.h"
#include "io.h"
-#include "state.h"
-
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
#include "aux.h"
#ifdef HOST
@@ -76,10 +71,10 @@ twi_proto_update (void)
u8 status_with_crc[8];
u8 *status = &status_with_crc[1];
status[0] = 0
- | (state_aux[1].blocked << 3)
- | (state_aux[1].finished << 2)
- | (state_aux[0].blocked << 1)
- | (state_aux[0].finished << 0);
+ | (control_state_is_blocked (&cs_aux[1].state) ? (1 << 3) : 0)
+ | (control_state_is_finished (&cs_aux[1].state) ? (1 << 2) : 0)
+ | (control_state_is_blocked (&cs_aux[0].state) ? (1 << 1) : 0)
+ | (control_state_is_finished (&cs_aux[0].state) ? (1 << 0) : 0);
status[1] = PINC;
status[2] = twi_proto.seq;
status[3] = v16_to_v8 (aux[0].pos, 1);
@@ -119,15 +114,15 @@ twi_proto_callback (u8 *buf, u8 size)
/* 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);
+ cs_aux[0].speed.max = buf[4];
+ aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]));
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);
+ cs_aux[1].speed.max = buf[4];
+ aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]));
break;
case c ('B', 5):
/* Find the zero position.
@@ -139,10 +134,10 @@ twi_proto_callback (u8 *buf, u8 size)
{
if (buf[4])
aux_traj_find_zero_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]), 0);
+ v8_to_v16 (buf[5], buf[6]));
else
aux_traj_find_limit_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]), 0);
+ v8_to_v16 (buf[5], buf[6]));
}
else
buf[0] = 0;
@@ -154,7 +149,7 @@ twi_proto_callback (u8 *buf, u8 size)
* - 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);
+ v8_to_v16 (buf[4], buf[5]));
else
buf[0] = 0;
break;
@@ -163,9 +158,8 @@ twi_proto_callback (u8 *buf, u8 size)
* - 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);
+ output_set (&output_aux[buf[2]], 0);
+ control_state_set_mode (&cs_aux[buf[2]].state, CS_MODE_NONE, 0);
}
else
buf[0] = 0;
diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py
index 49b16cc1..87e00642 100644
--- a/digital/mimot/tools/mimot/init.py
+++ b/digital/mimot/tools/mimot/init.py
@@ -7,7 +7,6 @@ target_marcel = dict (
a1a = 16, a1sm = 0x60, a1ss = 0x10,
a1be = 256, a1bs = 0x18, a1bc = 5,
E = 0x3ff, D = 0x1ff,
- w = 0x03,
)
target_robospierre = dict (
a0kp = 8, a0kd = 1,
@@ -17,7 +16,6 @@ target_robospierre = dict (
a1a = 0.5, a1sm = 0x30, a1ss = 0x08,
a1be = 64, a1bs = 0x08, a1bc = 5,
E = 0x3ff, D = 0x1ff,
- w = 0x03,
)
target = {
'marcel': target_marcel,