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