From 4895e54c666db9720abda0aa8ae901058679aee3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 18 Dec 2011 15:19:06 +0100 Subject: digital/asserv: convert to new control system --- digital/asserv/src/asserv/Makefile | 18 +- digital/asserv/src/asserv/aux.c | 120 ++++----- digital/asserv/src/asserv/aux.h | 20 +- digital/asserv/src/asserv/avrconfig.h | 33 +++ digital/asserv/src/asserv/counter.h | 43 ---- digital/asserv/src/asserv/counter_ext.avr.c | 229 ----------------- digital/asserv/src/asserv/counter_tcc.avr.c | 169 ------------- digital/asserv/src/asserv/cs.c | 84 +++++++ digital/asserv/src/asserv/cs.h | 46 ++++ digital/asserv/src/asserv/eeprom.avr.c | 194 ++++++++------- digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 353 ++++++++++++++------------- digital/asserv/src/asserv/models.host.c | 233 ++++-------------- digital/asserv/src/asserv/models.host.h | 16 +- digital/asserv/src/asserv/motor_model.host.c | 100 -------- digital/asserv/src/asserv/motor_model.host.h | 68 ------ digital/asserv/src/asserv/pos.c | 208 ---------------- digital/asserv/src/asserv/pos.h | 62 ----- digital/asserv/src/asserv/postrack.c | 8 +- digital/asserv/src/asserv/pwm.avr.c | 62 ----- digital/asserv/src/asserv/pwm.h | 108 -------- digital/asserv/src/asserv/pwm_config.h | 52 ---- digital/asserv/src/asserv/pwm_mp.avr.c | 126 ---------- digital/asserv/src/asserv/pwm_mp.avr.h | 34 --- digital/asserv/src/asserv/pwm_ocr.avr.c | 221 ----------------- digital/asserv/src/asserv/pwm_ocr.avr.h | 37 --- digital/asserv/src/asserv/seq.c | 31 +++ digital/asserv/src/asserv/seq.h | 95 +++++++ digital/asserv/src/asserv/simu.host.c | 144 ++++------- digital/asserv/src/asserv/speed.c | 165 ------------- digital/asserv/src/asserv/speed.h | 58 ----- digital/asserv/src/asserv/state.c | 31 --- digital/asserv/src/asserv/state.h | 127 ---------- digital/asserv/src/asserv/test_counter.c | 133 ---------- digital/asserv/src/asserv/test_motor_model.c | 92 ------- digital/asserv/src/asserv/timer.avr.c | 5 +- digital/asserv/src/asserv/traj.c | 244 +++++++++--------- digital/asserv/src/asserv/traj.h | 20 +- digital/asserv/src/asserv/twi_proto.c | 89 +++---- 39 files changed, 944 insertions(+), 2936 deletions(-) delete mode 100644 digital/asserv/src/asserv/counter.h delete mode 100644 digital/asserv/src/asserv/counter_ext.avr.c delete mode 100644 digital/asserv/src/asserv/counter_tcc.avr.c create mode 100644 digital/asserv/src/asserv/cs.c create mode 100644 digital/asserv/src/asserv/cs.h delete mode 100644 digital/asserv/src/asserv/motor_model.host.c delete mode 100644 digital/asserv/src/asserv/motor_model.host.h delete mode 100644 digital/asserv/src/asserv/pos.c delete mode 100644 digital/asserv/src/asserv/pos.h delete mode 100644 digital/asserv/src/asserv/pwm.avr.c delete mode 100644 digital/asserv/src/asserv/pwm.h delete mode 100644 digital/asserv/src/asserv/pwm_config.h delete mode 100644 digital/asserv/src/asserv/pwm_mp.avr.c delete mode 100644 digital/asserv/src/asserv/pwm_mp.avr.h delete mode 100644 digital/asserv/src/asserv/pwm_ocr.avr.c delete mode 100644 digital/asserv/src/asserv/pwm_ocr.avr.h create mode 100644 digital/asserv/src/asserv/seq.c create mode 100644 digital/asserv/src/asserv/seq.h delete mode 100644 digital/asserv/src/asserv/speed.c delete mode 100644 digital/asserv/src/asserv/speed.h delete mode 100644 digital/asserv/src/asserv/state.c delete mode 100644 digital/asserv/src/asserv/state.h delete mode 100644 digital/asserv/src/asserv/test_counter.c delete mode 100644 digital/asserv/src/asserv/test_motor_model.c (limited to 'digital/asserv') 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.h b/digital/asserv/src/asserv/counter.h deleted file mode 100644 index 198660e4..00000000 --- a/digital/asserv/src/asserv/counter.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef counter_h -#define counter_h -/* counter.h */ -/* asserv - Position & speed motor control on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -extern uint16_t counter_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]; - -void -counter_init (void); - -void -counter_update_step (void); - -void -counter_update (void); - -#endif /* counter_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/cs.h b/digital/asserv/src/asserv/cs.h new file mode 100644 index 00000000..5a1737ec --- /dev/null +++ b/digital/asserv/src/asserv/cs.h @@ -0,0 +1,46 @@ +#ifndef cs_h +#define cs_h +/* cs.h - Control system definition. */ +/* 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_system/control_system.h" +#include "modules/motor/encoder/encoder_corrector.h" + +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 +cs_init (void); + +void +cs_update_prepare (void); + +void +cs_update (void); + +#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 -#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/seq.c b/digital/asserv/src/asserv/seq.c new file mode 100644 index 00000000..6cbf596f --- /dev/null +++ b/digital/asserv/src/asserv/seq.c @@ -0,0 +1,31 @@ +/* seq.c */ +/* 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 "common.h" +#include "seq.h" + +seq_t seq_main; + +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 #include -#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.c b/digital/asserv/src/asserv/state.c deleted file mode 100644 index 712b445c..00000000 --- a/digital/asserv/src/asserv/state.c +++ /dev/null @@ -1,31 +0,0 @@ -/* state.c */ -/* asserv - Position & speed motor control on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "state.h" - -struct state_t state_main; - -struct state_t state_aux[AC_ASSERV_AUX_NB]; - 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 - -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 -#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: -- cgit v1.2.3