summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/Makefile18
-rw-r--r--digital/asserv/src/asserv/aux.c120
-rw-r--r--digital/asserv/src/asserv/aux.h20
-rw-r--r--digital/asserv/src/asserv/avrconfig.h33
-rw-r--r--digital/asserv/src/asserv/counter_ext.avr.c229
-rw-r--r--digital/asserv/src/asserv/counter_tcc.avr.c169
-rw-r--r--digital/asserv/src/asserv/cs.c84
-rw-r--r--digital/asserv/src/asserv/cs.h (renamed from digital/asserv/src/asserv/counter.h)29
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c194
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c353
-rw-r--r--digital/asserv/src/asserv/models.host.c233
-rw-r--r--digital/asserv/src/asserv/models.host.h16
-rw-r--r--digital/asserv/src/asserv/motor_model.host.c100
-rw-r--r--digital/asserv/src/asserv/motor_model.host.h68
-rw-r--r--digital/asserv/src/asserv/pos.c208
-rw-r--r--digital/asserv/src/asserv/pos.h62
-rw-r--r--digital/asserv/src/asserv/postrack.c8
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c62
-rw-r--r--digital/asserv/src/asserv/pwm.h108
-rw-r--r--digital/asserv/src/asserv/pwm_config.h52
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.c126
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.h34
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.c221
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.h37
-rw-r--r--digital/asserv/src/asserv/seq.c (renamed from digital/asserv/src/asserv/state.c)10
-rw-r--r--digital/asserv/src/asserv/seq.h95
-rw-r--r--digital/asserv/src/asserv/simu.host.c144
-rw-r--r--digital/asserv/src/asserv/speed.c165
-rw-r--r--digital/asserv/src/asserv/speed.h58
-rw-r--r--digital/asserv/src/asserv/state.h127
-rw-r--r--digital/asserv/src/asserv/test_counter.c133
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c92
-rw-r--r--digital/asserv/src/asserv/timer.avr.c5
-rw-r--r--digital/asserv/src/asserv/traj.c244
-rw-r--r--digital/asserv/src/asserv/traj.h20
-rw-r--r--digital/asserv/src/asserv/twi_proto.c89
37 files changed, 888 insertions, 2880 deletions
diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile
index 4a61d08b..d19d9909 100644
--- a/digital/asserv/src/asserv/Makefile
+++ b/digital/asserv/src/asserv/Makefile
@@ -1,15 +1,13 @@
BASE = ../../../avr
PROGS = asserv
-AVR_PROGS = test_counter
-HOST_PROGS = test_motor_model
-asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pwm_mp.avr.c \
- pwm_ocr.avr.c pos.c speed.c postrack.c traj.c aux.c \
- twi_proto.c eeprom.avr.c state.c \
- simu.host.c motor_model.host.c models.host.c
-test_counter_SOURCES = test_counter.c timer.avr.c
-test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c
-MODULES = proto uart utils math/fixed twi spi
-test_motor_model_MODULES =
+asserv_SOURCES = main.c timer.avr.c \
+ postrack.c traj.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 spi \
+ motor/encoder motor/encoder/ext motor/output motor/output/pwm_mp \
+ motor/control_system motor/speed_control motor/pos_control \
+ motor/blocking_detection motor/motor_model
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
AVR_MCU = atmega128
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index c300e3f3..e4cce306 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/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. */
@@ -59,23 +56,17 @@ enum
AUX_TRAJ_FIND_ZERO,
/* Find zero by forcing into limit. */
AUX_TRAJ_FIND_LIMIT,
- /* 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;
@@ -88,7 +79,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. */
@@ -98,43 +110,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);
}
/** Find zero mode. */
@@ -151,15 +159,15 @@ aux_traj_find_zero (struct aux_t *aux)
case AUX_TRAJ_FIND_ZERO_NOT_REVERSE:
if (zero)
{
- aux->speed->cons = -aux->speed->cons;
+ aux->cs->speed.cons = -aux->cs->speed.cons;
aux->traj_mode = AUX_TRAJ_FIND_ZERO;
}
break;
case AUX_TRAJ_FIND_ZERO:
if (!zero)
{
- aux->speed->cons = 0;
- state_finish (aux->state);
+ speed_control_set_speed (&aux->cs->speed, 0);
+ control_state_finished (&aux->cs->state);
aux->pos = 0;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -169,22 +177,20 @@ 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, uint8_t seq)
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed)
{
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);
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Start find zero reverse mode. */
void
-aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT_REVERSE;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Find limit mode. */
@@ -192,15 +198,11 @@ void
aux_traj_find_limit (struct aux_t *aux)
{
/* If blocking, limit is found. */
- 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;
- aux->state->variant = 0;
- pwm_set (aux->pwm, 0);
+ control_state_set_mode (&aux->cs->state, CS_MODE_NONE, 0);
+ control_state_finished (&aux->cs->state);
+ output_set (aux->cs->output, 0);
aux->pos = 0;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -208,23 +210,25 @@ 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, uint8_t seq)
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed)
{
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);
}
/** Update trajectories for one motor. */
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/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 167f880e..98a407b3 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/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. */
@@ -59,16 +56,19 @@ 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_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos);
void
-aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed);
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed);
+
+void
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed);
void
aux_traj_update (void);
diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h
index a07fa21d..7779e974 100644
--- a/digital/asserv/src/asserv/avrconfig.h
+++ b/digital/asserv/src/asserv/avrconfig.h
@@ -121,4 +121,37 @@
/** Same thing for an optionnal second SPI driver. */
#define AC_SPI1_DRIVER NONE
+/* 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 4
+/** Use external memory hardware interface. */
+#define AC_ENCODER_EXT_USE_XMEM 1
+/** Reverse flag for each encoder (1 to reverse direction). */
+#define AC_ENCODER_EXT_REVERSE 1, 0, 1, 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 1
+
+/* motor/output - Output module. */
+/** Use Output Compare PWM output. */
+#define AC_OUTPUT_USE_PWM_OCR 0
+/** Use Motor Power PWM output. */
+#define AC_OUTPUT_USE_PWM_MP 1
+/** Define module and module index for each output. */
+#define AC_OUTPUT_LIST (pwm_mp, 0), (pwm_mp, 1), (pwm_mp, 2), (pwm_mp, 3)
+
+/* motor/output/pwm_mp - Motor Power board PWM output module. */
+/** Number of outputs, there is two outputs per board. */
+#define AC_OUTPUT_PWM_MP_NB 4
+/** Slave select for first Motor Power board.
+ * WARNING: this must match hardware SS pin if using hardware SPI! */
+#define AC_OUTPUT_PWM_MP_SPI_SS_IO_0 B, 0
+/** Slave select for next Motor Power boards. */
+#define AC_OUTPUT_PWM_MP_SPI_SS_IO_1 E, 4
+
#endif /* avrconfig_h */
diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c
deleted file mode 100644
index 29c51b87..00000000
--- a/digital/asserv/src/asserv/counter_ext.avr.c
+++ /dev/null
@@ -1,229 +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.
- *
- * There is additionnal support for error correction on the right counter.
- */
-
-/** Define the left counter address. */
-#define COUNTER_LEFT 0
-/** Define the right counter address. */
-#define COUNTER_RIGHT 1
-/** Define the first auxiliary counter address. */
-#define COUNTER_AUX0 2
-/** Define the second auxiliary counter address. */
-#define COUNTER_AUX1 3
-
-/** Define to 1 to reverse the left counter. */
-#define COUNTER_LEFT_REVERSE 1
-/** Define to 1 to reverse the right counter. */
-#define COUNTER_RIGHT_REVERSE 0
-/** Define to 1 to reverse the first auxiliary counter. */
-#define COUNTER_AUX0_REVERSE 1
-/** Define to 1 to reverse the second auxiliary counter. */
-#define COUNTER_AUX1_REVERSE 1
-
-/** Left counter shift. */
-#define COUNTER_LEFT_SHIFT 0
-/** Right counter shift. */
-#define COUNTER_RIGHT_SHIFT 0
-/** First auxiliary counter shift. */
-#define COUNTER_AUX0_SHIFT 0
-/** Second auxiliary counter shift. */
-#define COUNTER_AUX1_SHIFT 0
-
-/** Define to 1 to use the AVR External Memory system, or 0 to use hand made
- * signals. */
-#define COUNTER_USE_XMEM 1
-
-/** Last values. */
-static uint16_t counter_left_old, counter_right_old,
- counter_aux_old[AC_ASSERV_AUX_NB];
-/** New values, being updated by step update. */
-static int16_t counter_left_new_step, counter_right_new_step,
- counter_aux_new_step[AC_ASSERV_AUX_NB];
-/** Last raw step values */
-static uint8_t counter_left_old_step, counter_right_old_step,
- counter_aux_old_step[AC_ASSERV_AUX_NB];
-/** Overall counter values. */
-uint16_t counter_left, counter_right,
- counter_aux[AC_ASSERV_AUX_NB];
-/** 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_left_diff, counter_right_diff,
- counter_aux_diff[AC_ASSERV_AUX_NB];
-
-#if !COUNTER_USE_XMEM
-# define COUNTER_ALE _BV (2)
-# define COUNTER_RD _BV (1)
-# define COUNTER_WR _BV (0)
-#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 = n;
- PORTG &= ~COUNTER_ALE;
- PORTA = 0;
- DDRA = 0;
- PORTG &= ~COUNTER_RD;
- utils_nop ();
- utils_nop ();
- v = PINA;
- PORTG |= COUNTER_RD;
- PORTG |= COUNTER_ALE;
- DDRA = 0xff;
- 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
- PORTG |= COUNTER_ALE | COUNTER_RD | COUNTER_WR;
- DDRG |= COUNTER_ALE | COUNTER_RD | COUNTER_WR;
- PORTA = 0;
- DDRA = 0xff;
-#endif
- /* Begin with safe values. */
- counter_left_old_step = counter_read (COUNTER_LEFT);
- counter_right_old_step = counter_read (COUNTER_RIGHT);
- 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 left, right, aux0, aux1;
- int8_t diff;
- /* Sample counters. */
- left = counter_read (COUNTER_LEFT);
- right = counter_read (COUNTER_RIGHT);
- aux0 = counter_read (COUNTER_AUX0);
- aux1 = counter_read (COUNTER_AUX1);
- /* Update step counters. */
- 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;
- 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 ();
- /* 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_left_diff = (int16_t) (counter_left_old - left);
-#endif
- 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_right_diff = (int16_t) (counter_right_old - right);
-#endif
- 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;
- /* 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/asserv/src/asserv/counter_tcc.avr.c b/digital/asserv/src/asserv/counter_tcc.avr.c
deleted file mode 100644
index 419e764f..00000000
--- a/digital/asserv/src/asserv/counter_tcc.avr.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/* counter_tcc.avr.c - Internal counter support. */
-/* 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 "counter.h"
-
-#include "modules/utils/utils.h"
-#include "io.h"
-
-#include "misc.h"
-
-/**
- * This file add support for an AVR internal counter. This uses one TCC
- * hardware per counter, and an external circuitry to compute rotation
- * direction.
- */
-
-/** 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
-
-/** Forward and reverse counter values. */
-static uint8_t counter_left_frw, counter_left_rev,
- counter_right_frw, counter_right_rev;
-/** Last TCNT values. */
-static uint8_t counter_left_old, counter_right_old;
-/** Overall counter values. */
-uint16_t counter_left, counter_right;
-/** Counter differences since last update.
- * Maximum of 9 significant bits, sign included. */
-int16_t counter_left_diff, counter_right_diff;
-
-/** Initialize the counters. */
-void
-counter_init (void)
-{
- /* Left counter. */
- /* External, rising edge. */
- TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
- 0, 0, 0, 0, 0, 1, 1, 1);
- /* Right counter. */
- /* Normal counter. */
- TCCR3A = 0;
- /* External, rising edge. */
- TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
- 0, 0, 0, 0, 0, 1, 1, 1);
- /* Begin with safe values. */
- counter_left_frw = 0;
- counter_left_rev = 0;
- counter_left_old = TCNT2;
- counter_right_frw = 0;
- counter_right_rev = 0;
- counter_right_old = TCNT3L;
- /* Interrupts for direction. */
- EICRB = 0x05;
- EIFR = _BV (4) | _BV (5);
- EIMSK = _BV (4) | _BV (5);
-}
-
-/** Left direction change. */
-SIGNAL (SIG_INTERRUPT4)
-{
- uint8_t c;
- c = TCNT2;
- if (PINE & _BV (4))
- {
- counter_left_rev += c - counter_left_old;
- LED1 (!COUNTER_LEFT_REVERSE);
- }
- else
- {
- counter_left_frw += c - counter_left_old;
- LED1 (COUNTER_LEFT_REVERSE);
- }
- counter_left_old = c;
-}
-
-/** Right direction change. */
-SIGNAL (SIG_INTERRUPT5)
-{
- uint8_t c;
- c = TCNT3L;
- if (PINE & _BV (5))
- {
- counter_right_rev += c - counter_right_old;
- LED2 (!COUNTER_RIGHT_REVERSE);
- }
- else
- {
- counter_right_frw += c - counter_right_old;
- LED2 (COUNTER_RIGHT_REVERSE);
- }
- counter_right_old = c;
-}
-
-void
-counter_update_step (void)
-{
- /* No support for update steps. */
-}
-
-/** Update overall counter values and compute diffs. */
-void
-counter_update (void)
-{
- uint8_t c;
- /* Disable ints. */
- EIMSK &= ~(_BV (4) | _BV (5));
- /* Read left counter. */
- c = TCNT2;
- if (PINE & _BV (4))
- counter_left_frw += c - counter_left_old;
- else
- counter_left_rev += c - counter_left_old;
- counter_left_old = c;
- /* Read right counter. */
- c = TCNT3L;
- if (PINE & _BV (5))
- counter_right_frw += c - counter_right_old;
- else
- counter_right_rev += c - counter_right_old;
- counter_right_old = c;
- /* Update counter values and diffs. */
-#if COUNTER_LEFT_REVERSE == 0
- counter_left_diff = counter_left_frw;
- counter_left_diff -= counter_left_rev;
-#else
- counter_left_diff = counter_left_rev;
- counter_left_diff -= counter_left_frw;
-#endif
- counter_left_frw = 0;
- counter_left_rev = 0;
- counter_left += counter_left_diff;
-#if COUNTER_RIGHT_REVERSE == 0
- counter_right_diff = counter_right_frw;
- counter_right_diff -= counter_right_rev;
-#else
- counter_right_diff = counter_right_rev;
- counter_right_diff -= counter_right_frw;
-#endif
- counter_right_frw = 0;
- counter_right_rev = 0;
- counter_right += counter_right_diff;
- /* Enable ints. */
- EIMSK |= _BV (4) | _BV (5);
-}
-
diff --git a/digital/asserv/src/asserv/cs.c b/digital/asserv/src/asserv/cs.c
new file mode 100644
index 00000000..74f44591
--- /dev/null
+++ b/digital/asserv/src/asserv/cs.c
@@ -0,0 +1,84 @@
+/* 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_left, encoder_right, encoder_aux[AC_ASSERV_AUX_NB];
+encoder_corrector_t encoder_right_corrector;
+output_t output_left, output_right, output_aux[AC_ASSERV_AUX_NB];
+
+control_system_polar_t cs_main;
+control_system_single_t cs_aux[AC_ASSERV_AUX_NB];
+
+void
+cs_init (void)
+{
+ uint8_t i;
+ /* Initialise encoders. */
+ encoder_init (0, &encoder_left);
+ encoder_init (1, &encoder_right);
+ encoder_init (2, &encoder_aux[0]);
+ encoder_init (3, &encoder_aux[1]);
+ /* Initialise outputs. */
+ output_left.max = OUTPUT_MAX;
+ output_init (0, &output_left);
+ output_right.max = OUTPUT_MAX;
+ output_init (1, &output_right);
+ output_aux[0].max = OUTPUT_MAX;
+ output_init (2, &output_aux[0]);
+ output_aux[1].max = OUTPUT_MAX;
+ output_init (3, &output_aux[1]);
+ /* Initialise control system. */
+ control_system_polar_init (&cs_main);
+ cs_main.encoder_left = &encoder_left;
+ cs_main.encoder_right = &encoder_right;
+ cs_main.output_left = &output_left;
+ cs_main.output_right = &output_right;
+ 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;
+ control_system_polar_update_prepare (&cs_main);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ control_system_single_update_prepare (&cs_aux[i]);
+}
+
+void
+cs_update (void)
+{
+ uint8_t i;
+ control_system_polar_update (&cs_main);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ control_system_single_update (&cs_aux[i]);
+}
+
diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/cs.h
index 198660e4..5a1737ec 100644
--- a/digital/asserv/src/asserv/counter.h
+++ b/digital/asserv/src/asserv/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,20 +24,23 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "modules/motor/control_system/control_system.h"
+#include "modules/motor/encoder/encoder_corrector.h"
-extern uint16_t counter_left, counter_right,
- counter_aux[AC_ASSERV_AUX_NB];
-extern uint32_t counter_right_correction;
-extern int16_t counter_left_diff, counter_right_diff,
- counter_aux_diff[AC_ASSERV_AUX_NB];
+extern encoder_t encoder_left, encoder_right, encoder_aux[AC_ASSERV_AUX_NB];
+extern encoder_corrector_t encoder_right_corrector;
+extern output_t output_left, output_right, output_aux[AC_ASSERV_AUX_NB];
+
+extern control_system_polar_t cs_main;
+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/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index ddc10f3c..004a2ecb 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/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"
#include "postrack.h"
#include "traj.h"
@@ -71,49 +68,62 @@ eeprom_read_params (void)
uint16_t *p16;
if (eeprom_read_byte (p8++) != EEPROM_KEY)
return;
- speed_theta.max = eeprom_read_byte (p8++);
- speed_alpha.max = eeprom_read_byte (p8++);
- speed_aux[0].max = eeprom_read_byte (p8++);
- speed_aux[1].max = eeprom_read_byte (p8++);
- speed_theta.slow = eeprom_read_byte (p8++);
- speed_alpha.slow = 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++));
- counter_right_correction = eeprom_read_32 (p8); p8 += 4;
+ cs_main.speed_theta.max = eeprom_read_byte (p8++);
+ cs_main.speed_alpha.max = eeprom_read_byte (p8++);
+ cs_aux[0].speed.max = eeprom_read_byte (p8++);
+ cs_aux[1].speed.max = eeprom_read_byte (p8++);
+ cs_main.speed_theta.slow = eeprom_read_byte (p8++);
+ cs_main.speed_alpha.slow = 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_left, eeprom_read_byte (p8++));
+ output_set_reverse (&output_right, eeprom_read_byte (p8++));
+ output_set_reverse (&output_aux[0], eeprom_read_byte (p8++));
+ output_set_reverse (&output_aux[1], eeprom_read_byte (p8++));
+ encoder_corrector_set_correction (&encoder_right_corrector,
+ eeprom_read_32 (p8)); p8 += 4;
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
- speed_theta.acc = eeprom_read_word (p16++);
- speed_alpha.acc = eeprom_read_word (p16++);
- speed_aux[0].acc = eeprom_read_word (p16++);
- speed_aux[1].acc = eeprom_read_word (p16++);
- pos_theta.kp = eeprom_read_word (p16++);
- pos_alpha.kp = eeprom_read_word (p16++);
- pos_aux[0].kp = eeprom_read_word (p16++);
- pos_aux[1].kp = eeprom_read_word (p16++);
- pos_theta.ki = eeprom_read_word (p16++);
- pos_alpha.ki = eeprom_read_word (p16++);
- pos_aux[0].ki = eeprom_read_word (p16++);
- pos_aux[1].ki = eeprom_read_word (p16++);
- pos_theta.kd = eeprom_read_word (p16++);
- pos_alpha.kd = eeprom_read_word (p16++);
- pos_aux[0].kd = eeprom_read_word (p16++);
- pos_aux[1].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_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_main.speed_theta.acc = eeprom_read_word (p16++);
+ cs_main.speed_alpha.acc = eeprom_read_word (p16++);
+ cs_aux[0].speed.acc = eeprom_read_word (p16++);
+ cs_aux[1].speed.acc = eeprom_read_word (p16++);
+ cs_main.pos_theta.kp = eeprom_read_word (p16++);
+ cs_main.pos_alpha.kp = eeprom_read_word (p16++);
+ cs_aux[0].pos.kp = eeprom_read_word (p16++);
+ cs_aux[1].pos.kp = eeprom_read_word (p16++);
+ cs_main.pos_theta.ki = eeprom_read_word (p16++);
+ cs_main.pos_alpha.ki = eeprom_read_word (p16++);
+ cs_aux[0].pos.ki = eeprom_read_word (p16++);
+ cs_aux[1].pos.ki = eeprom_read_word (p16++);
+ cs_main.pos_theta.kd = eeprom_read_word (p16++);
+ cs_main.pos_alpha.kd = eeprom_read_word (p16++);
+ cs_aux[0].pos.kd = eeprom_read_word (p16++);
+ cs_aux[1].pos.kd = eeprom_read_word (p16++);
+ cs_main.blocking_detection_theta.error_limit = eeprom_read_word (p16++);
+ cs_main.blocking_detection_theta.speed_limit = eeprom_read_word (p16++);
+ cs_main.blocking_detection_theta.counter_limit = eeprom_read_word (p16++);
+ cs_main.blocking_detection_alpha.error_limit = eeprom_read_word (p16++);
+ cs_main.blocking_detection_alpha.speed_limit = eeprom_read_word (p16++);
+ cs_main.blocking_detection_alpha.counter_limit = 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_main.pos_theta.e_sat = eeprom_read_word (p16++);
+ cs_main.pos_theta.i_sat = eeprom_read_word (p16++);
+ cs_main.pos_theta.d_sat = eeprom_read_word (p16++);
+ cs_main.pos_alpha.e_sat = eeprom_read_word (p16++);
+ cs_main.pos_alpha.i_sat = eeprom_read_word (p16++);
+ cs_main.pos_alpha.d_sat = 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++);
traj_eps = eeprom_read_word (p16++);
traj_aeps = eeprom_read_word (p16++);
traj_set_angle_limit (eeprom_read_word (p16++));
@@ -126,49 +136,61 @@ 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_theta.max);
- eeprom_write_byte (p8++, speed_alpha.max);
- eeprom_write_byte (p8++, speed_aux[0].max);
- eeprom_write_byte (p8++, speed_aux[1].max);
- eeprom_write_byte (p8++, speed_theta.slow);
- eeprom_write_byte (p8++, speed_alpha.slow);
- eeprom_write_byte (p8++, speed_aux[0].slow);
- eeprom_write_byte (p8++, speed_aux[1].slow);
- eeprom_write_byte (p8++, pwm_reverse);
- eeprom_write_32 (p8, counter_right_correction); p8 += 4;
+ eeprom_write_byte (p8++, cs_main.speed_theta.max);
+ eeprom_write_byte (p8++, cs_main.speed_alpha.max);
+ eeprom_write_byte (p8++, cs_aux[0].speed.max);
+ eeprom_write_byte (p8++, cs_aux[1].speed.max);
+ eeprom_write_byte (p8++, cs_main.speed_theta.slow);
+ eeprom_write_byte (p8++, cs_main.speed_alpha.slow);
+ eeprom_write_byte (p8++, cs_aux[0].speed.slow);
+ eeprom_write_byte (p8++, cs_aux[1].speed.slow);
+ eeprom_write_byte (p8++, output_left.reverse);
+ eeprom_write_byte (p8++, output_right.reverse);
+ eeprom_write_byte (p8++, output_aux[0].reverse);
+ eeprom_write_byte (p8++, output_aux[1].reverse);
+ eeprom_write_32 (p8, encoder_right_corrector.correction); p8 += 4;
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
- eeprom_write_word (p16++, speed_theta.acc);
- eeprom_write_word (p16++, speed_alpha.acc);
- eeprom_write_word (p16++, speed_aux[0].acc);
- eeprom_write_word (p16++, speed_aux[1].acc);
- eeprom_write_word (p16++, pos_theta.kp);
- eeprom_write_word (p16++, pos_alpha.kp);
- eeprom_write_word (p16++, pos_aux[0].kp);
- eeprom_write_word (p16++, pos_aux[1].kp);
- eeprom_write_word (p16++, pos_theta.ki);
- eeprom_write_word (p16++, pos_alpha.ki);
- eeprom_write_word (p16++, pos_aux[0].ki);
- eeprom_write_word (p16++, pos_aux[1].ki);
- eeprom_write_word (p16++, pos_theta.kd);
- eeprom_write_word (p16++, pos_alpha.kd);
- eeprom_write_word (p16++, pos_aux[0].kd);
- eeprom_write_word (p16++, pos_aux[1].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_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_main.speed_theta.acc);
+ eeprom_write_word (p16++, cs_main.speed_alpha.acc);
+ eeprom_write_word (p16++, cs_aux[0].speed.acc);
+ eeprom_write_word (p16++, cs_aux[1].speed.acc);
+ eeprom_write_word (p16++, cs_main.pos_theta.kp);
+ eeprom_write_word (p16++, cs_main.pos_alpha.kp);
+ eeprom_write_word (p16++, cs_aux[0].pos.kp);
+ eeprom_write_word (p16++, cs_aux[1].pos.kp);
+ eeprom_write_word (p16++, cs_main.pos_theta.ki);
+ eeprom_write_word (p16++, cs_main.pos_alpha.ki);
+ eeprom_write_word (p16++, cs_aux[0].pos.ki);
+ eeprom_write_word (p16++, cs_aux[1].pos.ki);
+ eeprom_write_word (p16++, cs_main.pos_theta.kd);
+ eeprom_write_word (p16++, cs_main.pos_alpha.kd);
+ eeprom_write_word (p16++, cs_aux[0].pos.kd);
+ eeprom_write_word (p16++, cs_aux[1].pos.kd);
+ eeprom_write_word (p16++, cs_main.blocking_detection_theta.error_limit);
+ eeprom_write_word (p16++, cs_main.blocking_detection_theta.speed_limit);
+ eeprom_write_word (p16++, cs_main.blocking_detection_theta.counter_limit);
+ eeprom_write_word (p16++, cs_main.blocking_detection_alpha.error_limit);
+ eeprom_write_word (p16++, cs_main.blocking_detection_alpha.speed_limit);
+ eeprom_write_word (p16++, cs_main.blocking_detection_alpha.counter_limit);
+ 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_main.pos_theta.e_sat);
+ eeprom_write_word (p16++, cs_main.pos_theta.i_sat);
+ eeprom_write_word (p16++, cs_main.pos_theta.d_sat);
+ eeprom_write_word (p16++, cs_main.pos_alpha.e_sat);
+ eeprom_write_word (p16++, cs_main.pos_alpha.i_sat);
+ eeprom_write_word (p16++, cs_main.pos_alpha.d_sat);
+ 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);
eeprom_write_word (p16++, traj_eps);
eeprom_write_word (p16++, traj_aeps);
eeprom_write_word (p16++, traj_angle_limit);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 000a0e14..c4cec931 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x4e
+#define EEPROM_KEY 0x4f
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 677fea84..5d91d90e 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -32,16 +32,14 @@
#include "io.h"
#include "misc.h"
-#include "state.h"
-#include "counter.h"
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
+#include "cs.h"
#include "postrack.h"
#include "traj.h"
#include "aux.h"
+#include "seq.h"
+
#include "twi_proto.h"
#include "eeprom.h"
@@ -105,13 +103,11 @@ main (int argc, char **argv)
PORTF = 0xfc;
PORTG = 0x18;
LED_SETUP;
- pwm_init ();
timer_init ();
- counter_init ();
uart0_init ();
twi_proto_init ();
postrack_init ();
- speed_init ();
+ cs_init ();
traj_init ();
aux_init ();
eeprom_read_params ();
@@ -126,43 +122,49 @@ main (int argc, char **argv)
static void
main_loop (void)
{
+ main_timer[3] = timer_read ();
+ /* Compute absolute position. */
+ postrack_update ();
+ aux_pos_update ();
+ main_timer[4] = timer_read ();
+ /* Compute trajectory. */
+ traj_update ();
+ 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 ();
+ encoder_corrector_update (&encoder_right_corrector, &encoder_right);
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. */
- postrack_update ();
- aux_pos_update ();
- /* Compute trajectory. */
- if (state_main.mode >= MODE_TRAJ)
- traj_update ();
- aux_traj_update ();
- /* Speed control. */
- speed_update ();
- main_timer[3] = timer_read ();
+ /* Sequences. */
+ seq_update (&seq_main, &cs_main.state);
+ seq_update (&seq_aux[0], &cs_aux[0].state);
+ seq_update (&seq_aux[1], &cs_aux[1].state);
/* Stats. */
if (main_sequence_ack
- && (state_main.sequence_ack != state_main.sequence_finish
- || state_aux[0].sequence_ack != state_aux[0].sequence_finish
- || state_aux[1].sequence_ack != state_aux[1].sequence_finish)
+ && (seq_main.ack != seq_main.finish
+ || seq_aux[0].ack != seq_aux[0].finish
+ || seq_aux[1].ack != seq_aux[1].finish)
&& !--main_sequence_ack_cpt)
{
- proto_send3b ('A', state_main.sequence_finish,
- state_aux[0].sequence_finish,
- state_aux[1].sequence_finish);
+ proto_send3b ('A', seq_main.finish,
+ seq_aux[0].finish,
+ seq_aux[1].finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
- proto_send4w ('C', counter_left, counter_right,
- counter_aux[0], counter_aux[1]);
+ proto_send4w ('C', encoder_left.cur, encoder_right.cur,
+ encoder_aux[0].cur, encoder_aux[1].cur);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_postrack && !--main_stat_postrack_cpt)
@@ -177,20 +179,26 @@ main_loop (void)
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send4b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8,
- speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ proto_send4b ('S', cs_main.speed_theta.cur >> 8,
+ cs_main.speed_alpha.cur >> 8,
+ 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_theta.e_old, pos_theta.i,
- pos_alpha.e_old, pos_alpha.i);
+ proto_send4w ('P', cs_main.pos_theta.last_error,
+ cs_main.pos_theta.i,
+ cs_main.pos_alpha.last_error,
+ cs_main.pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
if (main_stat_pos_aux && !--main_stat_pos_aux_cpt)
{
- proto_send4w ('Q', pos_aux[0].e_old, pos_aux[0].i,
- pos_aux[1].e_old, pos_aux[1].i);
+ proto_send4w ('Q', 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_aux_cpt = main_stat_pos_aux;
}
#ifdef HOST
@@ -204,8 +212,8 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send4w ('W', pwm_left.cur, pwm_right.cur,
- pwm_aux[0].cur, pwm_aux[1].cur);
+ proto_send4w ('W', output_left.cur, output_right.cur,
+ output_aux[0].cur, output_aux[1].cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -232,17 +240,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)
@@ -255,120 +266,105 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Commands. */
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);
+ output_set (&output_left, 0);
+ output_set (&output_right, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
break;
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', 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]));
+ output_set (&output_left, v8_to_v16 (args[0], args[1]));
+ output_set (&output_right, v8_to_v16 (args[2], args[3]));
+ control_state_set_mode (&cs_main.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', 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]);
+ cs_main.pos_theta.cons += v8_to_v16 (args[0], args[1]);
+ cs_main.pos_alpha.cons += v8_to_v16 (args[2], args[3]);
+ control_state_set_mode (&cs_main.state, CS_MODE_POS_CONTROL, 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', 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;
+ speed_control_set_speed (&cs_main.speed_theta, 0);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('s', 2):
/* 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;
+ speed_control_set_speed (&cs_main.speed_theta, args[0]);
+ speed_control_set_speed (&cs_main.speed_alpha, args[1]);
+ control_state_set_mode (&cs_main.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', 9):
/* Set speed controlled position consign.
* - d: theta consign offset.
* - d: alpha consign offset.
* - b: sequence number. */
- if (args[8] == state_main.sequence)
+ if (!seq_start (&seq_main, args[8]))
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]);
+ speed_control_pos_offset (&cs_main.speed_theta,
+ v8_to_v32 (args[0], args[1], args[2],
+ args[3]));
+ speed_control_pos_offset (&cs_main.speed_alpha,
+ v8_to_v32 (args[4], args[5], args[6],
+ args[7]));
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('l', 5):
/* Set linear speed controlled position consign.
* - d: consign offset.
* - b: sequence number. */
- if (args[4] == state_main.sequence)
+ if (!seq_start (&seq_main, args[4]))
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;
- state_start (&state_main, MODE_SPEED, args[4]);
+ speed_control_pos_offset (&cs_main.speed_theta,
+ v8_to_v32 (args[0], args[1], args[2], args[3]));
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ traj_speed_start ();
break;
case c ('a', 5):
/* Set angular speed controlled position consign.
* - d: angle offset.
* - b: sequence number. */
- if (args[4] == state_main.sequence)
+ if (!seq_start (&seq_main, args[4]))
break;
traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2],
- args[3]), args[4]);
+ args[3]));
break;
case c ('S', 6):
/* Set auxiliary speed controlled position consign.
@@ -376,29 +372,28 @@ 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;
case c ('f', 2):
/* Go to the wall.
* - b: 0: forward, 1: backward.
* - b: sequence number. */
- if (args[1] == state_main.sequence)
+ if (!seq_start (&seq_main, args[1]))
break;
- traj_ftw_start (args[0], args[1]);
+ traj_ftw_start (args[0]);
break;
case c ('f', 3):
/* Go to the wall, using center with a delay.
* - b: 0: forward, 1: backward.
* - b: delay.
* - b: sequence number. */
- if (args[1] == state_main.sequence)
+ if (!seq_start (&seq_main, args[2]))
break;
- traj_ftw_start_center (args[0], args[1], args[2]);
+ traj_ftw_start_center (args[0], args[1]);
break;
case c ('f', 12):
/* Push the wall.
@@ -408,7 +403,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: init_a, f0.16.
* - b: sequence number. */
{
- if (args[11] == state_main.sequence)
+ if (!seq_start (&seq_main, args[11]))
break;
int32_t angle;
if (args[9] == 0xff && args[10] == 0xff)
@@ -418,15 +413,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_ptw_start (args[0],
v8_to_v32 (args[1], args[2], args[3], args[4]),
v8_to_v32 (args[5], args[6], args[7], args[8]),
- angle, args[11]);
+ angle);
}
break;
case c ('F', 1):
/* Go to the dispenser.
* - b: sequence number. */
- if (args[0] == state_main.sequence)
+ if (!seq_start (&seq_main, args[0]))
break;
- traj_gtd_start (args[0]);
+ traj_gtd_start ();
break;
case c ('x', 10):
/* Go to position.
@@ -434,20 +429,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: y, f24.8.
* - b: backward (see traj.h).
* - b: sequence number. */
- if (args[9] == state_main.sequence)
+ if (!seq_start (&seq_main, args[9]))
break;
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]);
+ args[8]);
break;
case c ('x', 3):
/* Go to angle.
* - d: a, f0.16.
* - b: sequence number. */
- if (args[2] == state_main.sequence)
+ if (!seq_start (&seq_main, args[2]))
break;
- traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
- args[2]);
+ traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0));
break;
case c ('x', 12):
/* Go to position, then angle.
@@ -456,12 +450,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: a, f0.16.
* - b: backward (see traj.h).
* - b: sequence number. */
- if (args[11] == state_main.sequence)
+ if (!seq_start (&seq_main, args[11]))
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]);
+ args[10]);
break;
case c ('y', 4):
/* Auxiliary go to position.
@@ -469,9 +463,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', 3):
/* Auxiliary find zero.
@@ -479,25 +473,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: speed.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[2] == state->sequence)
+ if (!seq_start (seq, args[2]))
break;
if (args[0] == 0)
- aux_traj_find_limit_start (auxp, args[1], args[2]);
+ aux_traj_find_limit_start (auxp, args[1]);
else
- aux_traj_find_zero_reverse_start (auxp, args[1], args[2]);
+ aux_traj_find_zero_reverse_start (auxp, args[1]);
break;
case c ('a', 3):
/* Set all acknoledge.
* - b: main ack sequence number.
* - b: first auxiliary ack sequence number.
* - b: second auxiliary ack sequence number. */
- state_acknowledge (&state_aux[0], args[1]);
- state_acknowledge (&state_aux[1], args[2]);
+ seq_acknowledge (&seq_aux[0], args[1]);
+ seq_acknowledge (&seq_aux[1], args[2]);
/* no break; */
case c ('a', 1):
/* Set main acknoledge.
* - b: main ack sequence number. */
- state_acknowledge (&state_main, args[0]);
+ seq_acknowledge (&seq_main, args[0]);
break;
/* Stats.
* - b: interval between stats. */
@@ -555,21 +549,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
switch (args[1])
{
case 't':
- pos = &pos_theta;
- speed = &speed_theta;
+ pos = &cs_main.pos_theta;
+ speed = &cs_main.speed_theta;
+ bd = &cs_main.blocking_detection_theta;
break;
case 'a':
- pos = &pos_alpha;
- speed = &speed_alpha;
+ pos = &cs_main.pos_alpha;
+ speed = &cs_main.speed_alpha;
+ bd = &cs_main.blocking_detection_alpha;
break;
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))
@@ -598,8 +596,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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]);
+ encoder_corrector_set_correction (&encoder_right_corrector,
+ v8_to_v32 (args[1], args[2],
+ args[3], args[4]));
break;
case c ('f', 3):
/* Set footing.
@@ -649,19 +648,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_main.pos_theta.e_sat =
+ cs_main.pos_alpha.e_sat =
+ 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_main.pos_theta.i_sat =
+ cs_main.pos_alpha.i_sat =
+ 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_main.pos_theta.d_sat =
+ cs_main.pos_alpha.d_sat =
+ cs_aux[0].pos.d_sat =
+ cs_aux[1].pos.d_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('e', 5):
traj_eps = v8_to_v16 (args[1], args[2]);
@@ -673,7 +684,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('w', 2):
/* Set PWM direction.
* - b: bits: 0000[aux1][aux0][right][left]. */
- pwm_set_reverse (args[1]);
+ output_set_reverse (&output_left, (args[1] & 1) ? 1 : 0);
+ output_set_reverse (&output_right, (args[1] & 2) ? 1 : 0);
+ output_set_reverse (&output_aux[0], (args[1] & 4) ? 1 : 0);
+ output_set_reverse (&output_aux[1], (args[1] & 8) ? 1 : 0);
break;
case c ('E', 2):
/* Write to eeprom.
@@ -686,38 +700,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('P', 1):
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
- proto_send1d ('c', counter_right_correction);
+ proto_send1d ('c', encoder_right_corrector.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_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_send2w ('e', traj_eps, traj_aeps);
proto_send1w ('l', traj_angle_limit);
- proto_send1b ('w', pwm_reverse);
+ proto_send1b ('w', (output_left.reverse ? 1 : 0)
+ | (output_right.reverse ? 2 : 0)
+ | (output_aux[0].reverse ? 4 : 0)
+ | (output_aux[1].reverse ? 8 : 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/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 3839af1e..72524932 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -25,7 +25,6 @@
#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"
@@ -34,87 +33,13 @@
#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }
-/* RE25CLL with 1:10 gearbox model. */
-static const struct motor_def_t re25cll_model =
-{
- /* Motor characteristics. */
- 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)). */
- 2.18, /* Terminal resistance (Ohm). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 12.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 10, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.0, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
-/* RE25G with 1:20.25 gearbox model. */
-static const struct motor_def_t re25g_model =
-{
- /* Motor characteristics. */
- 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)). */
- 2.32, /* Terminal resistance (Ohm). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 20.25, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.0, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
-/* AMAX32GHP with 1:16 gearbox model. */
-static const struct motor_def_t 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.0, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
-/* Faulhaber 2657 and a 1:9.7 ratio gearbox. */
-static const struct motor_def_t faulhaber_2657_model =
-{
- /* Motor characteristics. */
- 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
- 34.8 / 1000, /* Torque constant (N.m/A). */
- 0, /* Bearing friction (N.m/(rad/s)). */
- 2.84, /* Terminal resistance (Ohm). */
- 0.380 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 9.7, /* Gearbox ratio. */
- 0.80, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.0, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
/* Gloubi, Efrei 2006. */
static const struct robot_t gloubi_robot =
{
/* Main motors. */
- &re25cll_model,
+ &motor_model_def_re25cll_x10,
+ /* Motors voltage (V). */
+ 12.0,
/* Number of steps on the main motors encoders. */
500,
/* Wheel radius (m). */
@@ -127,14 +52,16 @@ static const struct robot_t gloubi_robot =
13.0, // approx
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, { 0, 0 }, NULL, NULL, NO_CORNER, NULL
};
/* Taz, APBTeam/Efrei 2005. */
static const struct robot_t taz_robot =
{
/* Main motors. */
- &re25cll_model,
+ &motor_model_def_re25cll_x10,
+ /* Motors voltage (V). */
+ 12.0,
/* Number of steps on the main motors encoders. */
500,
/* Wheel radius (m). */
@@ -147,14 +74,16 @@ static const struct robot_t taz_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, { 0, 0 }, NULL, NULL, NO_CORNER, NULL
};
/* TazG, Taz with RE25G motors. */
static const struct robot_t tazg_robot =
{
/* Main motors. */
- &re25g_model,
+ &motor_model_def_re25g_x20_25,
+ /* Motors voltage (V). */
+ 24.0,
/* Number of steps on the main motors encoders. */
500,
/* Wheel radius (m). */
@@ -167,35 +96,16 @@ static const struct robot_t tazg_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
-};
-
-/* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */
-static const struct motor_def_t giboulee_arm_model =
-{
- /* Motor characteristics. */
- 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)). */
- 2.18, /* Terminal resistance (Ohm). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* WARNING: Giboulée arm use a 12V motor on 24V power, PWM should be
- * limited to half scale. */
- /* Gearbox characteristics. */
- 10, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.200 * 0.1 * 0.1, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, { 0, 0 }, NULL, NULL, NO_CORNER, NULL
};
/* Giboulée, APBTeam 2008. */
static const struct robot_t giboulee_robot =
{
/* Main motors. */
- &amax32ghp_model,
+ &motor_model_def_amax32ghp_x16,
+ /* Motors voltage (V). */
+ 24.0,
/* Number of steps on the main motors encoders. */
2500,
/* Wheel radius (m). */
@@ -213,16 +123,18 @@ static const struct robot_t giboulee_robot =
/** Distance between the encoders wheels (m). */
0.28,
/** Auxiliary motors, NULL if not present. */
- { &giboulee_arm_model, NULL },
+ { &motor_model_def_re25cll_x10, NULL },
/** Number of steps for each auxiliary motor encoder. */
{ 500, 0 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 0.200 * 0.1 * 0.1, 0 },
/** Sensor update function. */
simu_sensor_update_giboulee,
- NULL, NO_CORNER,
+ NULL, NO_CORNER, NULL
};
/* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */
-static const struct motor_def_t aquajim_arm_model =
+static motor_model_def_t aquajim_arm_model_def =
{
/* Motor characteristics. */
317 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
@@ -230,34 +142,11 @@ static const struct motor_def_t aquajim_arm_model =
0, /* Bearing friction (N.m/(rad/s)). */
0.316, /* Terminal resistance (Ohm). */
0.08 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
/* Gearbox characteristics. */
4.0 * 80.0 / 15.0, /* Gearbox ratio. */
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
- 0.05 * 2.5 * 0.06 * 0.06, /* Load (kg.m^2). */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
-
-/* AquaJim elevator model, with a RE25CLL and a 1:10 ratio gearbox. */
-static const struct motor_def_t aquajim_elevator_model =
-{
- /* Motor characteristics. */
- 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)). */
- 2.18, /* Terminal resistance (Ohm). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* WARNING: this motor uses a 12V motor on 24V power, PWM should be
- * limited to half scale. */
- /* Gearbox characteristics. */
- 10, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.200 * 0.01 * 0.01,/* Load (kg.m^2). */
- /* This is a pifometric estimation. */
+ 0.0, /* Load (kg.m^2). */
/* Hardware limits. */
-INFINITY, +INFINITY,
};
@@ -266,7 +155,9 @@ static const struct motor_def_t aquajim_elevator_model =
static const struct robot_t aquajim_robot =
{
/* Main motors. */
- &amax32ghp_model,
+ &motor_model_def_amax32ghp_x16,
+ /* Motors voltage (V). */
+ 24.0,
/* Number of steps on the main motors encoders. */
2500,
/* Wheel radius (m). */
@@ -284,61 +175,30 @@ static const struct robot_t aquajim_robot =
/** Distance between the encoders wheels (m). */
0.28,
/** Auxiliary motors, NULL if not present. */
- { &aquajim_arm_model, &aquajim_elevator_model },
+ { &aquajim_arm_model_def, &motor_model_def_re25cll_x10 },
/** Number of steps for each auxiliary motor encoder. */
{ 250, 250 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 0.05 * 2.5 * 0.06 * 0.06, 0.200 * 0.01 * 0.01 /* Pif */ },
/** Sensor update function. */
simu_sensor_update_aquajim,
- NULL, NO_CORNER,
+ NULL, NO_CORNER, NULL
};
-/* Marcel elevator model, with a Faulhaber 2657 and a 1:9.7 ratio gearbox. */
-static const struct motor_def_t marcel_elevator_model =
-{
- /* Motor characteristics. */
- 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
- 34.8 / 1000, /* Torque constant (N.m/A). */
- 0, /* Bearing friction (N.m/(rad/s)). */
- 2.84, /* Terminal resistance (Ohm). */
- 0.380 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* Gearbox characteristics. */
- 9.7, /* Gearbox ratio. */
- 0.80, /* Gearbox efficiency. */
- /* Load characteristics. */
- 1.0 * 0.01115 * 0.01115,/* Load (kg.m^2). */
- /* This is a pifometric estimation. */
- /* Hardware limits. */
- 0.0, +INFINITY,
-};
-
-/* Marcel gate model, with a RE25CLL and a 1:10 ratio gearbox. */
-static const struct motor_def_t marcel_gate_model =
+void
+marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left,
+ struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[])
{
- /* Motor characteristics. */
- 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)). */
- 2.18, /* Terminal resistance (Ohm). */
- 0.24 / 1000, /* Terminal inductance (H). */
- 24.0, /* Maximum voltage (V). */
- /* WARNING: this motor uses a 12V motor on 24V power, PWM should be
- * limited to half scale. */
- /* Gearbox characteristics. */
- 10, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load characteristics. */
- 0.100 * 0.01 * 0.01,/* Load (kg.m^2). */
- /* This is a pifometric estimation. */
- /* Hardware limits. */
- -INFINITY, +INFINITY,
-};
+ aux_motor[0].m.th_min = 0.0;
+}
/* Marcel, APBTeam 2010. */
static const struct robot_t marcel_robot =
{
/* Main motors. */
- &amax32ghp_model,
+ &motor_model_def_amax32ghp_x16,
+ /* Motors voltage (V). */
+ 24.0,
/* Number of steps on the main motors encoders. */
2500,
/* Wheel radius (m). */
@@ -356,19 +216,25 @@ static const struct robot_t marcel_robot =
/** Distance between the encoders wheels (m). */
0.28,
/** Auxiliary motors, NULL if not present. */
- { &marcel_elevator_model, &marcel_gate_model },
+ { &motor_model_def_faulhaber_2657_x9_7, &motor_model_def_re25cll_x10 },
/** Number of steps for each auxiliary motor encoder. */
{ 256, 250 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 1.0 * 0.01115 * 0.01115, 0.100 * 0.01 * 0.01 /* Pif */ },
/** Sensor update function. */
simu_sensor_update_marcel,
NULL, NO_CORNER,
+ /** Initialisation function. */
+ marcel_robot_init,
};
/* Robospierre, APBTeam 2011. */
static const struct robot_t robospierre_robot =
{
/* Main motors. */
- &faulhaber_2657_model,
+ &motor_model_def_faulhaber_2657_x9_7,
+ /* Motors voltage (V). */
+ 24.0,
/* Number of steps on the main motors encoders. */
2500,
/* Wheel radius (m). */
@@ -389,6 +255,8 @@ static const struct robot_t robospierre_robot =
{ NULL, NULL },
/** Number of steps for each auxiliary motor encoder. */
{ 0, 0 },
+ /** Load for auxiliary motors (kg.m^2). */
+ { 0, 0 },
/** Sensor update function. */
NULL,
/** Table test function, return false if given robot point is not in
@@ -396,6 +264,8 @@ static const struct robot_t robospierre_robot =
simu_table_test_robospierre,
/** Robot corners, from front left, then clockwise. */
{ { 150, 110 }, { 150, -110 }, { -150, -110 }, { -150, 110 } },
+ /** Initialisation function. */
+ NULL,
};
/* Table of models. */
@@ -429,8 +299,8 @@ models_get (const char *name)
/** Initialise simulation models. */
void
-models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
- struct motor_t *main_motor_right, struct motor_t aux_motor[])
+models_init (const struct robot_t *robot, motor_model_t *main_motor_left,
+ motor_model_t *main_motor_right, motor_model_t aux_motor[])
{
int i;
if (main_motor_left)
@@ -456,10 +326,13 @@ models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
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, main_motor_left, main_motor_right, aux_motor);
}
diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h
index d8f1dcb8..d865aa6b 100644
--- a/digital/asserv/src/asserv/models.host.h
+++ b/digital/asserv/src/asserv/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 / (14745600.0 / 256 / 256))
@@ -34,7 +35,9 @@
struct robot_t
{
/** Main motors. */
- const struct motor_def_t *main_motor;
+ const motor_model_def_t *main_motor;
+ /** Motors voltage (V). */
+ double u_max;
/** Number of steps on the main motors encoders. */
int main_encoder_steps;
/** Wheel radius (m). */
@@ -52,9 +55,11 @@ struct robot_t
/** Distance between the encoders wheels (m). */
double encoder_footing;
/** 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];
/** 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);
/** Table test function, return false if given robot point is not in
@@ -62,6 +67,9 @@ struct robot_t
int (*table_test) (double p_x, double p_y);
/** Robot corners, from front left, then clockwise. */
double corners[CORNERS_NB][2];
+ /** Initialisation function. */
+ void (*init) (const struct robot_t *robot, motor_model_t *main_motor_left,
+ motor_model_t *main_motor_right, motor_model_t aux_motor[]);
};
/** Get a pointer to a model by name, or return 0. */
@@ -70,7 +78,7 @@ models_get (const char *name);
/** Initialise simulation models. */
void
-models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
- struct motor_t *main_motor_right, struct motor_t aux_motor[]);
+models_init (const struct robot_t *robot, motor_model_t *main_motor_left,
+ motor_model_t *main_motor_right, motor_model_t aux_motor[]);
#endif /* models_host_h */
diff --git a/digital/asserv/src/asserv/motor_model.host.c b/digital/asserv/src/asserv/motor_model.host.c
deleted file mode 100644
index f87e7db6..00000000
--- a/digital/asserv/src/asserv/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/asserv/src/asserv/motor_model.host.h b/digital/asserv/src/asserv/motor_model.host.h
deleted file mode 100644
index 283b6fd6..00000000
--- a/digital/asserv/src/asserv/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/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
deleted file mode 100644
index 4b223331..00000000
--- a/digital/asserv/src/asserv/pos.c
+++ /dev/null
@@ -1,208 +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!).
- */
-
-/** Theta/alpha control states. */
-struct pos_t pos_theta, pos_alpha;
-
-/** 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 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_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_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_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_theta);
- pos_reset (pos_alpha);
- state_blocked (state);
- pwm_set (pwm_left, 0);
- pwm_set (pwm_right, 0);
- }
- else
- {
- /* Compute PID. */
- pid_theta = pos_compute_pid (error_theta, pos_theta);
- pid_alpha = pos_compute_pid (error_alpha, pos_alpha);
- /* Update PWM. */
- pwm_set (pwm_left, pid_theta - pid_alpha);
- pwm_set (pwm_right, pid_theta + pid_alpha);
- }
- }
-}
-
-/** Update PWM for a single motor system. */
-void
-pos_update_single (struct state_t *state, struct pos_t *pos,
- int16_t counter_diff, struct pwm_t *pwm)
-{
- if (state->mode >= MODE_POS)
- {
- int16_t pid;
- int32_t error;
- /* Update current shaft position. */
- pos->cur += counter_diff;
- /* Compute error. */
- error = pos->cons - pos->cur;
- /* Test or blocking. */
- if (UTILS_ABS (error) > pos->blocked_error_limit
- && UTILS_ABS (counter_diff) < pos->blocked_speed_limit)
- pos->blocked_counter++;
- else
- pos->blocked_counter = 0;
- if (!(state->variant & 4)
- && pos->blocked_counter > pos->blocked_counter_limit)
- {
- /* Blocked. */
- pos_reset (pos);
- state_blocked (state);
- pwm_set (pwm, 0);
- }
- else
- {
- /* Compute PID. */
- pid = pos_compute_pid (error, pos);
- /* Update PWM. */
- pwm_set (pwm, pid);
- }
- }
-}
-
-/** Update PWM according to consign. */
-void
-pos_update (void)
-{
- uint8_t i;
- pos_update_polar (&state_main, &pos_theta, &pos_alpha,
- counter_left_diff, counter_right_diff,
- &pwm_left, &pwm_right);
- 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/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
deleted file mode 100644
index 2cca2111..00000000
--- a/digital/asserv/src/asserv/pos.h
+++ /dev/null
@@ -1,62 +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_theta, pos_alpha;
-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/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c
index a3b2e8cb..503059f1 100644
--- a/digital/asserv/src/asserv/postrack.c
+++ b/digital/asserv/src/asserv/postrack.c
@@ -28,7 +28,7 @@
#include "modules/math/fixed/fixed.h"
#include "modules/math/math.h"
-#include "counter.h"
+#include "cs.h"
/** Current position, f24.8. */
int32_t postrack_x, postrack_y;
@@ -60,9 +60,9 @@ void
postrack_update (void)
{
int32_t d, dd, da, na, dsc;
- d = counter_right_diff + counter_left_diff; /* 10b */
+ d = encoder_right.diff + encoder_left.diff; /* 10b */
d <<= 16; /* 10.16b */
- if (counter_right_diff == counter_left_diff)
+ if (encoder_right.diff == encoder_left.diff)
{
/* Line. */
postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8;
@@ -71,7 +71,7 @@ postrack_update (void)
else
{
/* Arc. */
- dd = counter_right_diff - counter_left_diff; /* 10b */
+ dd = encoder_right.diff - encoder_left.diff; /* 10b */
dd <<= 16; /* 10.16b */
da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */
/* New angle. */
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
deleted file mode 100644
index cf8df769..00000000
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ /dev/null
@@ -1,62 +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_mp.avr.h"
-#include "pwm_ocr.avr.h"
-
-/** PWM control states. */
-struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
-struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
-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_mp_init ();
- pwm_ocr_init ();
-}
-
-/** Update the hardware PWM values. */
-void
-pwm_update (void)
-{
- pwm_mp_update ();
- 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/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
deleted file mode 100644
index 1d09bde7..00000000
--- a/digital/asserv/src/asserv/pwm.h
+++ /dev/null
@@ -1,108 +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.
- *
- * }}} */
-
-/** Define the absolute maximum PWM value.
- * This value is lowered until the bug relatives to maximum value is fixed
- * (rounding after shifting bug). */
-#define PWM_MAX 0x3f0
-
-/** 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_left, pwm_right, 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_left pwm_left.cur
-#define PWM_VALUE_pwm_right pwm_right.cur
-#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_left PWM_MAX
-#define PWM_MAX_FOR_pwm_right PWM_MAX
-#define PWM_MAX_FOR_pwm_aux0 PWM_MAX
-#define PWM_MAX_FOR_pwm_aux1 (PWM_MAX / 2)
-
-/** 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_left 0x10
-#define PWM_MIN_FOR_pwm_right 0x10
-#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_left _BV (0)
-#define PWM_REVERSE_BIT_pwm_right _BV (1)
-#define PWM_REVERSE_BIT_pwm_aux0 _BV (2)
-#define PWM_REVERSE_BIT_pwm_aux1 _BV (3)
-
-/** 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/asserv/src/asserv/pwm_config.h b/digital/asserv/src/asserv/pwm_config.h
deleted file mode 100644
index 48b99217..00000000
--- a/digital/asserv/src/asserv/pwm_config.h
+++ /dev/null
@@ -1,52 +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))
-#define PWM3or4 (defined (PWM3) || defined (PWM4))
-#ifdef PWM1
-# define PWM1c(x) x
-#else
-# define PWM1c(x) 0
-#endif
-#ifdef PWM2
-# define PWM2c(x) x
-#else
-# define PWM2c(x) 0
-#endif
-#ifdef PWM3
-# define PWM3c(x) x
-#else
-# define PWM3c(x) 0
-#endif
-#ifdef PWM4
-# define PWM4c(x) x
-#else
-# define PWM4c(x) 0
-#endif
-
-#endif /* pwm_config_h */
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c
deleted file mode 100644
index 2349148a..00000000
--- a/digital/asserv/src/asserv/pwm_mp.avr.c
+++ /dev/null
@@ -1,126 +0,0 @@
-/* pwm_mp.avr.c - PWM to be used with motor-power-avr. */
-/* 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_mp.avr.h"
-#include "pwm.h"
-
-#include "modules/spi/spi.h"
-#include "modules/utils/utils.h"
-#include "io.h"
-
-/** Assign PWM outputs. */
-#define PWM1 pwm_left
-#define PWM2 pwm_right
-#define PWM3 pwm_aux0
-#define PWM4 pwm_aux1
-
-#include "pwm_config.h"
-
-/** When true, start transmission. */
-uint8_t pwm_mp_go;
-
-/** Initialise communication with motor-power-avr. */
-void
-pwm_mp_init (void)
-{
-#if PWM3or4
- /* Only needed for PWM3 and 4 because it is done in spi_init for SS (port
- * B0). */
- PORTE |= _BV (4);
- DDRE |= _BV (4);
-#endif
-#if PWM1or2 || PWM3or4
- spi_init (SPI_MASTER, SPI_CPOL_FALLING | SPI_CPHA_SETUP, SPI_MSB_FIRST,
- SPI_FOSC_DIV16);
-#endif
- /* Reset PWM values at reset. */
- pwm_mp_go = 1;
- pwm_mp_update ();
- pwm_mp_go = 0;
-}
-
-#if PWM1or2 || PWM3or4
-
-/** Send command using SPI. */
-static void
-pwm_mp_send (int16_t pwm1, int16_t pwm2,
- uint8_t invert1, uint8_t invert2)
-{
- uint8_t v;
- uint8_t cks;
- /* Convert to 12 bits. */
- int16_t pwm1c = pwm1 << 1;
- int16_t pwm2c = pwm2 << 1;
- if (invert1)
- pwm1c = -pwm1c;
- if (invert2)
- pwm2c = -pwm2c;
- /* Send, computing checksum on the way. */
- cks = 0x42;
- v = ((pwm1c >> 4) & 0xf0) | ((pwm2c >> 8) & 0x0f);
- spi_send (v);
- cks ^= v;
- v = pwm1c;
- spi_send (v);
- cks ^= v;
- v = pwm2c;
- spi_send (v);
- cks ^= v;
- spi_send (cks);
-}
-
-#endif /* PWM1or2 || PWM3or4 */
-
-/** Update PWM values. */
-void
-pwm_mp_update (void)
-{
-#if PWM1or2 || PWM3or4
- if (PWM1c (PWM_VALUE (PWM1)) || PWM2c (PWM_VALUE (PWM2))
- || PWM3c (PWM_VALUE (PWM3)) || PWM4c (PWM_VALUE (PWM4)))
- pwm_mp_go = 1;
- if (!pwm_mp_go)
- return;
-#endif
-#if PWM1or2
- /* Chip enable. */
- PORTB &= ~_BV (0);
- pwm_mp_send (PWM1c (PWM_VALUE (PWM1)), PWM2c (PWM_VALUE (PWM2)),
- PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)),
- PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2)));
- /* Chip disable. */
- PORTB |= _BV (0);
-#endif
-#if PWM3or4
- /* Chip enable. */
- PORTE &= ~_BV (4);
- pwm_mp_send (PWM3c (PWM_VALUE (PWM3)), PWM4c (PWM_VALUE (PWM4)),
- PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)),
- PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4)));
- /* Chip disable. */
- PORTE |= _BV (4);
-#endif
-}
-
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.h b/digital/asserv/src/asserv/pwm_mp.avr.h
deleted file mode 100644
index cf30578f..00000000
--- a/digital/asserv/src/asserv/pwm_mp.avr.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef pwm_mp_avr_h
-#define pwm_mp_avr_h
-/* pwm_mp.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_mp_init (void);
-
-void
-pwm_mp_update (void);
-
-#endif /* pwm_mp_avr_h */
diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.c b/digital/asserv/src/asserv/pwm_ocr.avr.c
deleted file mode 100644
index 25f2cfc7..00000000
--- a/digital/asserv/src/asserv/pwm_ocr.avr.c
+++ /dev/null
@@ -1,221 +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. */
-#undef PWM1
-#undef PWM2
-#undef PWM3
-#undef PWM4
-
-#define PWM1_OCR OCR1B
-#define PWM1_OCR_BIT 6
-#define PWM1_DIR 4
-#define PWM2_OCR OCR1C
-#define PWM2_OCR_BIT 7
-#define PWM2_DIR 5
-#define PWM3_OCR OCR3B
-#define PWM3_OCR_BIT 4
-#define PWM3_DIR 2
-#define PWM4_OCR OCR3C
-#define PWM4_OCR_BIT 5
-#define PWM4_DIR 3
-
-#include "pwm_config.h"
-
-/** PWM reverse direction, port B. */
-static uint8_t pwm_ocr_dir_reverse_b;
-/** PWM reverse direction, port E. */
-static uint8_t pwm_ocr_dir_reverse_e;
-
-/** Initialise PWM generator. */
-void
-pwm_ocr_init (void)
-{
- /* Fast PWM, TOP = 0x3ff, OCnB & OCnC with positive logic.
- f_IO without prescaler.
- Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */
-#if PWM1or2
- TCCR1A =
- regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
- 0, 0, 1, 0, 1, 0, 1, 1);
- TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
- 0, 0, 0, 0, 1, 0, 0, 1);
- /* Enable pwm and direction outputs in DDRB. */
- DDRB |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR))
- | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR));
-#endif
-#if PWM3or4
- TCCR3A =
- regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30,
- 0, 0, 1, 0, 1, 0, 1, 1);
- TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
- 0, 0, 0, 0, 1, 0, 0, 1);
- /* Enable pwm and direction outputs in DDRE. */
- DDRE |= PWM3c (_BV (PWM3_OCR_BIT) | _BV (PWM3_DIR))
- | PWM4c (_BV (PWM4_OCR_BIT) | _BV (PWM4_DIR));
-#endif
-}
-
-/** Update the hardware PWM values. */
-void
-pwm_ocr_update (void)
-{
-#if PWM1or2
- uint8_t dir_b;
- /* Sample port B. */
- dir_b = PORTB & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR)));
-# ifdef PWM1
- uint16_t pwm1;
- /* Set PWM1. */
- if (PWM_VALUE (PWM1) == 0)
- {
- pwm1 = 0;
- }
- else if (PWM_VALUE (PWM1) < 0)
- {
- pwm1 = -PWM_VALUE (PWM1);
- }
- else
- {
- dir_b |= _BV (PWM1_DIR);
- pwm1 = PWM_VALUE (PWM1);
- }
-# endif /* PWM1 */
-# ifdef PWM2
- uint16_t pwm2;
- /* Set PWM2. */
- if (PWM_VALUE (PWM2) == 0)
- {
- pwm2 = 0;
- }
- else if (PWM_VALUE (PWM2) < 0)
- {
- pwm2 = -PWM_VALUE (PWM2);
- }
- else
- {
- dir_b |= _BV (PWM2_DIR);
- pwm2 = PWM_VALUE (PWM2);
- }
-# endif /* PWM2 */
-#endif /* PWM1or2 */
-#if PWM3or4
- uint8_t dir_e;
- /* Sample port E. */
- dir_e = PORTE & ~(PWM3c (_BV (PWM3_DIR)) | PWM4c (_BV (PWM4_DIR)));
-# ifdef PWM3
- uint16_t pwm3;
- /* Set PWM3. */
- if (PWM_VALUE (PWM3) == 0)
- {
- pwm3 = 0;
- }
- else if (PWM_VALUE (PWM3) < 0)
- {
- pwm3 = -PWM_VALUE (PWM3);
- }
- else
- {
- dir_e |= _BV (PWM3_DIR);
- pwm3 = PWM_VALUE (PWM3);
- }
-# endif /* PWM3 */
-# ifdef PWM4
- uint16_t pwm4;
- /* Set PWM4. */
- if (PWM_VALUE (PWM4) == 0)
- {
- pwm4 = 0;
- }
- else if (PWM_VALUE (PWM4) < 0)
- {
- pwm4 = -PWM_VALUE (PWM4);
- }
- else
- {
- dir_e |= _BV (PWM4_DIR);
- pwm4 = PWM_VALUE (PWM4);
- }
-# endif /* PWM4 */
-#endif /* PWM3or4 */
- /* 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_b ^= pwm_ocr_dir_reverse_b;
- PORTB = dir_b;
-# ifdef PWM1
- PWM1_OCR = pwm1;
-# endif
-# ifdef PWM2
- PWM2_OCR = pwm2;
-# endif
-#endif /* PWM1or2 */
-#if PWM3or4
- dir_e ^= pwm_ocr_dir_reverse_e;
- PORTE = dir_e;
-# ifdef PWM3
- PWM3_OCR = pwm3;
-# endif
-# ifdef PWM4
- PWM4_OCR = pwm4;
-# endif
-#endif /* PWM3or4 */
-}
-
-void
-pwm_ocr_set_reverse (uint8_t reverse)
-{
- pwm_reverse = reverse;
- pwm_ocr_dir_reverse_b = 0;
- pwm_ocr_dir_reverse_e = 0;
-#ifdef PWM1
- if (reverse & PWM_REVERSE_BIT (PWM1))
- pwm_ocr_dir_reverse_b |= _BV (PWM1_DIR);
-#endif
-#ifdef PWM2
- if (reverse & PWM_REVERSE_BIT (PWM2))
- pwm_ocr_dir_reverse_b |= _BV (PWM2_DIR);
-#endif
-#ifdef PWM3
- if (reverse & PWM_REVERSE_BIT (PWM3))
- pwm_ocr_dir_reverse_e |= _BV (PWM3_DIR);
-#endif
-#ifdef PWM4
- if (reverse & PWM_REVERSE_BIT (PWM4))
- pwm_ocr_dir_reverse_e |= _BV (PWM4_DIR);
-#endif
-}
-
diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.h b/digital/asserv/src/asserv/pwm_ocr.avr.h
deleted file mode 100644
index cd013824..00000000
--- a/digital/asserv/src/asserv/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/asserv/src/asserv/state.c b/digital/asserv/src/asserv/seq.c
index 712b445c..6cbf596f 100644
--- a/digital/asserv/src/asserv/state.c
+++ b/digital/asserv/src/asserv/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,9 +23,9 @@
*
* }}} */
#include "common.h"
-#include "state.h"
+#include "seq.h"
-struct state_t state_main;
+seq_t seq_main;
-struct state_t state_aux[AC_ASSERV_AUX_NB];
+seq_t seq_aux[AC_ASSERV_AUX_NB];
diff --git a/digital/asserv/src/asserv/seq.h b/digital/asserv/src/asserv/seq.h
new file mode 100644
index 00000000..14e35cca
--- /dev/null
+++ b/digital/asserv/src/asserv/seq.h
@@ -0,0 +1,95 @@
+#ifndef seq_h
+#define seq_h
+/* seq.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * 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 "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. 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. This one is
+ * directly implemented in control system. */
+
+/** Current sequence state. */
+struct seq_t
+{
+ /** 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 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 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 ack;
+};
+typedef struct seq_t seq_t;
+
+/** Main motors sequence state. */
+extern seq_t seq_main;
+
+/** Auxiliary motor states. */
+extern seq_t seq_aux[AC_ASSERV_AUX_NB];
+
+/** 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)
+{
+ if (new == seq->cur)
+ return 0;
+ else
+ {
+ seq->cur = new;
+ return 1;
+ }
+}
+
+/** Acknowledge a command completion and blocked state. */
+static inline void
+seq_acknowledge (seq_t *seq, uint8_t ack)
+{
+ seq->ack = ack;
+}
+
+/** Update sequence state according to control state. */
+static inline void
+seq_update (seq_t *seq, control_state_t *state)
+{
+ if (control_state_is_blocked (state))
+ seq->finish = seq->cur | 0x80;
+ else if (control_state_is_finished (state))
+ seq->finish = seq->cur;
+}
+
+#endif /* seq_h */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 7053ae60..1c609cc6 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -39,52 +39,30 @@
#include <time.h>
#include <sys/time.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 DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
-/** Overall counter values. */
-uint16_t counter_left, counter_right,
- counter_aux[AC_ASSERV_AUX_NB];
-/** Counter differences since last update.
- * Maximum of 9 significant bits, sign included. */
-int16_t counter_left_diff, counter_right_diff,
- counter_aux_diff[AC_ASSERV_AUX_NB];
-/** 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_left = PWM_INIT_FOR (pwm_left);
-struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
-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_left_model, simu_right_model,
- simu_aux_model[AC_ASSERV_AUX_NB];
+motor_model_t simu_left_model, simu_right_model,
+ simu_aux_model[AC_ASSERV_AUX_NB];
/** Computed simulated position (mm, rad). */
double simu_pos_x, simu_pos_y, simu_pos_a;
-/** Full counter values. */
-uint32_t simu_counter_left, simu_counter_right,
- simu_counter_aux[AC_ASSERV_AUX_NB];
-double simu_counter_left_th, simu_counter_right_th;
+/** Full encoder values. */
+uint32_t simu_encoder_left, simu_encoder_right,
+ simu_encoder_aux[AC_ASSERV_AUX_NB];
+double simu_encoder_left_th, simu_encoder_right_th;
/** Use mex. */
int simu_mex;
@@ -307,13 +285,13 @@ simu_step (void)
int i;
double old_left_th, old_right_th, old_aux_th[AC_ASSERV_AUX_NB];
/* Convert pwm value into voltage. */
- 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));
+ simu_left_model.u = simu_robot->u_max
+ * ((double) output_left.cur / (OUTPUT_MAX + 1));
+ simu_right_model.u = simu_robot->u_max
+ * ((double) output_right.cur / (OUTPUT_MAX + 1));
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. */
old_left_th = simu_left_model.th;
old_right_th = simu_right_model.th;
@@ -358,14 +336,14 @@ simu_step (void)
old_out = out;
}
}
- /* Modify counters. */
- uint32_t counter_left_new;
- uint32_t counter_right_new;
+ /* Modify encoders. */
+ uint32_t encoder_left_new;
+ uint32_t encoder_right_new;
if (!simu_robot->encoder_separated)
{
- counter_left_new = simu_left_model.th / (2*M_PI)
+ encoder_left_new = simu_left_model.th / (2*M_PI)
* simu_robot->main_encoder_steps;
- counter_right_new = simu_right_model.th / (2*M_PI)
+ encoder_right_new = simu_right_model.th / (2*M_PI)
* simu_robot->main_encoder_steps;
}
else
@@ -380,40 +358,36 @@ simu_step (void)
* (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_encoder_left_th += left_enc_diff / simu_robot->encoder_wheel_r;
+ simu_encoder_right_th += right_enc_diff / simu_robot->encoder_wheel_r;
+ encoder_left_new = simu_encoder_left_th / (2*M_PI)
* simu_robot->main_encoder_steps;
- counter_right_new = simu_counter_right_th / (2*M_PI)
+ encoder_right_new = simu_encoder_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 auxiliary counter. */
+ /* Update an integer encoder. */
+ encoder_left.diff = encoder_left_new - simu_encoder_left;
+ encoder_left.cur += encoder_left.diff;
+ simu_encoder_left = encoder_left_new;
+ encoder_right.diff = encoder_right_new - simu_encoder_right;
+ encoder_right.cur += encoder_right.diff;
+ simu_encoder_right = encoder_right_new;
+ /* 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. */
@@ -448,18 +422,18 @@ simu_send (void)
simu_pos_a_sent = simu_pos_a_to_send;
}
/* Send PWM. */
- static int16_t pwm_left_sent, pwm_right_sent;
+ static int16_t output_left_sent, output_right_sent;
if (first
- || pwm_left_sent == pwm_left.cur
- || pwm_right_sent == pwm_right.cur)
+ || output_left_sent == output_left.cur
+ || output_right_sent == output_right.cur) // BUG?
{
m = mex_msg_new (simu_mex_pwm);
- mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur);
+ mex_msg_push (m, "hh", output_left.cur, output_right.cur);
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- mex_msg_push (m, "h", pwm_aux[i].cur);
+ mex_msg_push (m, "h", output_aux[i].cur);
mex_node_send (m);
- pwm_left_sent = pwm_left.cur;
- pwm_right_sent = pwm_right.cur;
+ output_left_sent = output_left.cur;
+ output_right_sent = output_right.cur;
}
/* Send Aux position. */
static int32_t simu_aux_model_sent[AC_ASSERV_AUX_NB];
@@ -542,30 +516,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)
{
@@ -581,9 +531,3 @@ eeprom_clear_params (void)
{
}
-void
-pwm_set_reverse (uint8_t reverse)
-{
- pwm_reverse = reverse;
-}
-
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
deleted file mode 100644
index 32102177..00000000
--- a/digital/asserv/src/asserv/speed.c
+++ /dev/null
@@ -1,165 +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.
- */
-
-/** Theta/alpha speed control states. */
-struct speed_t speed_theta, speed_alpha;
-
-/** Auxiliaries speed control states. */
-struct speed_t speed_aux[AC_ASSERV_AUX_NB];
-
-/** Initialise speed control states. */
-void
-speed_init (void)
-{
- uint8_t i;
- speed_theta.pos = &pos_theta;
- speed_alpha.pos = &pos_alpha;
- 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 two motors system. */
-static void
-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 (speed0);
- speed_update_by (speed1);
- /* Check for completion. */
- if (state->mode == MODE_SPEED
- && (speed0->use_pos || speed1->use_pos)
- && (!speed0->use_pos || speed0->cur == 0)
- && (!speed1->use_pos || speed1->cur == 0))
- {
- state_finish (state);
- }
- }
-}
-
-/** 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;
- speed_update_double (&state_main, &speed_theta, &speed_alpha);
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- speed_update_single (&state_aux[i], &speed_aux[i]);
-}
-
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
deleted file mode 100644
index 66ad53e2..00000000
--- a/digital/asserv/src/asserv/speed.h
+++ /dev/null
@@ -1,58 +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_theta, speed_alpha;
-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/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h
deleted file mode 100644
index 2f0cc538..00000000
--- a/digital/asserv/src/asserv/state.h
+++ /dev/null
@@ -1,127 +0,0 @@
-#ifndef state_h
-#define state_h
-/* state.h */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-
-/** There is two mechanism to acknowledge long lived command completion.
- *
- * The first one is dedicated to unreliable and full duplex channels like the
- * serial port. It is based on sequence numbers.
- *
- * The second one is simpler, but can only be used with reliable and half
- * duplex channels like the TWI bus. It just use a flag to remember command
- * completion witch is reseted when a new command arrives. */
-
-/** Control mode. */
-enum state_mode_t
-{
- /** Simple PWM setup mode. */
- MODE_PWM,
- /** Position control mode. */
- MODE_POS,
- /** Speed control mode. */
- MODE_SPEED,
- /** Trajectory control mode. */
- MODE_TRAJ,
-};
-
-/** Current motor state. */
-struct state_t
-{
- /** Current motor control mode. */
- uint8_t mode;
- /** Control mode variant.
- * Used for main motors:
- * - bit 0: disable theta position control.
- * - bit 1: disable alpha position control.
- * - bit 2: disable blocking detection. */
- uint8_t variant;
- /** Sequence number of the currently processed command, should be between
- * 1 and 127. When a command is received on the serial port it is ignored
- * if its sequence number is equal to the current sequence number. In
- * this case a duplicated message is inferred. */
- uint8_t sequence;
- /** Sequence number of the most recently finished command. When a command
- * is finished, the current sequence number is copied to this variable. */
- uint8_t sequence_finish;
- /** Sequence number of the most recently acknowledged command. Until this
- * is not equal to the last finished command sequence number, a message is
- * generated on the serial line. */
- uint8_t sequence_ack;
- /** Simpler flag based mechanism, indicates if the last received command
- * is finished. */
- uint8_t finished;
- /** Wether the motor is blocked. */
- uint8_t blocked;
-};
-
-/** Main motors state. */
-extern struct state_t state_main;
-
-/** Auxiliary motor states. */
-extern struct state_t state_aux[AC_ASSERV_AUX_NB];
-
-/** Start a new command execution. */
-static inline void
-state_start (struct state_t *motor, uint8_t mode, uint8_t sequence)
-{
- motor->mode = mode;
- motor->variant = 0;
- motor->sequence = sequence;
- motor->finished = 0;
- motor->blocked = 0;
-}
-
-/** Signal the current command completion. */
-static inline void
-state_finish (struct state_t *motor)
-{
- motor->sequence_finish = motor->sequence;
- motor->finished = 1;
-}
-
-/** Signal the current command is blocked, disable motor control until a new
- * command is given. */
-static inline void
-state_blocked (struct state_t *motor)
-{
- motor->sequence_finish = motor->sequence | 0x80;
- motor->blocked = 1;
- motor->mode = MODE_PWM;
-}
-
-/** Acknowledge a command completion and blocked state. */
-static inline void
-state_acknowledge (struct state_t *motor, uint8_t sequence)
-{
- motor->sequence_ack = sequence;
- if (sequence == motor->sequence_finish)
- {
- motor->finished = 0;
- motor->blocked = 0;
- }
-}
-
-#endif /* state_h */
diff --git a/digital/asserv/src/asserv/test_counter.c b/digital/asserv/src/asserv/test_counter.c
deleted file mode 100644
index 548cc3ee..00000000
--- a/digital/asserv/src/asserv/test_counter.c
+++ /dev/null
@@ -1,133 +0,0 @@
-/* test_counter.c */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2008 Nicolas Schodet
- *
- * APBTeam:
- * Web: http://apbteam.org/
- * Email: team AT apbteam DOT org
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- *
- * }}} */
-#include "common.h"
-#include "modules/utils/utils.h"
-#include "modules/uart/uart.h"
-#include "modules/proto/proto.h"
-
-#include "timer.h"
-#include "misc.h"
-
-#include "counter_ext.avr.c"
-
-uint8_t read, read_cpt, read_mode;
-uint8_t ind, ind_cpt, ind_init;
-uint8_t count, count_cpt;
-
-int
-main (void)
-{
- uint8_t read_old = 0;
- uint8_t old_ind = 0;
- const int total = 5000;
- LED_SETUP;
- timer_init ();
- counter_init ();
- uart0_init ();
- proto_send0 ('z');
- sei ();
- while (1)
- {
- timer_wait ();
- if (count)
- counter_update ();
- if (read && !--read_cpt)
- {
- uint8_t r0, r1, r2, r3;
- r0 = counter_read (0);
- r1 = counter_read (1);
- r2 = counter_read (2);
- r3 = counter_read (3);
- if (read_mode == 0 || (read_mode == 1 && r3 != read_old)
- || (read_mode == 2
- && (r0 == 0 || r1 == 0 || r2 == 0 || r3 == 0)))
- {
- proto_send4b ('r', r0, r1, r2, r3);
- read_old = r3;
- }
- read_cpt = read;
- }
- if (ind && !--ind_cpt)
- {
- uint8_t i = counter_read (3);
- if (!ind_init && i != old_ind)
- {
- uint8_t eip = old_ind + total;
- uint8_t eim = old_ind - total;
- proto_send7b ('i', old_ind, i, eip, eim, i - eip, i - eim,
- i == eip || i == eim);
- }
- old_ind = i;
- ind_init = 0;
- ind_cpt = ind;
- }
- if (count && !--count_cpt)
- {
- proto_send4w ('C', counter_left, counter_right,
- counter_aux[0], counter_aux[1]);
- count_cpt = count;
- }
- while (uart0_poll ())
- proto_accept (uart0_getc ());
- }
-}
-
-/** Handle incoming messages. */
-void
-proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
-{
-#define c(cmd, size) (cmd << 8 | size)
- switch (c (cmd, size))
- {
- case c ('z', 0):
- /* Reset. */
- utils_reset ();
- break;
- case c ('r', 1):
- read_cpt = read = args[0];
- read_mode = 0;
- break;
- case c ('R', 1):
- read_cpt = read = args[0];
- read_mode = 1;
- break;
- case c ('Z', 1):
- read_cpt = read = args[0];
- read_mode = 2;
- break;
- case c ('i', 1):
- ind_cpt = ind = args[0];
- ind_init = 1;
- break;
- case c ('C', 1):
- count_cpt = count = args[0];
- break;
- default:
- proto_send0 ('?');
- return;
- }
- proto_send (cmd, size, args);
-#undef c
-}
diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c
deleted file mode 100644
index 044bab2e..00000000
--- a/digital/asserv/src/asserv/test_motor_model.c
+++ /dev/null
@@ -1,92 +0,0 @@
-/* test_motor_model.c - Test 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"
-#include "models.host.h"
-
-#include <stdio.h>
-
-void simu (struct motor_t *m, double t)
-{
- int i, s;
- s = t / m->h;
- for (i = 0; i < s; i++)
- {
- motor_model_step (m);
- printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th);
- }
-}
-
-int
-main (int argc, char **argv)
-{
- struct motor_t ms;
- struct motor_t *m;
- const struct robot_t *mr;
- /* Check arguments. */
- if (argc != 2)
- {
- fprintf (stderr, "syntax: %s MODEL\n", argv[0]);
- return 1;
- }
- mr = models_get (argv[1]);
- if (!mr)
- {
- fprintf (stderr, "model unknown\n");
- return 1;
- }
- models_init (mr, &ms, NULL, NULL);
- m = &ms;
- /* Make a step response simulation. */
- printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta");
- m->u = m->m.u_max;
- printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th);
- simu (m, 1.0);
- m->u = 0.0;
- simu (m, 1.0);
- return 0;
-}
-
-void
-simu_sensor_update_giboulee (void)
-{
-}
-
-void
-simu_sensor_update_aquajim (void)
-{
-}
-
-void
-simu_sensor_update_marcel (void)
-{
-}
-
-int
-simu_table_test_robospierre (double p_x, double p_y)
-{
- return 0;
-}
-
diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c
index eec4414a..cce154f4 100644
--- a/digital/asserv/src/asserv/timer.avr.c
+++ b/digital/asserv/src/asserv/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/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index e147ad5d..67215514 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -32,12 +32,8 @@
#include <math.h>
-#include "state.h"
-
-#include "pos.h"
-#include "speed.h"
+#include "cs.h"
#include "postrack.h"
-#include "pwm.h"
#include "contacts.h"
@@ -56,6 +52,8 @@
/** Traj mode enum. */
enum
{
+ /* Detect end of speed controled position control. */
+ TRAJ_SPEED,
/* Go to the wall. */
TRAJ_FTW,
/* Go to the dispenser. */
@@ -112,18 +110,39 @@ traj_init (void)
traj_set_angle_limit (traj_angle_limit);
}
-/** Angle offset. Directly handled to speed layer. */
+/** Wait for zero speed mode. */
+void
+traj_speed (void)
+{
+ if ((!cs_main.speed_theta.use_pos
+ || cs_main.speed_theta.pos_cons == cs_main.pos_theta.cons)
+ && (!cs_main.speed_alpha.use_pos
+ || cs_main.speed_alpha.pos_cons == cs_main.pos_alpha.cons))
+ {
+ control_state_finished (&cs_main.state);
+ traj_mode = TRAJ_DONE;
+ }
+}
+
+/** Start speed mode. */
void
-traj_angle_offset_start (int32_t angle, uint8_t seq)
+traj_speed_start (void)
+{
+ /* Speed setup should have been done yet. */
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
+ traj_mode = TRAJ_SPEED;
+}
+
+/** Angle offset. */
+void
+traj_angle_offset_start (int32_t angle)
{
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);
+ speed_control_set_speed (&cs_main.speed_theta, 0);
+ speed_control_pos_offset (&cs_main.speed_alpha, arc);
+ traj_speed_start ();
}
/** Go to the wall mode. */
@@ -131,9 +150,8 @@ static void
traj_ftw (void)
{
uint8_t left, center, right;
- int16_t speed;
- speed = speed_theta.slow;
- speed *= 256;
+ int8_t speed;
+ speed = cs_main.speed_theta.slow;
if (!traj_backward)
{
left = !IO_GET (CONTACT_FRONT_LEFT_IO);
@@ -156,64 +174,56 @@ traj_ftw (void)
traj_center_delay--;
}
}
- speed_theta.use_pos = speed_alpha.use_pos = 0;
- speed_theta.cons = speed;
- speed_alpha.cons = 0;
- state_main.variant = 0;
+ speed_control_set_speed (&cs_main.speed_theta, speed);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
if (!left && !right)
{
/* Backward. */
-#ifndef HOST
- /* No angular control. */
- state_main.variant = 2;
-#endif
}
else if (!center && (!left || !right))
{
-#ifndef HOST
- /* No angular control. */
- state_main.variant = 2;
-#else
+#ifdef HOST
/* 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;
+ speed_control_set_speed (&cs_main.speed_theta, speed / 4);
+ speed_control_set_speed (&cs_main.speed_alpha,
+ left ? speed / 2 : -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);
+ speed_control_hard_stop (&cs_main.speed_theta);
+ speed_control_hard_stop (&cs_main.speed_alpha);
+ control_state_finished (&cs_main.state);
traj_mode = TRAJ_DONE;
}
}
/** Start go to the wall mode. */
void
-traj_ftw_start (uint8_t backward, uint8_t seq)
+traj_ftw_start (uint8_t backward)
{
traj_mode = TRAJ_FTW;
traj_backward = backward;
traj_use_center = 0;
- state_start (&state_main, MODE_TRAJ, seq);
+#ifndef HOST
+ /* No angular control. */
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_POS_CONTROL_ALPHA);
+#else
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
+#endif
}
/** Start go to the wall mode, with center sensor. */
void
-traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq)
+traj_ftw_start_center (uint8_t backward, uint8_t center_delay)
{
traj_mode = TRAJ_FTW;
traj_backward = backward;
traj_use_center = 1;
traj_center_delay = center_delay;
- state_start (&state_main, MODE_TRAJ, seq);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Push the wall mode. */
@@ -221,7 +231,7 @@ static void
traj_ptw (void)
{
/* If blocking, the wall was found. */
- if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit)
+ if (cs_main.blocking_detection_theta.blocked)
{
/* Initialise position. */
if (traj_init_x != -1)
@@ -231,13 +241,10 @@ traj_ptw (void)
if (traj_init_a != -1)
postrack_a = traj_init_a;
/* Stop motor control. */
- pos_reset (&pos_theta);
- pos_reset (&pos_alpha);
- state_main.variant = 0;
- state_main.mode = MODE_PWM;
- pwm_set (&pwm_left, 0);
- pwm_set (&pwm_right, 0);
- state_finish (&state_main);
+ output_set (cs_main.output_left, 0);
+ output_set (cs_main.output_right, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
+ control_state_finished (&cs_main.state);
traj_mode = TRAJ_DONE;
}
}
@@ -245,59 +252,52 @@ traj_ptw (void)
/** Start push the wall mode. Position is initialised unless -1. */
void
traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y,
- int32_t init_a, uint8_t seq)
+ int32_t init_a)
{
- int16_t speed;
+ int8_t speed;
traj_mode = TRAJ_PTW;
traj_init_x = init_x;
traj_init_y = init_y;
traj_init_a = init_a;
- state_start (&state_main, MODE_TRAJ, seq);
/* Use slow speed, without alpha control. */
- speed = speed_theta.slow;
- speed *= 256;
+ speed = cs_main.speed_theta.slow;
if (backward)
speed = -speed;
- speed_theta.use_pos = speed_alpha.use_pos = 0;
- speed_theta.cons = speed;
- speed_alpha.cons = 0;
- state_main.variant = 2;
+ speed_control_set_speed (&cs_main.speed_theta, speed);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_POS_CONTROL_ALPHA
+ | CS_MODE_BLOCKING_DETECTION);
}
/** Go to the dispenser mode. */
static void
traj_gtd (void)
{
- int16_t speed;
- speed = speed_theta.slow;
- speed *= 256;
- speed_theta.use_pos = speed_alpha.use_pos = 0;
+ int8_t speed;
+ speed = cs_main.speed_theta.slow;
if (IO_GET (CONTACT_CENTER_IO))
{
- speed_theta.cons = speed;
- speed_alpha.cons = 0;
+ speed_control_set_speed (&cs_main.speed_theta, speed);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
}
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_main.variant = 0;
- state_finish (&state_main);
+ speed_control_hard_stop (&cs_main.speed_theta);
+ speed_control_hard_stop (&cs_main.speed_alpha);
+ control_state_finished (&cs_main.state);
traj_mode = TRAJ_DONE;
}
}
/** Start go to the dispenser mode. */
void
-traj_gtd_start (uint8_t seq)
+traj_gtd_start (void)
{
traj_mode = TRAJ_GTD;
- state_start (&state_main, MODE_TRAJ, seq);
- state_main.variant = 2;
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_POS_CONTROL_ALPHA);
}
/** Go to position mode. */
@@ -310,8 +310,7 @@ traj_goto (void)
&& 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;
+ traj_mode = TRAJ_SPEED;
}
else
{
@@ -338,32 +337,30 @@ traj_goto (void)
}
int32_t arc = fixed_mul_f824 (arad, postrack_footing);
/* Compute consign. */
- speed_alpha.pos_cons = pos_alpha.cur;
- speed_alpha.pos_cons += arc;
+ speed_control_pos_offset_from_here (&cs_main.speed_alpha, arc);
if (UTILS_ABS (arad) < traj_angle_limit_rad)
{
- speed_theta.pos_cons = pos_theta.cur;
- speed_theta.pos_cons += dt >> 8;
+ speed_control_pos_offset_from_here (&cs_main.speed_theta,
+ dt >> 8);
}
else
{
- speed_theta.pos_cons = pos_theta.cons;
+ speed_control_set_speed (&cs_main.speed_theta, 0);
}
}
}
/** 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_goto_start (uint32_t x, uint32_t y, uint8_t backward)
{
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);
+ speed_control_pos_offset (&cs_main.speed_theta, 0);
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Go to angle mode. */
@@ -375,8 +372,7 @@ traj_goto_angle (void)
if (UTILS_ABS (da) < traj_aeps)
{
/* Near enough, stop, let speed terminate the movement. */
- state_main.mode = MODE_SPEED;
- traj_mode = TRAJ_DONE;
+ traj_mode = TRAJ_SPEED;
}
else
{
@@ -385,21 +381,19 @@ traj_goto_angle (void)
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;
+ speed_control_pos_offset_from_here (&cs_main.speed_alpha, arc);
}
}
/** Start go to angle mode (a: f8.24). */
void
-traj_goto_angle_start (uint32_t a, uint8_t seq)
+traj_goto_angle_start (uint32_t a)
{
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);
+ speed_control_pos_offset (&cs_main.speed_theta, 0);
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Go to position, then angle mode. */
@@ -423,46 +417,50 @@ traj_goto_xya (void)
/** 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_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward)
{
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);
+ speed_control_pos_offset (&cs_main.speed_theta, 0);
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
}
/* Compute new speed according the defined trajectory. */
void
traj_update (void)
{
- switch (traj_mode)
+ if (cs_main.state.modes & CS_MODE_TRAJ_CONTROL)
{
- case TRAJ_FTW:
- traj_ftw ();
- break;
- case TRAJ_PTW:
- traj_ptw ();
- break;
- case TRAJ_GTD:
- traj_gtd ();
- 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;
+ switch (traj_mode)
+ {
+ case TRAJ_SPEED:
+ traj_speed ();
+ break;
+ case TRAJ_FTW:
+ traj_ftw ();
+ break;
+ case TRAJ_PTW:
+ traj_ptw ();
+ break;
+ case TRAJ_GTD:
+ traj_gtd ();
+ 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;
+ }
}
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index ec345d0c..a2f36cda 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -40,30 +40,32 @@ void
traj_update (void);
void
-traj_angle_offset_start (int32_t angle, uint8_t seq);
+traj_speed_start (void);
void
-traj_ftw_start (uint8_t backward, uint8_t seq);
+traj_angle_offset_start (int32_t angle);
void
-traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq);
+traj_ftw_start (uint8_t backward);
+
+void
+traj_ftw_start_center (uint8_t backward, uint8_t center_delay);
void
traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y,
- int32_t init_a, uint8_t seq);
+ int32_t init_a);
void
-traj_gtd_start (uint8_t seq);
+traj_gtd_start (void);
void
-traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq);
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward);
void
-traj_goto_angle_start (uint32_t a, uint8_t seq);
+traj_goto_angle_start (uint32_t a);
void
-traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
- uint8_t seq);
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward);
void
traj_set_angle_limit (uint16_t a);
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index fcf0a9cc..0363d68f 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -32,11 +32,7 @@
#include "io.h"
#include "misc.h"
-#include "state.h"
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
#include "postrack.h"
#include "traj.h"
#include "aux.h"
@@ -79,14 +75,14 @@ twi_proto_update (void)
u8 status_with_crc[16];
u8 *status = &status_with_crc[1];
status[0] = 0
- | (state_aux[1].blocked << 7)
- | (state_aux[1].finished << 6)
- | (state_aux[0].blocked << 5)
- | (state_aux[0].finished << 4)
- | (speed_theta.cur < 0 ? (1 << 3) : 0)
- | (speed_theta.cur > 0 ? (1 << 2) : 0)
- | (state_main.blocked << 1)
- | (state_main.finished << 0);
+ | (control_state_is_blocked (&cs_aux[1].state) ? (1 << 7) : 0)
+ | (control_state_is_finished (&cs_aux[1].state) ? (1 << 6) : 0)
+ | (control_state_is_blocked (&cs_aux[0].state) ? (1 << 5) : 0)
+ | (control_state_is_finished (&cs_aux[0].state) ? (1 << 4) : 0)
+ | (cs_main.speed_theta.cur < 0 ? (1 << 3) : 0)
+ | (cs_main.speed_theta.cur > 0 ? (1 << 2) : 0)
+ | (control_state_is_blocked (&cs_main.state) ? (1 << 1) : 0)
+ | (control_state_is_finished (&cs_main.state) ? (1 << 0) : 0);
status[1] = PINC;
status[2] = twi_proto.seq;
status[3] = v32_to_v8 (postrack_x, 3);
@@ -111,6 +107,7 @@ twi_proto_update (void)
static void
twi_proto_callback (u8 *buf, u8 size)
{
+ int32_t offset;
/* Check CRC. */
if (crc_compute (buf + 1, size - 1) != buf[0])
return;
@@ -132,41 +129,36 @@ twi_proto_callback (u8 *buf, u8 size)
break;
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);
+ output_set (&output_left, 0);
+ output_set (&output_right, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
break;
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;
+ speed_control_set_speed (&cs_main.speed_theta, 0);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
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]);
+ offset = v8_to_v32 (0xff, buf[2], buf[3], buf[4]);
else
- 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);
+ offset = v8_to_v32 (0, buf[2], buf[3], buf[4]);
+ speed_control_pos_offset (&cs_main.speed_theta, offset);
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ traj_speed_start ();
break;
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);
+ traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8);
break;
case c ('f', 1):
/* Go to the wall.
* - b: 0: forward, 1: backward. */
- traj_ftw_start (buf[2], 0);
+ traj_ftw_start (buf[2]);
break;
case c ('G', 9):
/* Push the wall.
@@ -183,18 +175,18 @@ twi_proto_callback (u8 *buf, u8 size)
traj_ptw_start (buf[2],
v8_to_v32 (buf[3], buf[4], buf[5], 0xff),
v8_to_v32 (buf[6], buf[7], buf[8], 0xff),
- angle, 0);
+ angle);
}
break;
case c ('g', 2):
/* Go to the wall using center sensor with delay.
* - b: 0: forward, 1: backward.
* - b: delay. */
- traj_ftw_start_center (buf[2], buf[3], 0);
+ traj_ftw_start_center (buf[2], buf[3]);
break;
case c ('F', 0):
/* Go to the dispenser. */
- traj_gtd_start (0);
+ traj_gtd_start ();
break;
case c ('x', 7):
/* Go to position.
@@ -203,12 +195,12 @@ twi_proto_callback (u8 *buf, u8 size)
* - 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);
+ buf[8]);
break;
case c ('y', 2):
/* Go to angle.
* - w: angle. */
- traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
+ traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0));
break;
case c ('X', 9):
/* Go to position, then angle.
@@ -219,31 +211,31 @@ twi_proto_callback (u8 *buf, u8 size)
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);
+ buf[10]);
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);
+ cs_aux[0].speed.max = buf[4];
+ aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]));
break;
case c ('B', 1):
/* Find the aux0 zero position.
* - b: speed. */
- aux_traj_find_limit_start (&aux[0], buf[2], 0);
+ aux_traj_find_limit_start (&aux[0], buf[2]);
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 ('C', 1):
/* Find the aux1 zero position.
* - b: speed. */
- aux_traj_find_zero_reverse_start (&aux[1], buf[2], 0);
+ aux_traj_find_zero_reverse_start (&aux[1], buf[2]);
break;
case c ('r', 1):
/* Set aux zero pwm.
@@ -251,9 +243,8 @@ twi_proto_callback (u8 *buf, u8 size)
*/
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;
@@ -313,10 +304,10 @@ twi_proto_params (u8 *buf, u8 size)
* - 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];
+ cs_main.speed_theta.max = buf[0];
+ cs_main.speed_alpha.max = buf[1];
+ cs_main.speed_theta.slow = buf[2];
+ cs_main.speed_alpha.slow = buf[3];
eat = 4;
break;
default: