From 4cd9c4458eaa49c269171145f4bc2a184948eae1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 Apr 2011 21:26:15 +0200 Subject: digital/mimot: switch to differential drive --- digital/mimot/src/dirty/Makefile | 2 +- digital/mimot/src/dirty/aux.c | 283 ----------------------- digital/mimot/src/dirty/aux.h | 79 ------- digital/mimot/src/dirty/avrconfig.h | 2 - digital/mimot/src/dirty/contacts.h | 6 +- digital/mimot/src/dirty/counter.h | 5 +- digital/mimot/src/dirty/counter_ext.avr.c | 99 +++++---- digital/mimot/src/dirty/eeprom.avr.c | 104 ++++++--- digital/mimot/src/dirty/eeprom.h | 2 +- digital/mimot/src/dirty/main.c | 358 +++++++++++++++++++----------- digital/mimot/src/dirty/models.host.c | 76 ++++--- digital/mimot/src/dirty/models.host.h | 25 ++- digital/mimot/src/dirty/pos.c | 75 ++++--- digital/mimot/src/dirty/pos.h | 2 +- digital/mimot/src/dirty/postrack.c | 103 +++++++++ digital/mimot/src/dirty/postrack.h | 41 ++++ digital/mimot/src/dirty/pwm.avr.c | 5 +- digital/mimot/src/dirty/pwm.h | 18 +- digital/mimot/src/dirty/pwm_ocr.avr.c | 4 +- digital/mimot/src/dirty/simu.host.c | 210 ++++++++++-------- digital/mimot/src/dirty/simu.host.h | 1 + digital/mimot/src/dirty/speed.c | 27 ++- digital/mimot/src/dirty/speed.h | 2 +- digital/mimot/src/dirty/state.c | 2 +- digital/mimot/src/dirty/state.h | 4 +- digital/mimot/src/dirty/traj.c | 349 +++++++++++++++++++++++++++++ digital/mimot/src/dirty/traj.h | 61 +++++ digital/mimot/src/dirty/twi_proto.c | 151 +++++++++---- digital/mimot/tools/inter_mimot.py | 136 ++++++++++++ digital/mimot/tools/mimot/init.py | 27 +-- digital/mimot/tools/mimot/mex.py | 76 ++----- digital/mimot/tools/mimot/mimot.py | 201 +++++++++++++---- 32 files changed, 1619 insertions(+), 917 deletions(-) delete mode 100644 digital/mimot/src/dirty/aux.c delete mode 100644 digital/mimot/src/dirty/aux.h create mode 100644 digital/mimot/src/dirty/postrack.c create mode 100644 digital/mimot/src/dirty/postrack.h create mode 100644 digital/mimot/src/dirty/traj.c create mode 100644 digital/mimot/src/dirty/traj.h create mode 100644 digital/mimot/tools/inter_mimot.py diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile index 979aae7e..342c8616 100644 --- a/digital/mimot/src/dirty/Makefile +++ b/digital/mimot/src/dirty/Makefile @@ -1,7 +1,7 @@ BASE = ../../../avr PROGS = dirty dirty_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c \ - pwm_ocr.avr.c pos.c speed.c aux.c \ + pwm_ocr.avr.c pos.c speed.c postrack.c traj.c \ twi_proto.c eeprom.avr.c state.c \ simu.host.c motor_model.host.c models.host.c MODULES = proto uart utils math/fixed twi diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c deleted file mode 100644 index e00e2c08..00000000 --- a/digital/mimot/src/dirty/aux.c +++ /dev/null @@ -1,283 +0,0 @@ -/* aux.c - Auxiliary motors commands. */ -/* asserv - Position & speed motor control on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "aux.h" - -#include "modules/utils/utils.h" -#include "io.h" - -#include "state.h" - -#include "counter.h" -#include "pos.h" -#include "speed.h" -#include "pwm.h" - -#include "contacts.h" - -#ifdef HOST -# include "simu.host.h" -#endif - -/** Motors states. */ -struct aux_t aux[AC_ASSERV_AUX_NB]; - -/** Trajectory modes. */ -enum -{ - /* Goto position, with blocking detection. */ - AUX_TRAJ_GOTO, - /* Goto position, try to unblock. */ - AUX_TRAJ_GOTO_UNBLOCK, - /* Speed control, apply open loop PWM on blocking. */ - AUX_TRAJ_CLAMP, - /* Find zero mode, turn until zero is not seen. */ - AUX_TRAJ_FIND_ZERO_NOT, - /* Find zero mode, turn until zero is seen. */ - AUX_TRAJ_FIND_ZERO, - /* Find zero by forcing into limit. */ - AUX_TRAJ_FIND_LIMIT, - /* Wait for mechanical elasticity. */ - AUX_TRAJ_FIND_LIMIT_WAIT, - /* Everything done. */ - AUX_TRAJ_DONE, -}; - -/** Initialise motors states. */ -void -aux_init (void) -{ - aux[0].state = &state_aux[0]; - aux[0].speed = &speed_aux[0]; - aux[0].pwm = &pwm_aux[0]; - aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO); - aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO); - aux[0].handle_blocking = 0; - aux[1].state = &state_aux[1]; - aux[1].speed = &speed_aux[1]; - aux[1].pwm = &pwm_aux[1]; - aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO); - aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO); - aux[1].handle_blocking = 0; -} - -/** Update positions. */ -void -aux_pos_update (void) -{ - uint8_t i; - /* Easy... */ - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - aux[i].pos += counter_aux_diff[i]; -} - -/** Goto position. */ -void -aux_traj_goto (struct aux_t *aux) -{ - switch (aux->traj_mode) - { - case AUX_TRAJ_GOTO: - if (aux->speed->pos->blocked_counter - > aux->speed->pos->blocked_counter_limit) - { - aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK; - aux->speed->pos_cons = aux->speed->pos->cur; - aux->speed->pos_cons -= 250; - aux->wait = 225 / 2; - } - else if (UTILS_ABS ((int32_t) (aux->speed->pos_cons - - aux->speed->pos->cur)) < 300) - { - aux->traj_mode = AUX_TRAJ_DONE; - aux->state->variant = 0; - state_finish (aux->state); - } - break; - case AUX_TRAJ_GOTO_UNBLOCK: - if (!--aux->wait) - { - aux->traj_mode = AUX_TRAJ_GOTO; - aux->speed->pos_cons = aux->goto_pos; - } - break; - } -} - -void -aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq) -{ - aux->traj_mode = AUX_TRAJ_GOTO; - aux->speed->use_pos = 1; - aux->speed->pos_cons = aux->speed->pos->cur; - aux->speed->pos_cons += (int16_t) (pos - aux->pos); - aux->goto_pos = aux->speed->pos_cons; - state_start (aux->state, MODE_TRAJ, seq); - if (aux->handle_blocking) - aux->state->variant = 4; -} - -/** Speed control, then clamp. */ -void -aux_traj_clamp (struct aux_t *aux) -{ - /* If blocking, stop control, clamp. */ - if (aux->speed->pos->blocked_counter - > aux->speed->pos->blocked_counter_limit) - { - state_finish (aux->state); - pos_reset (aux->speed->pos); - aux->speed->cur = 0; - aux->state->mode = MODE_PWM; - pwm_set (aux->pwm, aux->clampin_pwm); - aux->traj_mode = AUX_TRAJ_DONE; - } -} - -void -aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm, - uint8_t seq) -{ - aux->traj_mode = AUX_TRAJ_CLAMP; - aux->clampin_pwm = clampin_pwm; - aux->speed->use_pos = 0; - aux->speed->cons = speed << 8; - state_start (aux->state, MODE_TRAJ, seq); - aux->state->variant = 4; -} - -/** Find zero mode. */ -void -aux_traj_find_zero (struct aux_t *aux) -{ - uint8_t zero = *aux->zero_pin & aux->zero_bv; - switch (aux->traj_mode) - { - case AUX_TRAJ_FIND_ZERO_NOT: - if (zero) - aux->traj_mode = AUX_TRAJ_FIND_ZERO; - break; - case AUX_TRAJ_FIND_ZERO: - if (!zero) - { - aux->speed->cons = 0; - state_finish (aux->state); - aux->pos = 0; - aux->traj_mode = AUX_TRAJ_DONE; - } - break; - } -} - -/** Start find zero mode. */ -void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) -{ - aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; - aux->speed->use_pos = 0; - aux->speed->cons = speed << 8; - state_start (aux->state, MODE_TRAJ, seq); -} - -/** Find limit mode. */ -void -aux_traj_find_limit (struct aux_t *aux) -{ - switch (aux->traj_mode) - { - case AUX_TRAJ_FIND_LIMIT: - /* If blocking, limit is found. */ - if (aux->speed->pos->blocked_counter - > aux->speed->pos->blocked_counter_limit) - { - pos_reset (aux->speed->pos); - aux->speed->cons = 0; - aux->speed->cur = 0; - aux->state->variant = 1; - pwm_set (aux->pwm, 0); - aux->traj_mode = AUX_TRAJ_FIND_LIMIT_WAIT; - aux->wait = 3 * 225; - } - break; - case AUX_TRAJ_FIND_LIMIT_WAIT: - if (!--aux->wait) - { - state_finish (aux->state); - aux->pos = 0; - aux->traj_mode = AUX_TRAJ_DONE; - } - break; - } -} - -/** Start find limit mode. */ -void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq) -{ - aux->traj_mode = AUX_TRAJ_FIND_LIMIT; - aux->speed->use_pos = 0; - aux->speed->cons = speed << 8; - state_start (aux->state, MODE_TRAJ, seq); - aux->state->variant = 4; -} - -/** Update trajectories for one motor. */ -static void -aux_traj_update_single (struct aux_t *aux) -{ - if (aux->state->mode >= MODE_TRAJ) - { - switch (aux->traj_mode) - { - case AUX_TRAJ_GOTO: - case AUX_TRAJ_GOTO_UNBLOCK: - aux_traj_goto (aux); - break; - case AUX_TRAJ_CLAMP: - aux_traj_clamp (aux); - break; - case AUX_TRAJ_FIND_ZERO_NOT: - case AUX_TRAJ_FIND_ZERO: - aux_traj_find_zero (aux); - break; - case AUX_TRAJ_FIND_LIMIT: - case AUX_TRAJ_FIND_LIMIT_WAIT: - aux_traj_find_limit (aux); - break; - case AUX_TRAJ_DONE: - break; - } - } -} - -/** Update trajectories. */ -void -aux_traj_update (void) -{ - uint8_t i; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - aux_traj_update_single (&aux[i]); -} - diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h deleted file mode 100644 index f31b789a..00000000 --- a/digital/mimot/src/dirty/aux.h +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef aux_h -#define aux_h -/* aux.h */ -/* asserv - Position & speed motor control on AVR. {{{ - * - * Copyright (C) 2008 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -/** Auxiliary motor informations. */ -struct aux_t -{ - /** Associated state. */ - struct state_t *state; - /** Controlled speed. */ - struct speed_t *speed; - /** Associated PWM. */ - struct pwm_t *pwm; - /** Absolute position. */ - int16_t pos; - /** Trajectory mode. */ - uint8_t traj_mode; - /** Goto position position. */ - uint32_t goto_pos; - /** Clamping PWM. */ - int16_t clampin_pwm; - /** Wait counter. */ - uint16_t wait; - /** Top zero port input register. */ - volatile uint8_t *zero_pin; - /** Top zero port bit value. */ - uint8_t zero_bv; - /** Handle blocking by aux instead of pos. */ - uint8_t handle_blocking; -}; - -extern struct aux_t aux[AC_ASSERV_AUX_NB]; - -void -aux_init (void); - -void -aux_pos_update (void); - -void -aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq); - -void -aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm, - uint8_t seq); - -void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq); - -void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq); - -void -aux_traj_update (void); - -#endif /* aux_h */ diff --git a/digital/mimot/src/dirty/avrconfig.h b/digital/mimot/src/dirty/avrconfig.h index 33ed88ec..e3c368d6 100644 --- a/digital/mimot/src/dirty/avrconfig.h +++ b/digital/mimot/src/dirty/avrconfig.h @@ -84,8 +84,6 @@ #define AC_PROTO_QUOTE 1 /* asserv. */ -/** Number of auxiliary motors. */ -#define AC_ASSERV_AUX_NB 2 /** Use external counters. */ #define AC_ASSERV_COUNTER_EXTERNAL 1 /** TWI address. */ diff --git a/digital/mimot/src/dirty/contacts.h b/digital/mimot/src/dirty/contacts.h index 1de0c57d..73b76bc8 100644 --- a/digital/mimot/src/dirty/contacts.h +++ b/digital/mimot/src/dirty/contacts.h @@ -26,7 +26,9 @@ * }}} */ /** Define contacts. */ -#define CONTACT_AUX0_ZERO_IO C, 0 -#define CONTACT_AUX1_ZERO_IO C, 1 +#define CONTACT_BACK_LEFT_IO C, 0 +#define CONTACT_BACK_RIGHT_IO C, 1 +#define CONTACT_FRONT_LEFT_IO C, 2 +#define CONTACT_FRONT_RIGHT_IO C, 3 #endif /* contacts_h */ diff --git a/digital/mimot/src/dirty/counter.h b/digital/mimot/src/dirty/counter.h index b4b4979d..111d9f67 100644 --- a/digital/mimot/src/dirty/counter.h +++ b/digital/mimot/src/dirty/counter.h @@ -25,8 +25,9 @@ * * }}} */ -extern uint16_t counter_aux[AC_ASSERV_AUX_NB]; -extern int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; +extern uint16_t counter_left, counter_right; +extern uint32_t counter_right_correction; +extern int16_t counter_left_diff, counter_right_diff; void counter_init (void); diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c index e99d9dc0..630f6cec 100644 --- a/digital/mimot/src/dirty/counter_ext.avr.c +++ b/digital/mimot/src/dirty/counter_ext.avr.c @@ -33,38 +33,44 @@ * This file add support for an external counter like the hdlcounter or * avrcounter project. This can be better in order not to loose steps and * support more counters. + * + * There is additionnal support for error correction on the right counter. */ -/** Define the first auxiliary counter address. */ -#define COUNTER_AUX0 0 -/** Define the second auxiliary counter address. */ -#define COUNTER_AUX1 1 +/** Define the left counter address. */ +#define COUNTER_LEFT 0 +/** Define the right counter address. */ +#define COUNTER_RIGHT 1 -/** Define to 1 to reverse the first auxiliary counter. */ -#define COUNTER_AUX0_REVERSE 0 -/** Define to 1 to reverse the second auxiliary counter. */ -#define COUNTER_AUX1_REVERSE 0 +/** Define to 1 to reverse the left counter. */ +#define COUNTER_LEFT_REVERSE 0 +/** Define to 1 to reverse the right counter. */ +#define COUNTER_RIGHT_REVERSE 0 -/** First auxiliary counter shift. */ -#define COUNTER_AUX0_SHIFT 1 -/** Second auxiliary counter shift. */ -#define COUNTER_AUX1_SHIFT 1 +/** Left counter shift. */ +#define COUNTER_LEFT_SHIFT 0 +/** Right counter shift. */ +#define COUNTER_RIGHT_SHIFT 0 /** Define to 1 to use the AVR External Memory system, or 0 to use hand made * signals. */ #define COUNTER_USE_XMEM 0 /** Last values. */ -static uint16_t counter_aux_old[AC_ASSERV_AUX_NB]; +static uint16_t counter_left_old, counter_right_old; /** New values, being updated by step update. */ -static int16_t counter_aux_new_step[AC_ASSERV_AUX_NB]; +static int16_t counter_left_new_step, counter_right_new_step; /** Last raw step values */ -static uint8_t counter_aux_old_step[AC_ASSERV_AUX_NB]; +static uint8_t counter_left_old_step, counter_right_old_step; /** Overall counter values. */ -uint16_t counter_aux[AC_ASSERV_AUX_NB]; +uint16_t counter_left, counter_right; +/** Overall uncorrected counter values. */ +static int32_t counter_right_raw; +/** Correction factor (f8.24). */ +uint32_t counter_right_correction = 1L << 24; /** Counter differences since last update. * Maximum of 9 significant bits, sign included. */ -int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; +int16_t counter_left_diff, counter_right_diff; #if !COUNTER_USE_XMEM # define COUNTER_ALE_IO B, 4 @@ -119,8 +125,8 @@ counter_init (void) DDRB |= 0x0f; #endif /* Begin with safe values. */ - counter_aux_old_step[0] = counter_read (COUNTER_AUX0); - counter_aux_old_step[1] = counter_read (COUNTER_AUX1); + counter_left_old_step = counter_read (COUNTER_LEFT); + counter_right_old_step = counter_read (COUNTER_RIGHT); } /** Update one step. If counters are not read fast enough, they could @@ -128,18 +134,18 @@ counter_init (void) void counter_update_step (void) { - uint8_t aux0, aux1; + uint8_t left, right; int8_t diff; /* Sample counters. */ - aux0 = counter_read (COUNTER_AUX0); - aux1 = counter_read (COUNTER_AUX1); + left = counter_read (COUNTER_LEFT); + right = counter_read (COUNTER_RIGHT); /* Update step counters. */ - diff = (int8_t) (aux0 - counter_aux_old_step[0]); - counter_aux_old_step[0] = aux0; - counter_aux_new_step[0] += diff; - diff = (int8_t) (aux1 - counter_aux_old_step[1]); - counter_aux_old_step[1] = aux1; - counter_aux_new_step[1] += diff; + diff = (int8_t) (left - counter_left_old_step); + counter_left_old_step = left; + counter_left_new_step += diff; + diff = (int8_t) (right - counter_right_old_step); + counter_right_old_step = right; + counter_right_new_step += diff; } /** Update overall counter values and compute diffs. */ @@ -148,25 +154,30 @@ counter_update (void) { /* Wants fresh data. */ counter_update_step (); - /* First auxiliary counter. */ - uint16_t aux0 = counter_aux_new_step[0]; -#if !COUNTER_AUX0_REVERSE - counter_aux_diff[0] = (int16_t) (aux0 - counter_aux_old[0]); + /* Left counter. */ + uint16_t left = counter_left_new_step; +#if !COUNTER_LEFT_REVERSE + counter_left_diff = (int16_t) (left - counter_left_old); #else - counter_aux_diff[0] = (int16_t) (counter_aux_old[0] - aux0); + counter_left_diff = (int16_t) (counter_left_old - left); #endif - counter_aux_diff[0] >>= COUNTER_AUX0_SHIFT; - counter_aux_old[0] = aux0; - counter_aux[0] += counter_aux_diff[0]; - /* Second auxiliary counter. */ - uint16_t aux1 = counter_aux_new_step[1]; -#if !COUNTER_AUX1_REVERSE - counter_aux_diff[1] = (int16_t) (aux1 - counter_aux_old[1]); + counter_left_diff >>= COUNTER_LEFT_SHIFT; + counter_left_old = left; + counter_left += counter_left_diff; + /* Right counter. */ + uint16_t right = counter_right_new_step; +#if !COUNTER_RIGHT_REVERSE + counter_right_diff = (int16_t) (right - counter_right_old); #else - counter_aux_diff[1] = (int16_t) (counter_aux_old[1] - aux1); + counter_right_diff = (int16_t) (counter_right_old - right); #endif - counter_aux_diff[1] >>= COUNTER_AUX1_SHIFT; - counter_aux_old[1] = aux1; - counter_aux[1] += counter_aux_diff[1]; + counter_right_diff >>= COUNTER_RIGHT_SHIFT; + counter_right_old = right; + /* Fix right counter. */ + counter_right_raw += counter_right_diff; + uint16_t right_new = fixed_mul_f824 (counter_right_raw, + counter_right_correction); + counter_right_diff = (int16_t) (right_new - counter_right); + counter_right = right_new; } diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c index a20032d1..fb11dc9c 100644 --- a/digital/mimot/src/dirty/eeprom.avr.c +++ b/digital/mimot/src/dirty/eeprom.avr.c @@ -33,6 +33,8 @@ #include "pwm.h" #include "pos.h" #include "speed.h" +#include "postrack.h" +#include "traj.h" #define EEPROM_START 0 @@ -41,6 +43,26 @@ * your new format is not compatible with the old one or you will load * garbages in parameters. */ +static uint32_t +eeprom_read_32 (uint8_t *addr) +{ + uint8_t dw[4]; + dw[0] = eeprom_read_byte (addr++); + dw[1] = eeprom_read_byte (addr++); + dw[2] = eeprom_read_byte (addr++); + dw[3] = eeprom_read_byte (addr++); + return v8_to_v32 (dw[3], dw[2], dw[1], dw[0]); +} + +static void +eeprom_write_32 (uint8_t *addr, uint32_t dw) +{ + eeprom_write_byte (addr++, v32_to_v8 (dw, 0)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 1)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 2)); + eeprom_write_byte (addr++, v32_to_v8 (dw, 3)); +} + /* Read parameters from eeprom. */ void eeprom_read_params (void) @@ -49,29 +71,34 @@ eeprom_read_params (void) uint16_t *p16; if (eeprom_read_byte (p8++) != EEPROM_KEY) return; - speed_aux[0].max = eeprom_read_byte (p8++); - speed_aux[1].max = eeprom_read_byte (p8++); - speed_aux[0].slow = eeprom_read_byte (p8++); - speed_aux[1].slow = eeprom_read_byte (p8++); + speed_theta.max = eeprom_read_byte (p8++); + speed_alpha.max = eeprom_read_byte (p8++); + speed_theta.slow = eeprom_read_byte (p8++); + speed_alpha.slow = eeprom_read_byte (p8++); pwm_set_reverse (eeprom_read_byte (p8++)); + counter_right_correction = eeprom_read_32 (p8); p8 += 4; p16 = (uint16_t *) p8; - speed_aux[0].acc = eeprom_read_word (p16++); - speed_aux[1].acc = eeprom_read_word (p16++); - pos_aux[0].kp = eeprom_read_word (p16++); - pos_aux[1].kp = eeprom_read_word (p16++); - pos_aux[0].ki = eeprom_read_word (p16++); - pos_aux[1].ki = eeprom_read_word (p16++); - pos_aux[0].kd = eeprom_read_word (p16++); - pos_aux[1].kd = eeprom_read_word (p16++); - pos_aux[0].blocked_error_limit = eeprom_read_word (p16++); - pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++); - pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_error_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++); + postrack_set_footing (eeprom_read_word (p16++)); + speed_theta.acc = eeprom_read_word (p16++); + speed_alpha.acc = eeprom_read_word (p16++); + pos_theta.kp = eeprom_read_word (p16++); + pos_alpha.kp = eeprom_read_word (p16++); + pos_theta.ki = eeprom_read_word (p16++); + pos_alpha.ki = eeprom_read_word (p16++); + pos_theta.kd = eeprom_read_word (p16++); + pos_alpha.kd = eeprom_read_word (p16++); + pos_theta.blocked_error_limit = eeprom_read_word (p16++); + pos_theta.blocked_speed_limit = eeprom_read_word (p16++); + pos_theta.blocked_counter_limit = eeprom_read_word (p16++); + pos_alpha.blocked_error_limit = eeprom_read_word (p16++); + pos_alpha.blocked_speed_limit = eeprom_read_word (p16++); + pos_alpha.blocked_counter_limit = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); pos_i_sat = eeprom_read_word (p16++); pos_d_sat = eeprom_read_word (p16++); + traj_eps = eeprom_read_word (p16++); + traj_aeps = eeprom_read_word (p16++); + traj_set_angle_limit (eeprom_read_word (p16++)); } /* Write parameters to eeprom. */ @@ -81,29 +108,34 @@ eeprom_write_params (void) uint8_t *p8 = (uint8_t *) EEPROM_START; uint16_t *p16; eeprom_write_byte (p8++, EEPROM_KEY); - eeprom_write_byte (p8++, speed_aux[0].max); - eeprom_write_byte (p8++, speed_aux[1].max); - eeprom_write_byte (p8++, speed_aux[0].slow); - eeprom_write_byte (p8++, speed_aux[1].slow); + eeprom_write_byte (p8++, speed_theta.max); + eeprom_write_byte (p8++, speed_alpha.max); + eeprom_write_byte (p8++, speed_theta.slow); + eeprom_write_byte (p8++, speed_alpha.slow); eeprom_write_byte (p8++, pwm_reverse); + eeprom_write_32 (p8, counter_right_correction); p8 += 4; p16 = (uint16_t *) p8; - eeprom_write_word (p16++, speed_aux[0].acc); - eeprom_write_word (p16++, speed_aux[1].acc); - eeprom_write_word (p16++, pos_aux[0].kp); - eeprom_write_word (p16++, pos_aux[1].kp); - eeprom_write_word (p16++, pos_aux[0].ki); - eeprom_write_word (p16++, pos_aux[1].ki); - eeprom_write_word (p16++, pos_aux[0].kd); - eeprom_write_word (p16++, pos_aux[1].kd); - eeprom_write_word (p16++, pos_aux[0].blocked_error_limit); - eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit); - eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_error_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit); + eeprom_write_word (p16++, postrack_footing); + eeprom_write_word (p16++, speed_theta.acc); + eeprom_write_word (p16++, speed_alpha.acc); + eeprom_write_word (p16++, pos_theta.kp); + eeprom_write_word (p16++, pos_alpha.kp); + eeprom_write_word (p16++, pos_theta.ki); + eeprom_write_word (p16++, pos_alpha.ki); + eeprom_write_word (p16++, pos_theta.kd); + eeprom_write_word (p16++, pos_alpha.kd); + eeprom_write_word (p16++, pos_theta.blocked_error_limit); + eeprom_write_word (p16++, pos_theta.blocked_speed_limit); + eeprom_write_word (p16++, pos_theta.blocked_counter_limit); + eeprom_write_word (p16++, pos_alpha.blocked_error_limit); + eeprom_write_word (p16++, pos_alpha.blocked_speed_limit); + eeprom_write_word (p16++, pos_alpha.blocked_counter_limit); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_i_sat); eeprom_write_word (p16++, pos_d_sat); + eeprom_write_word (p16++, traj_eps); + eeprom_write_word (p16++, traj_aeps); + eeprom_write_word (p16++, traj_angle_limit); } /* Clear eeprom parameters. */ diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h index d061a9fd..9eb7c18e 100644 --- a/digital/mimot/src/dirty/eeprom.h +++ b/digital/mimot/src/dirty/eeprom.h @@ -26,7 +26,7 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x01 +#define EEPROM_KEY 0x02 void eeprom_read_params (void); diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index 5dcfd8c7..a04c53b6 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -37,7 +37,8 @@ #include "pwm.h" #include "pos.h" #include "speed.h" -#include "aux.h" +#include "postrack.h" +#include "traj.h" #include "twi_proto.h" #include "eeprom.h" @@ -54,8 +55,8 @@ uint8_t main_sequence_ack = 4, main_sequence_ack_cpt; /** Report of counters. */ uint8_t main_stat_counter, main_stat_counter_cpt; -/** Report of auxiliary position. */ -uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt; +/** Report of position. */ +uint8_t main_stat_postrack, main_stat_postrack_cpt; /** Statistics about speed control. */ uint8_t main_stat_speed, main_stat_speed_cpt; @@ -72,6 +73,11 @@ uint8_t main_stat_timer, main_stat_timer_cpt; /** Print input port. */ uint8_t main_print_pin, main_print_pin_cpt; +#ifdef HOST +/** Simulation data. */ +uint8_t main_simu, main_simu_cpt; +#endif /* HOST */ + /** Record timer value at different stage of computing. Used for performance * analisys. */ uint8_t main_timer[6]; @@ -93,8 +99,9 @@ main (int argc, char **argv) counter_init (); uart0_init (); twi_proto_init (); + postrack_init (); speed_init (); - aux_init (); + traj_init (); eeprom_read_params (); proto_send0 ('z'); sei (); @@ -119,46 +126,54 @@ main_loop (void) pwm_update (); main_timer[2] = timer_read (); /* Compute absolute position. */ - aux_pos_update (); + postrack_update (); /* Compute trajectory. */ - aux_traj_update (); + if (state_main.mode >= MODE_TRAJ) + traj_update (); /* Speed control. */ speed_update (); main_timer[3] = timer_read (); /* Stats. */ if (main_sequence_ack - && (state_aux[0].sequence_ack != state_aux[0].sequence_finish - || state_aux[1].sequence_ack != state_aux[1].sequence_finish) + && state_main.sequence_ack != state_main.sequence_finish && !--main_sequence_ack_cpt) { - proto_send2b ('A', state_aux[0].sequence_finish, - state_aux[1].sequence_finish); + proto_send1b ('A', state_main.sequence_finish); main_sequence_ack_cpt = main_sequence_ack; } if (main_stat_counter && !--main_stat_counter_cpt) { - proto_send2w ('C', counter_aux[0], counter_aux[1]); + proto_send2w ('C', counter_left, counter_right); main_stat_counter_cpt = main_stat_counter; } - if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) + if (main_stat_postrack && !--main_stat_postrack_cpt) { - proto_send2w ('Y', aux[0].pos, aux[1].pos); - main_stat_aux_pos_cpt = main_stat_aux_pos; + proto_send3d ('X', postrack_x, postrack_y, postrack_a); + main_stat_postrack_cpt = main_stat_postrack; } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8); + proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) { - proto_send4w ('P', pos_aux[0].e_old, pos_aux[0].i, - pos_aux[1].e_old, pos_aux[1].i); + proto_send4w ('P', pos_theta.e_old, pos_theta.i, + pos_alpha.e_old, pos_alpha.i); main_stat_pos_cpt = main_stat_pos; } +#ifdef HOST + if (main_simu && !--main_simu_cpt) + { + proto_send3w ('H', (uint16_t) simu_pos_x, + (uint16_t) simu_pos_y, + (uint16_t) (simu_pos_a * 1024)); + main_simu_cpt = main_simu; + } +#endif /* HOST */ if (main_stat_pwm && !--main_stat_pwm_cpt) { - proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur); + proto_send2w ('W', pwm_left.cur, pwm_right.cur); main_stat_pwm_cpt = main_stat_pwm; } if (main_stat_timer && !--main_stat_timer_cpt) @@ -184,19 +199,8 @@ void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { /* Many commands use the first argument as a selector. */ - struct aux_t *auxp = 0; - struct pwm_t *pwm = 0; struct pos_t *pos = 0; struct speed_t *speed = 0; - struct state_t *state = 0; - if (args[0] < AC_ASSERV_AUX_NB) - { - auxp = &aux[args[0]]; - pwm = &pwm_aux[args[0]]; - pos = &pos_aux[args[0]]; - speed = &speed_aux[args[0]]; - state = &state_aux[args[0]]; - } /* Decode command. */ #define c(cmd, size) (cmd << 8 | size) switch (c (cmd, size)) @@ -207,102 +211,132 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; /* Commands. */ case c ('w', 0): - /* Set zero auxiliary pwm. */ - pos_reset (&pos_aux[0]); - pos_reset (&pos_aux[1]); - state_aux[0].mode = MODE_PWM; - state_aux[1].mode = MODE_PWM; - pwm_set (&pwm_aux[0], 0); - pwm_set (&pwm_aux[1], 0); + /* Set zero pwm. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); break; - case c ('w', 3): - /* Set auxiliary pwm. - * - b: aux index. - * - w: pwm. */ - if (!auxp) { proto_send0 ('?'); return; } - pos_reset (pos); - state->mode = MODE_PWM; - pwm_set (pwm, v8_to_v16 (args[1], args[2])); + case c ('w', 4): + /* Set pwm. + * - w: left pwm. + * - w: right pwm. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, v8_to_v16 (args[0], args[1])); + pwm_set (&pwm_right, v8_to_v16 (args[2], args[3])); break; - case c ('c', 3): - /* Add to auxiliary position consign. - * - b: aux index. - * - w: consign offset. */ - if (!auxp) { proto_send0 ('?'); return; } - state->mode = MODE_POS; - state->variant = 0; - pos->cons += v8_to_v16 (args[1], args[2]); + case c ('c', 4): + /* Add to position consign. + * - w: theta consign offset. + * - w: alpha consign offset. */ + state_main.mode = MODE_POS; + state_main.variant = 0; + pos_theta.cons += v8_to_v16 (args[0], args[1]); + pos_alpha.cons += v8_to_v16 (args[2], args[3]); break; - case c ('s', 1): - /* Set auxiliary zero speed. - * - b: aux index. */ - if (!auxp) { proto_send0 ('?'); return; } - state->mode = MODE_SPEED; - state->variant = 0; - speed->use_pos = 0; - speed->cons = 0; + case c ('s', 0): + /* Stop (set zero speed). */ + state_main.mode = MODE_SPEED; + state_main.variant = 0; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = 0; + speed_alpha.cons = 0; break; case c ('s', 2): - /* Set auxiliary speed. - * - b: aux index. - * - b: speed. */ - if (!auxp) { proto_send0 ('?'); return; } - state->mode = MODE_SPEED; - state->variant = 0; - speed->use_pos = 0; - speed->cons = args[1] << 8; + /* Set speed. + * - b: theta speed. + * - b: alpha speed. */ + state_main.mode = MODE_SPEED; + state_main.variant = 0; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = args[0] << 8; + speed_alpha.cons = args[1] << 8; + break; + case c ('s', 9): + /* Set speed controlled position consign. + * - d: theta consign offset. + * - d: alpha consign offset. + * - b: sequence number. */ + if (args[8] == state_main.sequence) + break; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); + speed_alpha.pos_cons = pos_alpha.cons; + speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); + state_start (&state_main, MODE_SPEED, args[8]); break; - case c ('s', 6): - /* Set auxiliary speed controlled position consign. - * - b: aux index. + case c ('l', 5): + /* Set linear speed controlled position consign. * - d: consign offset. * - b: sequence number. */ - if (!auxp) { proto_send0 ('?'); return; } - if (args[5] == state->sequence) + if (args[4] == state_main.sequence) break; - speed->use_pos = 1; - speed->pos_cons = pos->cons; - speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]); - state_start (state, MODE_SPEED, args[5]); + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_SPEED, args[4]); break; - case c ('y', 4): - /* Auxiliary go to position. - * - b: aux index. - * - w: pos, i16. + case c ('a', 5): + /* Set angular speed controlled position consign. + * - d: angle offset. * - b: sequence number. */ - if (!auxp) { proto_send0 ('?'); return; } - if (args[3] == state->sequence) + if (args[4] == state_main.sequence) break; - aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]); + traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2], + args[3]), args[4]); break; - case c ('y', 5): - /* Auxiliary clamp. - * - b: aux index. - * - b: speed. - * - w: clamping PWM. + case c ('f', 2): + /* Go to the wall. + * - b: 0: forward, 1: backward. * - b: sequence number. */ - if (!auxp) { proto_send0 ('?'); return; } - if (args[4] == state->sequence) + if (args[1] == state_main.sequence) break; - aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]), - args[4]); + traj_ftw_start (args[0], args[1]); break; - case c ('y', 3): - /* Auxiliary find zero. - * - b: aux index. - * - b: speed. + case c ('x', 10): + /* Go to position. + * - d: x, f24.8. + * - d: y, f24.8. + * - b: backward (see traj.h). * - b: sequence number. */ - if (!auxp) { proto_send0 ('?'); return; } - if (args[2] == state->sequence) + if (args[9] == state_main.sequence) break; - aux_traj_find_limit_start (auxp, args[1], args[2]); + traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]), + v8_to_v32 (args[4], args[5], args[6], args[7]), + args[8], args[9]); break; - case c ('a', 2): - /* Set all acknoledge. - * - b: first auxiliary ack sequence number. - * - b: second auxiliary ack sequence number. */ - state_acknowledge (&state_aux[0], args[0]); - state_acknowledge (&state_aux[1], args[1]); + case c ('x', 3): + /* Go to angle. + * - d: a, f0.16. + * - b: sequence number. */ + if (args[2] == state_main.sequence) + break; + traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0), + args[2]); + break; + case c ('x', 12): + /* Go to position, then angle. + * - d: x, f24.8. + * - d: y, f24.8. + * - w: a, f0.16. + * - b: backward (see traj.h). + * - b: sequence number. */ + if (args[11] == state_main.sequence) + break; + traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]), + v8_to_v32 (args[4], args[5], args[6], args[7]), + v8_to_v32 (0, args[8], args[9], 0), + args[10], args[11]); + break; + case c ('a', 1): + /* Set main acknoledge. + * - b: main ack sequence number. */ + state_acknowledge (&state_main, args[0]); break; /* Stats. * - b: interval between stats. */ @@ -314,16 +348,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Counter stats. */ main_stat_counter_cpt = main_stat_counter = args[0]; break; - case c ('Y', 1): - /* Auxiliary position stats. */ - main_stat_aux_pos_cpt = main_stat_aux_pos = args[0]; + case c ('X', 1): + /* Position stats. */ + main_stat_postrack_cpt = main_stat_postrack = args[0]; break; case c ('S', 1): /* Motor speed control stats. */ main_stat_speed_cpt = main_stat_speed = args[0]; break; case c ('P', 1): - /* Auxiliary motor position control stats. */ + /* Motor position control stats. */ main_stat_pos_cpt = main_stat_pos = args[0]; break; case c ('W', 1): @@ -338,6 +372,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Input port stats. */ main_print_pin_cpt = main_print_pin = args[0]; break; +#ifdef HOST + case c ('H', 1): + /* Simulation data. */ + main_simu_cpt = main_simu = args[0]; + break; +#endif /* HOST */ default: /* Params. */ if (cmd == 'p') @@ -345,10 +385,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Many commands use the first argument as a selector. */ switch (args[1]) { - case 0: - case 1: - pos = &pos_aux[args[1]]; - speed = &speed_aux[args[1]]; + case 't': + pos = &pos_theta; + speed = &speed_theta; + break; + case 'a': + pos = &pos_alpha; + speed = &speed_alpha; break; default: pos = 0; @@ -357,6 +400,38 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) } switch (c (args[0], size)) { + case c ('X', 1): + /* Reset position. */ + postrack_x = 0; + postrack_y = 0; + postrack_a = 0; + break; + case c ('X', 5): + /* Set current x position. + * - d: x position. */ + postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('Y', 5): + /* Set current y position. + * - d: y position. */ + postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('A', 5): + /* Set current angle. + * - d: angle. */ + postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('c', 5): + /* Set right counter correction factor. + * - d: factor (f8.24). */ + counter_right_correction = v8_to_v32 (args[1], args[2], + args[3], args[4]); + break; + case c ('f', 3): + /* Set footing. + * - w: footing. */ + postrack_set_footing (v8_to_v16 (args[1], args[2])); + break; case c ('a', 4): /* Set acceleration. * - b: index. @@ -414,9 +489,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('D', 3): pos_d_sat = v8_to_v16 (args[1], args[2]); break; + case c ('e', 5): + traj_eps = v8_to_v16 (args[1], args[2]); + traj_aeps = v8_to_v16 (args[3], args[4]); + break; + case c ('l', 3): + traj_set_angle_limit (v8_to_v16 (args[1], args[2])); + break; case c ('w', 2): /* Set PWM direction. - * - b: bits: 0000[aux1][aux0][right][left]. */ + * - b: bits: 000000[right][left]. */ pwm_set_reverse (args[1]); break; case c ('E', 2): @@ -430,21 +512,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('P', 1): /* Print current settings. */ proto_send1b ('E', EEPROM_KEY); - proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc); - proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow); - proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow); - proto_send3w ('b', pos_aux[0].blocked_error_limit, - pos_aux[0].blocked_speed_limit, - pos_aux[0].blocked_counter_limit); - proto_send3w ('b', pos_aux[1].blocked_error_limit, - pos_aux[1].blocked_speed_limit, - pos_aux[1].blocked_counter_limit); - proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp); - proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki); - proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd); + proto_send1d ('c', counter_right_correction); + proto_send1w ('f', postrack_footing); + proto_send2w ('a', speed_theta.acc, speed_alpha.acc); + proto_send2b ('s', speed_theta.max, speed_theta.slow); + proto_send2b ('s', speed_alpha.max, speed_alpha.slow); + proto_send3w ('b', pos_theta.blocked_error_limit, + pos_theta.blocked_speed_limit, + pos_theta.blocked_counter_limit); + proto_send3w ('b', pos_alpha.blocked_error_limit, + pos_alpha.blocked_speed_limit, + pos_alpha.blocked_counter_limit); + proto_send2w ('p', pos_theta.kp, pos_alpha.kp); + proto_send2w ('i', pos_theta.ki, pos_alpha.ki); + proto_send2w ('d', pos_theta.kd, pos_alpha.kd); proto_send1w ('E', pos_e_sat); proto_send1w ('I', pos_i_sat); proto_send1w ('D', pos_d_sat); + proto_send2w ('e', traj_eps, traj_aeps); + proto_send1w ('l', traj_angle_limit); proto_send1b ('w', pwm_reverse); break; default: @@ -452,6 +538,24 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) return; } } +#ifdef HOST + else if (cmd == 'h') + { + switch (c (args[0], size)) + { + case c ('X', 7): + /* Set simulated position. + * - w: x position. + * - w: y position. + * - w: angle (rad) * 1024. */ + simu_pos_x = (double) v8_to_v16 (args[1], args[2]); + simu_pos_y = (double) v8_to_v16 (args[3], args[4]); + simu_pos_a = (double) (int16_t) v8_to_v16 (args[5], args[6]) + / 1024; + break; + } + } +#endif /* HOST */ else { proto_send0 ('?'); diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index efea35bc..0b9b3665 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -32,35 +32,43 @@ #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 = +/* RE25G with 1:20.25 gearbox model. */ +static const struct motor_def_t re25g_model = { /* Motor characteristics. */ - 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ - 26.10 / 1000, /* Torque constant (N.m/A). */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ 0, /* Bearing friction (N.m/(rad/s)). */ - 7.10, /* Terminal resistance (Ohm). */ - 0.265 / 1000, /* Terminal inductance (H). */ + 2.32, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ 24.0, /* Maximum voltage (V). */ /* Gearbox characteristics. */ - 3.71, /* Gearbox ratio. */ - 0.88, /* Gearbox efficiency. */ + 20.25, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ /* Load characteristics. */ - 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */ - /* This is a pifometric estimation. */ + 0.0, /* Load (kg.m^2). */ /* Hardware limits. */ - 0.0, +INFINITY, + -INFINITY, +INFINITY, }; -/* Marcel, APBTeam 2010. */ -static const struct robot_t marcel_robot = +/* TazG, Taz with RE25G motors. */ +static const struct robot_t tazg_robot = { - /** Auxiliary motors, NULL if not present. */ - { &marcel_clamp_f2342_model, &marcel_clamp_f2342_model }, - /** Number of steps for each auxiliary motor encoder. */ - { 256, 256 }, - /** Sensor update function. */ - simu_sensor_update_marcel, + /* Main motors. */ + &re25g_model, + /* Number of steps on the main motors encoders. */ + 500, + /* Wheel radius (m). */ + 0.04, + /* Distance between the wheels (m). */ + 0.30, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 0, + 0.0, 0.0, NULL }; /* Table of models. */ @@ -69,7 +77,7 @@ static const struct const char *name; const struct robot_t *robot; } models[] = { - { "marcel", &marcel_robot }, + { "tazg", &tazg_robot }, { 0, 0 } }; @@ -88,20 +96,24 @@ models_get (const char *name) /** Initialise simulation models. */ void -models_init (const struct robot_t *robot, struct motor_t aux_motor[]) +models_init (const struct robot_t *robot, struct motor_t *main_motor_left, + struct motor_t *main_motor_right) { - int i; - if (aux_motor) + if (main_motor_left) + { + main_motor_left->m = *robot->main_motor; + main_motor_left->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_left->h = ECHANT_PERIOD; + main_motor_left->d = 1000; + } + if (main_motor_right) { - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - { - if (robot->aux_motor[i]) - { - aux_motor[i].m = *robot->aux_motor[i]; - aux_motor[i].h = ECHANT_PERIOD; - aux_motor[i].d = 1000; - } - } + main_motor_right->m = *robot->main_motor; + main_motor_right->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_right->h = ECHANT_PERIOD; + main_motor_right->d = 1000; } } diff --git a/digital/mimot/src/dirty/models.host.h b/digital/mimot/src/dirty/models.host.h index 0e53aaea..e1bf3427 100644 --- a/digital/mimot/src/dirty/models.host.h +++ b/digital/mimot/src/dirty/models.host.h @@ -31,10 +31,24 @@ * Encoder characteristics are defined at gearbox output. */ struct robot_t { - /** Auxiliary motors, NULL if not present. */ - const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB]; - /** Number of steps for each auxiliary motor encoder. */ - int aux_encoder_steps[AC_ASSERV_AUX_NB]; + /** Main motors. */ + const struct motor_def_t *main_motor; + /** Number of steps on the main motors encoders. */ + int main_encoder_steps; + /** Wheel radius (m). */ + double wheel_r; + /** Distance between the wheels (m). */ + double footing; + /** Weight of the robot (kg). */ + double weight; + /** Distance of the gravity center from the center of motors axis (m). */ + double gravity_center_distance; + /** Whether the encoder is mounted on the main motor (false) or not (true). */ + int encoder_separated; + /** Encoder wheel radius (m). */ + double encoder_wheel_r; + /** Distance between the encoders wheels (m). */ + double encoder_footing; /** Sensor update function. */ void (*sensor_update) (void); }; @@ -45,6 +59,7 @@ models_get (const char *name); /** Initialise simulation models. */ void -models_init (const struct robot_t *robot, struct motor_t aux_motor[]); +models_init (const struct robot_t *robot, struct motor_t *main_motor_left, + struct motor_t *main_motor_right); #endif /* models_host_h */ diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c index 28684414..6304bb5c 100644 --- a/digital/mimot/src/dirty/pos.c +++ b/digital/mimot/src/dirty/pos.c @@ -40,8 +40,8 @@ * 16 bits are enough as long as there is no long blocking (see 2005 cup!). */ -/** Auxiliaries control states. */ -struct pos_t pos_aux[AC_ASSERV_AUX_NB]; +/** Theta/alpha control states. */ +struct pos_t pos_theta, pos_alpha; /** Error saturation. */ int32_t pos_e_sat = 1023; @@ -84,43 +84,59 @@ pos_compute_pid (int32_t e, struct pos_t *pos) return pid >> 8; } -/** Update PWM for a single motor system. */ -static void -pos_update_single (struct state_t *state, struct pos_t *pos, - int16_t counter_diff, struct pwm_t *pwm) +/** Update PWM for a polar motor system. */ +void +pos_update_polar (struct state_t *state, + struct pos_t *pos_theta, struct pos_t *pos_alpha, + int16_t counter_left_diff, int16_t counter_right_diff, + struct pwm_t *pwm_left, struct pwm_t *pwm_right) { if (state->mode >= MODE_POS) { - int16_t pid; - int32_t error; - /* Update current shaft position. */ - pos->cur += counter_diff; + int16_t pid_theta, pid_alpha; + int32_t error_theta, error_alpha; + int16_t cur_speed_theta, cur_speed_alpha; + /* Update current shaft positions. */ + cur_speed_theta = counter_left_diff + counter_right_diff; + cur_speed_alpha = counter_right_diff - counter_left_diff; + pos_theta->cur += cur_speed_theta; + pos_alpha->cur += cur_speed_alpha; + if (state->variant & 1) + pos_reset (pos_theta); + if (state->variant & 2) + pos_reset (pos_alpha); /* Compute error. */ - error = pos->cons - pos->cur; - /* Test or blocking. */ - if (UTILS_ABS (error) > pos->blocked_error_limit - && UTILS_ABS (counter_diff) < pos->blocked_speed_limit) - pos->blocked_counter++; + error_theta = pos_theta->cons - pos_theta->cur; + error_alpha = pos_alpha->cons - pos_alpha->cur; + /* Test for blocking. */ + if (UTILS_ABS (error_theta) > pos_theta->blocked_error_limit + && UTILS_ABS (cur_speed_theta) < pos_theta->blocked_speed_limit) + pos_theta->blocked_counter++; else - pos->blocked_counter = 0; - if (state->variant & 1) - { - pos_reset (pos); - } - else if (!(state->variant & 4) - && pos->blocked_counter > pos->blocked_counter_limit) + pos_theta->blocked_counter = 0; + if (UTILS_ABS (error_alpha) > pos_alpha->blocked_error_limit + && UTILS_ABS (cur_speed_alpha) < pos_alpha->blocked_speed_limit) + pos_alpha->blocked_counter++; + else + pos_alpha->blocked_counter = 0; + if (pos_theta->blocked_counter > pos_theta->blocked_counter_limit + || pos_alpha->blocked_counter > pos_alpha->blocked_counter_limit) { /* Blocked. */ - pos_reset (pos); + pos_reset (pos_theta); + pos_reset (pos_alpha); state_blocked (state); - pwm_set (pwm, 0); + pwm_set (pwm_left, 0); + pwm_set (pwm_right, 0); } else { /* Compute PID. */ - pid = pos_compute_pid (error, pos); + pid_theta = pos_compute_pid (error_theta, pos_theta); + pid_alpha = pos_compute_pid (error_alpha, pos_alpha); /* Update PWM. */ - pwm_set (pwm, pid); + pwm_set (pwm_left, pid_theta - pid_alpha); + pwm_set (pwm_right, pid_theta + pid_alpha); } } } @@ -129,10 +145,9 @@ pos_update_single (struct state_t *state, struct pos_t *pos, void pos_update (void) { - uint8_t i; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i], - &pwm_aux[i]); + pos_update_polar (&state_main, &pos_theta, &pos_alpha, + counter_left_diff, counter_right_diff, + &pwm_left, &pwm_right); } /** Reset position control state. To be called when the position control is diff --git a/digital/mimot/src/dirty/pos.h b/digital/mimot/src/dirty/pos.h index 514b091c..df6935b3 100644 --- a/digital/mimot/src/dirty/pos.h +++ b/digital/mimot/src/dirty/pos.h @@ -48,7 +48,7 @@ struct pos_t uint8_t blocked_counter; }; -extern struct pos_t pos_aux[AC_ASSERV_AUX_NB]; +extern struct pos_t pos_theta, pos_alpha; extern int32_t pos_e_sat, pos_i_sat, pos_d_sat; diff --git a/digital/mimot/src/dirty/postrack.c b/digital/mimot/src/dirty/postrack.c new file mode 100644 index 00000000..a3b2e8cb --- /dev/null +++ b/digital/mimot/src/dirty/postrack.c @@ -0,0 +1,103 @@ +/* postrack.c - Compute current position from counters. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "postrack.h" + +#include "modules/math/fixed/fixed.h" +#include "modules/math/math.h" + +#include "counter.h" + +/** Current position, f24.8. */ +int32_t postrack_x, postrack_y; +/** Current angle, f8.24. */ +int32_t postrack_a; +/** Distance between the weels, u16. */ +uint16_t postrack_footing; +/** Precomputed footing factor, f8.24. + * postrack_footing_factor = (1/2pi * 256) / postrack_footing + * Explanations: + * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we + * must divide by 2pi to convert unit (Arc=Angle * Radius only works for + * radians). + * - dd (see postrack_update) is in f10.16 format, we multiply by 256 to have + * a angle in f8.24 format. + * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */ +static uint32_t postrack_footing_factor; + +/** Initialise the position tracker. */ +void +postrack_init (void) +{ + /* Prevent division by 0 by providing a default large value. */ + postrack_set_footing (0x1000); +} + +/** Update the current position. */ +void +postrack_update (void) +{ + int32_t d, dd, da, na, dsc; + d = counter_right_diff + counter_left_diff; /* 10b */ + d <<= 16; /* 10.16b */ + if (counter_right_diff == counter_left_diff) + { + /* Line. */ + postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8; + postrack_y += fixed_mul_f824 (d, fixed_sin_f824 (postrack_a)) >> 8; + } + else + { + /* Arc. */ + dd = counter_right_diff - counter_left_diff; /* 10b */ + dd <<= 16; /* 10.16b */ + da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */ + /* New angle. */ + na = postrack_a + da; + /* Compute da in radians. */ + da = fixed_mul_f824 (da, 2 * M_PI * (1L << 24)); + /* X increment. */ + dsc = fixed_sin_f824 (na) - fixed_sin_f824 (postrack_a); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_x += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Y increment. */ + dsc = fixed_cos_f824 (postrack_a) - fixed_cos_f824 (na); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_y += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Angle update. */ + postrack_a = na; + postrack_a &= 0x00ffffff; + } +} + +/** Change the footing value. */ +void +postrack_set_footing (uint16_t footing) +{ + postrack_footing = footing; + postrack_footing_factor = + (uint32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing; +} + diff --git a/digital/mimot/src/dirty/postrack.h b/digital/mimot/src/dirty/postrack.h new file mode 100644 index 00000000..26fc4ee3 --- /dev/null +++ b/digital/mimot/src/dirty/postrack.h @@ -0,0 +1,41 @@ +#ifndef postrack_h +#define postrack_h +/* postrack.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +extern int32_t postrack_x, postrack_y; +extern int32_t postrack_a; +extern uint16_t postrack_footing; + +void +postrack_init (void); + +void +postrack_update (void); + +void +postrack_set_footing (uint16_t footing); + +#endif /* postrack_h */ diff --git a/digital/mimot/src/dirty/pwm.avr.c b/digital/mimot/src/dirty/pwm.avr.c index 37ec34b0..dc43343f 100644 --- a/digital/mimot/src/dirty/pwm.avr.c +++ b/digital/mimot/src/dirty/pwm.avr.c @@ -27,9 +27,8 @@ #include "pwm_ocr.avr.h" /** PWM control states. */ -struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { - PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) -}; +struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); +struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); /** PWM reverse directions. */ uint8_t pwm_reverse; diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h index a751ee9e..e48f66b9 100644 --- a/digital/mimot/src/dirty/pwm.h +++ b/digital/mimot/src/dirty/pwm.h @@ -39,34 +39,34 @@ struct pwm_t int16_t min; }; -extern struct pwm_t pwm_aux[AC_ASSERV_AUX_NB]; +extern struct pwm_t pwm_left, pwm_right; extern uint8_t pwm_reverse; /** Define current PWM value for each output. */ #define PWM_VALUE(x) PWM_VALUE_ (x) #define PWM_VALUE_(x) PWM_VALUE_ ## x -#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur -#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur +#define PWM_VALUE_pwm_left pwm_left.cur +#define PWM_VALUE_pwm_right pwm_right.cur /** Define maximum PWM value for each output. */ #define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x) #define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x -#define PWM_MAX_FOR_pwm_aux0 PWM_MAX -#define PWM_MAX_FOR_pwm_aux1 PWM_MAX +#define PWM_MAX_FOR_pwm_left PWM_MAX +#define PWM_MAX_FOR_pwm_right PWM_MAX /** Define minimum PWM value for each output, if the value is less than the * minimum, use 0. */ #define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x) #define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x -#define PWM_MIN_FOR_pwm_aux0 0x10 -#define PWM_MIN_FOR_pwm_aux1 0x10 +#define PWM_MIN_FOR_pwm_left 0x10 +#define PWM_MIN_FOR_pwm_right 0x10 /** Define which bit controls the PWM inversion. */ #define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x) #define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x -#define PWM_REVERSE_BIT_pwm_aux0 _BV (0) -#define PWM_REVERSE_BIT_pwm_aux1 _BV (1) +#define PWM_REVERSE_BIT_pwm_left _BV (0) +#define PWM_REVERSE_BIT_pwm_right _BV (1) /** State init macro. */ #define PWM_INIT_FOR(x) \ diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c index 34b69205..f10f37e7 100644 --- a/digital/mimot/src/dirty/pwm_ocr.avr.c +++ b/digital/mimot/src/dirty/pwm_ocr.avr.c @@ -30,8 +30,8 @@ #include "io.h" /** Assign PWM outputs. */ -#define PWM1 pwm_aux0 -#define PWM2 pwm_aux1 +#define PWM1 pwm_left +#define PWM2 pwm_right #define PWM1_OCR OCR1A #define PWM1_OCR_BIT 5 diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index af07d744..6aec1788 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -39,7 +39,6 @@ #include #include "pwm.h" -#include "aux.h" #include "contacts.h" @@ -50,15 +49,18 @@ uint8_t PORTB, PORTC, PORTD, PINC; /** Overall counter values. */ -uint16_t counter_aux[AC_ASSERV_AUX_NB]; +uint16_t counter_left, counter_right; /** Counter differences since last update. * Maximum of 9 significant bits, sign included. */ -int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; +int16_t counter_left_diff, counter_right_diff; +/** Overall uncorrected counter values. */ +static int32_t counter_right_raw; +/** Correction factor (f8.24). */ +uint32_t counter_right_correction = 1L << 24; /** PWM control states. */ -struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { - PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) -}; +struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); +struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); /** PWM reverse directions. */ uint8_t pwm_reverse; @@ -66,52 +68,25 @@ uint8_t pwm_reverse; const struct robot_t *simu_robot; /** Motor models. */ -struct motor_t simu_aux_model[AC_ASSERV_AUX_NB]; +struct motor_t simu_left_model, simu_right_model; + +/** Computed simulated position (mm, rad). */ +double simu_pos_x, simu_pos_y, simu_pos_a; /** Full counter values. */ -uint32_t simu_counter_aux[AC_ASSERV_AUX_NB]; +uint32_t simu_counter_left, simu_counter_right; +double simu_counter_left_th, simu_counter_right_th; /** Use mex. */ int simu_mex; /** Mex message types. */ -uint8_t simu_mex_aux; -uint8_t simu_mex_limits; +uint8_t simu_mex_position; +uint8_t simu_mex_pwm; /** Counter to limit the interval between information is sent. */ int simu_send_cpt; -static void -simu_handle_limits__update_limit (double *th_limit, int32_t limit_int, - double i_G, int sign) -{ - if (limit_int == -1) - /* No change. */ - ; - else if (limit_int == -2) - /* INFINITY. */ - *th_limit = sign > 0 ? INFINITY : -INFINITY; - else - *th_limit = (double) limit_int / 1024.0 * i_G; -} - -static void -simu_handle_limits (void *user, mex_msg_t *msg) -{ - int i; - int32_t limit_min_int, limit_max_int; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - { - mex_msg_pop (msg, "ll", &limit_min_int, &limit_max_int); - simu_handle_limits__update_limit (&simu_aux_model[i].m.th_min, - limit_min_int, - simu_aux_model[i].m.i_G, -1); - simu_handle_limits__update_limit (&simu_aux_model[i].m.th_max, - limit_max_int, - simu_aux_model[i].m.i_G, +1); - } -} - /** Initialise simulation. */ static void simu_init (void) @@ -128,9 +103,8 @@ simu_init (void) simu_send_cpt = simu_mex; mex_node_connect (); mex_instance = host_get_instance ("mimot0", 0); - simu_mex_aux = mex_node_reservef ("%s:aux", mex_instance); - simu_mex_limits = mex_node_reservef ("%s:limits", mex_instance); - mex_node_register (simu_mex_limits, simu_handle_limits, 0); + simu_mex_position = mex_node_reservef ("%s:position", mex_instance); + simu_mex_pwm = mex_node_reservef ("%s:pwm", mex_instance); } if (argc != 1) { @@ -143,50 +117,94 @@ simu_init (void) fprintf (stderr, "unknown model name: %s\n", argv[0]); exit (1); } - models_init (simu_robot, simu_aux_model); + models_init (simu_robot, &simu_left_model, &simu_right_model); + simu_pos_x = simu_pos_y = simu_pos_a = 0; } -/** Update sensors for Marcel. */ -void -simu_sensor_update_marcel (void) +/** Update simulation position. */ +static void +simu_pos_update (double dl, double dr, double footing) { + double d = 0.5 * (dl + dr); + double da = (dr - dl) / footing; + double na = simu_pos_a + da; + if (da < 0.0001 && da > -0.0001) + { + /* Avoid a division by zero when angle is too small. */ + double a = simu_pos_a + da * 0.5; + simu_pos_x += d * cos (a); + simu_pos_y += d * sin (a); + } + else + { + /* Radius of turn is d / da. */ + simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da; + simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da; + } + simu_pos_a = na; } /** Do a simulation step. */ static void simu_step (void) { - int i; - double old_aux_th[AC_ASSERV_AUX_NB]; + double old_left_th, old_right_th; /* Convert pwm value into voltage. */ - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - simu_aux_model[i].u = simu_aux_model[i].m.u_max - * ((double) pwm_aux[i].cur / (PWM_MAX + 1)); + simu_left_model.u = simu_left_model.m.u_max + * ((double) pwm_left.cur / (PWM_MAX + 1)); + simu_right_model.u = simu_right_model.m.u_max + * ((double) pwm_right.cur / (PWM_MAX + 1)); /* Make one step. */ - for (i = 0; i < AC_ASSERV_AUX_NB; i++) + old_left_th = simu_left_model.th; + old_right_th = simu_right_model.th; + motor_model_step (&simu_left_model); + motor_model_step (&simu_right_model); + /* Modify counters. */ + uint32_t counter_left_new; + uint32_t counter_right_new; + if (!simu_robot->encoder_separated) { - old_aux_th[i] = simu_aux_model[i].th; - if (simu_robot->aux_motor[i]) - motor_model_step (&simu_aux_model[i]); + counter_left_new = simu_left_model.th / (2*M_PI) + * simu_robot->main_encoder_steps; + counter_right_new = simu_right_model.th / (2*M_PI) + * simu_robot->main_encoder_steps; } - /* Update auxiliary counter. */ - for (i = 0; i < AC_ASSERV_AUX_NB; i++) + else { - if (simu_robot->aux_motor[i]) - { - uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI) - * simu_robot->aux_encoder_steps[i]; - counter_aux_diff[i] = counter_aux_new - simu_counter_aux[i]; - counter_aux[i] += counter_aux_diff[i]; - simu_counter_aux[i] = counter_aux_new; - } - else - { - counter_aux_diff[i] = 0; - counter_aux[i] = 0; - simu_counter_aux[i] = 0; - } + /* Thanks Thalès. */ + double left_diff = (simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r; + double right_diff = (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r; + double sum = left_diff + right_diff; + double diff = (left_diff - right_diff) + * (simu_robot->encoder_footing / simu_robot->footing); + double left_enc_diff = 0.5 * (sum + diff); + double right_enc_diff = 0.5 * (sum - diff); + simu_counter_left_th += left_enc_diff / simu_robot->encoder_wheel_r; + simu_counter_right_th += right_enc_diff / simu_robot->encoder_wheel_r; + counter_left_new = simu_counter_left_th / (2*M_PI) + * simu_robot->main_encoder_steps; + counter_right_new = simu_counter_right_th / (2*M_PI) + * simu_robot->main_encoder_steps; } + /* Update an integer counter. */ + counter_left_diff = counter_left_new - simu_counter_left; + counter_left += counter_left_diff; + simu_counter_left = counter_left_new; + counter_right_diff = counter_right_new - simu_counter_right; + counter_right_raw += counter_right_diff; + uint16_t right_new = fixed_mul_f824 (counter_right_raw, + counter_right_correction); + counter_right_diff = (int16_t) (right_new - counter_right); + counter_right = right_new; + simu_counter_right = counter_right_new; + /* Update position. */ + simu_pos_update ((simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, + (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, + simu_robot->footing * 1000); /* Update sensors. */ if (simu_robot->sensor_update) simu_robot->sensor_update (); @@ -197,31 +215,37 @@ static void simu_send (void) { static int first = 1; - int i; mex_msg_t *m; - /* Send Aux position. */ - static int32_t simu_aux_model_sent[AC_ASSERV_AUX_NB]; - int32_t simu_aux_model_to_send[AC_ASSERV_AUX_NB]; - int simu_aux_model_changed = 0; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) + /* Send position. */ + static int16_t simu_pos_x_sent, simu_pos_y_sent; + static int32_t simu_pos_a_sent; + int16_t simu_pos_x_to_send = simu_pos_x; + int16_t simu_pos_y_to_send = simu_pos_y; + int32_t simu_pos_a_to_send = 1024.0 * simu_pos_a; + if (first + || simu_pos_x_to_send != simu_pos_x_sent + || simu_pos_y_to_send != simu_pos_y_sent + || simu_pos_a_to_send != simu_pos_a_sent) { - simu_aux_model_to_send[i] = 1024.0 * simu_aux_model[i].th - / simu_aux_model[i].m.i_G; - if (!first && simu_aux_model_to_send[i] != simu_aux_model_sent[i]) - simu_aux_model_changed = 1; + m = mex_msg_new (simu_mex_position); + mex_msg_push (m, "hhl", simu_pos_x_to_send, simu_pos_y_to_send, + simu_pos_a_to_send); + mex_node_send (m); + simu_pos_x_sent = simu_pos_x_to_send; + simu_pos_y_sent = simu_pos_y_to_send; + simu_pos_a_sent = simu_pos_a_to_send; } - if (first || simu_aux_model_changed) + /* Send PWM. */ + static int16_t pwm_left_sent, pwm_right_sent; + if (first + || pwm_left_sent == pwm_left.cur + || pwm_right_sent == pwm_right.cur) { - m = mex_msg_new (simu_mex_aux); - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - { - if (simu_robot->aux_motor[i]) - mex_msg_push (m, "l", simu_aux_model_to_send[i]); - else - mex_msg_push (m, "l", 0); - simu_aux_model_sent[i] = simu_aux_model_to_send[i]; - } + m = mex_msg_new (simu_mex_pwm); + mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur); mex_node_send (m); + pwm_left_sent = pwm_left.cur; + pwm_right_sent = pwm_right.cur; } /* First send done. */ first = 0; diff --git a/digital/mimot/src/dirty/simu.host.h b/digital/mimot/src/dirty/simu.host.h index 6250bed1..83f88314 100644 --- a/digital/mimot/src/dirty/simu.host.h +++ b/digital/mimot/src/dirty/simu.host.h @@ -26,6 +26,7 @@ * }}} */ extern uint8_t PORTB, PORTC, PORTD, PINC; +extern double simu_pos_x, simu_pos_y, simu_pos_a; void timer_init (void); diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c index f9d4193e..cd68fec2 100644 --- a/digital/mimot/src/dirty/speed.c +++ b/digital/mimot/src/dirty/speed.c @@ -37,16 +37,15 @@ * shaft position. */ -/** Auxiliaries speed control states. */ -struct speed_t speed_aux[AC_ASSERV_AUX_NB]; +/** Theta/alpha speed control states. */ +struct speed_t speed_theta, speed_alpha; /** Initialise speed control states. */ void speed_init (void) { - uint8_t i; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - speed_aux[i].pos = &pos_aux[i]; + speed_theta.pos = &pos_theta; + speed_alpha.pos = &pos_alpha; } /** Update current speed according to a speed consign. */ @@ -104,18 +103,24 @@ speed_update_by (struct speed_t *speed) speed->pos->cons += speed->cur >> 8; } -/** Update shaft position consign for one motor system. */ +/** Update shaft position consign for two motors system. */ static void -speed_update_single (struct state_t *state, struct speed_t *speed) +speed_update_double (struct state_t *state, struct speed_t *speed0, + struct speed_t *speed1) { if (state->mode >= MODE_SPEED) { /* Update speed. */ - speed_update_by (speed); + speed_update_by (speed0); + speed_update_by (speed1); /* Check for completion. */ if (state->mode == MODE_SPEED - && speed->use_pos && speed->cur == 0) + && (speed0->use_pos || speed1->use_pos) + && (!speed0->use_pos || speed0->cur == 0) + && (!speed1->use_pos || speed1->cur == 0)) + { state_finish (state); + } } } @@ -123,8 +128,6 @@ speed_update_single (struct state_t *state, struct speed_t *speed) void speed_update (void) { - uint8_t i; - for (i = 0; i < AC_ASSERV_AUX_NB; i++) - speed_update_single (&state_aux[i], &speed_aux[i]); + speed_update_double (&state_main, &speed_theta, &speed_alpha); } diff --git a/digital/mimot/src/dirty/speed.h b/digital/mimot/src/dirty/speed.h index 10a0da2b..5d4fd855 100644 --- a/digital/mimot/src/dirty/speed.h +++ b/digital/mimot/src/dirty/speed.h @@ -46,7 +46,7 @@ struct speed_t uint8_t use_pos; }; -extern struct speed_t speed_aux[AC_ASSERV_AUX_NB]; +extern struct speed_t speed_theta, speed_alpha; void speed_init (void); diff --git a/digital/mimot/src/dirty/state.c b/digital/mimot/src/dirty/state.c index c6c90409..11ad1a4a 100644 --- a/digital/mimot/src/dirty/state.c +++ b/digital/mimot/src/dirty/state.c @@ -25,5 +25,5 @@ #include "common.h" #include "state.h" -struct state_t state_aux[AC_ASSERV_AUX_NB]; +struct state_t state_main; diff --git a/digital/mimot/src/dirty/state.h b/digital/mimot/src/dirty/state.h index 4916e691..4c1a28ed 100644 --- a/digital/mimot/src/dirty/state.h +++ b/digital/mimot/src/dirty/state.h @@ -77,8 +77,8 @@ struct state_t uint8_t blocked; }; -/** Auxiliary motor states. */ -extern struct state_t state_aux[AC_ASSERV_AUX_NB]; +/** Main motors state. */ +extern struct state_t state_main; /** Start a new command execution. */ static inline void diff --git a/digital/mimot/src/dirty/traj.c b/digital/mimot/src/dirty/traj.c new file mode 100644 index 00000000..a12b6f5b --- /dev/null +++ b/digital/mimot/src/dirty/traj.c @@ -0,0 +1,349 @@ +/* traj.c - Trajectories. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "traj.h" + +#include "modules/math/fixed/fixed.h" +#include "modules/math/math.h" +#include "modules/utils/utils.h" +#include "io.h" + +#include + +#include "state.h" + +#include "pos.h" +#include "speed.h" +#include "postrack.h" + +#include "contacts.h" + +#ifdef HOST +# include "simu.host.h" +#endif + +/* This file provides high level trajectories commands. */ + +/** Angle after which a backward movement is considered. */ +#define TRAJ_GOTO_BACKWARD_THRESHOLD ((int32_t) (0.55 * M_PI * (1L << 24))) + +/** The famous number. */ +#define PI_F824 ((int32_t) (M_PI * (1L << 24))) + +/** Traj mode enum. */ +enum +{ + /* Go to the wall. */ + TRAJ_FTW, + /* Go to position. */ + TRAJ_GOTO, + /* Go to angle. */ + TRAJ_GOTO_ANGLE, + /* Go to position, then angle. */ + TRAJ_GOTO_XYA, + /* Everything done. */ + TRAJ_DONE, +}; + +/** Traj mode. */ +uint8_t traj_mode; + +/** Epsilon, distance considered to be small enough. */ +int16_t traj_eps = 500; + +/** Angle epsilon, angle considered to be small enough (f0.16). */ +int16_t traj_aeps = 0x0100; + +/** Angle at which to start going forward (f0.16). */ +uint16_t traj_angle_limit = 0x2000; + +/** Angle at which to start going forward (rad, f8.24). */ +int32_t traj_angle_limit_rad; + +/** Go backward. */ +static uint8_t traj_backward; + +/** Go to position. */ +static uint32_t traj_goto_x, traj_goto_y; + +/** Go to angle. */ +static uint32_t traj_goto_a; + +/** Initialise computed factors. */ +void +traj_init (void) +{ + traj_set_angle_limit (traj_angle_limit); +} + +/** Angle offset. Directly handled to speed layer. */ +void +traj_angle_offset_start (int32_t angle, uint8_t seq) +{ + int32_t a = fixed_mul_f824 (angle, 2 * PI_F824); + uint32_t f = postrack_footing; + int32_t arc = fixed_mul_f824 (f, a); + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + speed_alpha.pos_cons += arc; + state_start (&state_main, MODE_SPEED, seq); +} + +/** Go to the wall mode. */ +static void +traj_ftw (void) +{ + uint8_t left, right; + int16_t speed; + speed = speed_theta.slow; + speed *= 256; + if (!traj_backward) + { + left = !IO_GET (CONTACT_FRONT_LEFT_IO); + right = !IO_GET (CONTACT_FRONT_RIGHT_IO); + } + else + { + speed = -speed; + left = !IO_GET (CONTACT_BACK_LEFT_IO); + right = !IO_GET (CONTACT_BACK_RIGHT_IO); + } + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 0; + if (!left && !right) + { + /* Backward. */ +#ifndef HOST + /* No angular control. */ + state_main.variant = 2; +#endif + } + else if (!left || !right) + { +#ifndef HOST + /* No angular control. */ + state_main.variant = 2; +#else + /* On host, we must do the job. */ + speed_theta.cons = speed / 4; + if (left) + speed_alpha.cons = speed / 2; + else + speed_alpha.cons = -speed / 2; +#endif + } + else + { + /* Stay here. */ + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cur; + speed_alpha.pos_cons = pos_alpha.cur; + speed_theta.cur = 0; + speed_alpha.cur = 0; + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start go to the wall mode. */ +void +traj_ftw_start (uint8_t backward, uint8_t seq) +{ + traj_mode = TRAJ_FTW; + traj_backward = backward; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to position mode. */ +static void +traj_goto (void) +{ + int32_t dx = traj_goto_x - postrack_x; + int32_t dy = traj_goto_y - postrack_y; + if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8 + && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8) + { + /* Near enough, stop, let speed terminate the movement. */ + state_main.mode = MODE_SPEED; + traj_mode = TRAJ_DONE; + } + else + { + /* Projection of destination in robot base. */ + int32_t c = fixed_cos_f824 (postrack_a); + int32_t s = fixed_sin_f824 (postrack_a); + int32_t dt = fixed_mul_f824 (dx, c) + fixed_mul_f824 (dy, s); + int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s); + /* Compute arc length. */ + int32_t arad = atan2 (da, dt) * (1L << 24); + if (traj_backward & TRAJ_BACKWARD) + { + if (arad > 0) + arad = - PI_F824 + arad; + else + arad = PI_F824 + arad; + } + if (traj_backward & TRAJ_REVERT_OK) + { + if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = - PI_F824 + arad; + else if (arad < - TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = PI_F824 + arad; + } + int32_t arc = fixed_mul_f824 (arad, postrack_footing); + /* Compute consign. */ + speed_alpha.pos_cons = pos_alpha.cur; + speed_alpha.pos_cons += arc; + if (UTILS_ABS (arad) < traj_angle_limit_rad) + { + speed_theta.pos_cons = pos_theta.cur; + speed_theta.pos_cons += dt >> 8; + } + else + { + speed_theta.pos_cons = pos_theta.cons; + } + } +} + +/** Start go to position mode (x, y: f24.8). */ +void +traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq) +{ + traj_mode = TRAJ_GOTO; + traj_goto_x = x; + traj_goto_y = y; + traj_backward = backward; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to angle mode. */ +static void +traj_goto_angle (void) +{ + /* There is some tricky parts to handle rotation direction. */ + int16_t da = (uint16_t) (traj_goto_a >> 8) - (uint16_t) (postrack_a >> 8); + if (UTILS_ABS (da) < traj_aeps) + { + /* Near enough, stop, let speed terminate the movement. */ + state_main.mode = MODE_SPEED; + traj_mode = TRAJ_DONE; + } + else + { + /* Compute arc length. */ + int32_t arad = fixed_mul_f824 (((int32_t) da) << 8, + 2 * PI_F824); + int32_t arc = fixed_mul_f824 (arad, postrack_footing); + /* Compute consign. */ + speed_alpha.pos_cons = pos_alpha.cur; + speed_alpha.pos_cons += arc; + } +} + +/** Start go to angle mode (a: f8.24). */ +void +traj_goto_angle_start (uint32_t a, uint8_t seq) +{ + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_a = a; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to position, then angle mode. */ +static void +traj_goto_xya (void) +{ + int32_t dx = traj_goto_x - postrack_x; + int32_t dy = traj_goto_y - postrack_y; + if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8 + && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8) + { + /* Near enough, now do a go to angle. */ + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_angle (); + } + else + { + traj_goto (); + } +} + +/** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */ +void +traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward, + uint8_t seq) +{ + traj_mode = TRAJ_GOTO_XYA; + traj_goto_x = x; + traj_goto_y = y; + traj_goto_a = a; + traj_backward = backward; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/* Compute new speed according the defined trajectory. */ +void +traj_update (void) +{ + switch (traj_mode) + { + case TRAJ_FTW: + traj_ftw (); + break; + case TRAJ_GOTO: + traj_goto (); + break; + case TRAJ_GOTO_ANGLE: + traj_goto_angle (); + break; + case TRAJ_GOTO_XYA: + traj_goto_xya (); + break; + case TRAJ_DONE: + break; + } +} + +/* Set angle limit. */ +void +traj_set_angle_limit (uint16_t a) +{ + traj_angle_limit = a; + traj_angle_limit_rad = (uint32_t) a * (uint32_t) ((1 << 8) * M_PI * 2); +} + diff --git a/digital/mimot/src/dirty/traj.h b/digital/mimot/src/dirty/traj.h new file mode 100644 index 00000000..268ccd1c --- /dev/null +++ b/digital/mimot/src/dirty/traj.h @@ -0,0 +1,61 @@ +#ifndef traj_h +#define traj_h +/* traj.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#define TRAJ_BACKWARD 1 +#define TRAJ_REVERT_OK 2 + +extern uint8_t traj_mode; +extern int16_t traj_eps; +extern int16_t traj_aeps; +extern uint16_t traj_angle_limit; + +void +traj_init (void); + +void +traj_update (void); + +void +traj_angle_offset_start (int32_t angle, uint8_t seq); + +void +traj_ftw_start (uint8_t backward, uint8_t seq); + +void +traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq); + +void +traj_goto_angle_start (uint32_t a, uint8_t seq); + +void +traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward, + uint8_t seq); + +void +traj_set_angle_limit (uint16_t a); + +#endif /* traj_h */ diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index 3048c594..1de7d30d 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -36,7 +36,8 @@ #include "pwm.h" #include "pos.h" #include "speed.h" -#include "aux.h" +#include "postrack.h" +#include "traj.h" #ifdef HOST # include "simu.host.h" @@ -73,19 +74,23 @@ twi_proto_update (void) while ((read_data = twi_slave_poll (buf, sizeof (buf)))) twi_proto_callback (buf, read_data); /* Update status. */ - u8 status_with_crc[8]; + u8 status_with_crc[12]; u8 *status = &status_with_crc[1]; status[0] = 0 - | (state_aux[1].blocked << 3) - | (state_aux[1].finished << 2) - | (state_aux[0].blocked << 1) - | (state_aux[0].finished << 0); + | (speed_theta.cur < 0 ? (1 << 3) : 0) + | (speed_theta.cur > 0 ? (1 << 2) : 0) + | (state_main.blocked << 1) + | (state_main.finished << 0); status[1] = PINC; status[2] = twi_proto.seq; - status[3] = v16_to_v8 (aux[0].pos, 1); - status[4] = v16_to_v8 (aux[0].pos, 0); - status[5] = v16_to_v8 (aux[1].pos, 1); - status[6] = v16_to_v8 (aux[1].pos, 0); + status[3] = v32_to_v8 (postrack_x, 3); + status[4] = v32_to_v8 (postrack_x, 2); + status[5] = v32_to_v8 (postrack_x, 1); + status[6] = v32_to_v8 (postrack_y, 3); + status[7] = v32_to_v8 (postrack_y, 2); + status[8] = v32_to_v8 (postrack_y, 1); + status[9] = v32_to_v8 (postrack_a, 2); + status[10] = v32_to_v8 (postrack_a, 1); /* Compute CRC. */ status_with_crc[0] = crc_compute (&status_with_crc[1], sizeof (status_with_crc) - 1); @@ -115,40 +120,68 @@ twi_proto_callback (u8 *buf, u8 size) /* Reset. */ utils_reset (); break; - case c ('b', 3): - /* Move the aux0. - * - w: new position. - * - b: speed. */ - speed_aux[0].max = buf[4]; - aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0); + case c ('w', 0): + /* Set zero pwm. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); break; - case c ('B', 1): - /* Find the zero position of the aux0. - * - b: speed. */ - aux_traj_find_limit_start (&aux[0], buf[2], 0); + case c ('s', 0): + /* Stop (set zero speed). */ + state_main.mode = MODE_SPEED; + state_main.variant = 0; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = 0; + speed_alpha.cons = 0; break; - case c ('c', 3): - /* Move the aux1. - * - w: new position. - * - b: speed. */ - speed_aux[1].max = buf[4]; - aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0); + case c ('l', 3): + /* Set linear speed controlled position consign. + * - 3b: theta consign offset. */ + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + if (buf[2] & 0x80) + speed_theta.pos_cons += v8_to_v32 (0xff, buf[2], buf[3], buf[4]); + else + speed_theta.pos_cons += v8_to_v32 (0, buf[2], buf[3], buf[4]); + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_SPEED, 0); break; - case c ('C', 1): - /* Find the zero position of the aux1. - * - b: speed. */ - aux_traj_find_limit_start (&aux[1], buf[2], 0); + case c ('a', 2): + /* Set angular speed controlled position consign. + * - w: angle offset. */ + traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0); break; - case c ('l', 4): - /* Clamp. - * - b: aux index. - * - b: speed. - * - w: claming PWM. */ - if (buf[2] < AC_ASSERV_AUX_NB) - aux_traj_clamp_start (&aux[buf[2]], buf[3], - v8_to_v16 (buf[4], buf[5]), 0); - else - buf[0] = 0; + case c ('f', 1): + /* Go to the wall. + * - b: 0: forward, 1: backward. */ + traj_ftw_start (buf[2], 0); + break; + case c ('x', 7): + /* Go to position. + * - 3b: x position. + * - 3b: y position. + * - b: backward (see traj.h). */ + traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), + v8_to_v32 (buf[5], buf[6], buf[7], 0), + buf[8], 0); + break; + case c ('y', 2): + /* Go to angle. + * - w: angle. */ + traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0); + break; + case c ('X', 9): + /* Go to position, then angle. + * - 3b: x position. + * - 3b: y position. + * - w: angle. + * - b: backward (see traj.h). */ + traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), + v8_to_v32 (buf[5], buf[6], buf[7], 0), + v8_to_v32 (0, buf[8], buf[9], 0), + buf[10], 0); break; case c ('p', x): /* Set parameters. */ @@ -173,6 +206,44 @@ twi_proto_params (u8 *buf, u8 size) size--; switch (*buf++) { + case 'X': + /* Set current X position. + * - 3b: X position. */ + if (size < 3) + return 1; + postrack_x = v8_to_v32 (buf[0], buf[1], buf[2], 0); + eat = 3; + break; + case 'Y': + /* Set current Y position. + * - 3b: Y position. */ + if (size < 3) + return 1; + postrack_y = v8_to_v32 (buf[0], buf[1], buf[2], 0); + eat = 3; + break; + case 'A': + /* Set current angle. + * - w: angle. */ + if (size < 2) + return 1; + postrack_a = v8_to_v32 (0, buf[0], buf[1], 0); + eat = 2; + break; + case 's': + /* Set maximum and slow speed. + * - b: theta max. + * - b: alpha max. + * - b: theta slow. + * - b: alpha slow. */ + if (size < 4) + return 1; + speed_theta.max = buf[0]; + speed_alpha.max = buf[1]; + speed_theta.slow = buf[2]; + speed_alpha.slow = buf[3]; + eat = 4; + break; default: return 1; } diff --git a/digital/mimot/tools/inter_mimot.py b/digital/mimot/tools/inter_mimot.py new file mode 100644 index 00000000..219c432b --- /dev/null +++ b/digital/mimot/tools/inter_mimot.py @@ -0,0 +1,136 @@ +# inter_mimot - Mimot interface. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# }}} +"""Inter, communicating with the mimot board.""" +import math + +import mimot +import mimot.init +import proto.popen_io +import serial + +from inter.inter import Inter +from Tkinter import * + +class InterMimot (Inter): + """Inter, communicating with the mimot board.""" + + def __init__ (self, argv): + # Asserv. + if argv[0] == '!': + io = proto.popen_io.PopenIO (argv[1:]) + i = mimot.init.host + else: + io = serial.Serial (argv[0]) + i = mimot.init.target + self.a = mimot.Proto (io, **i) + self.a.async = True + # Inter. + Inter.__init__ (self) + self.tk.createfilehandler (self.a, READABLE, self.read) + self.timeout () + # Query position. + self.a.register_pos () + self.a.position.register (self.pos) + + def createWidgets (self): + Inter.createWidgets (self) + self.ArggButton = Button (self.rightFrame, text = 'Argggg!', + command = self.emergency_stop) + self.ArggButton.pack () + self.actionVar = StringVar () + self.actionVar.set ('goto') + self.actionSetPosButton = Radiobutton (self.rightFrame, + text = 'set pos', value = 'set_pos', + variable = self.actionVar) + self.actionSetPosButton.pack () + self.actionGotoButton = Radiobutton (self.rightFrame, + text = 'goto', value = 'goto', + variable = self.actionVar) + self.actionGotoButton.pack () + self.backwardVar = IntVar () + self.backwardButton = Checkbutton (self.rightFrame, + text = 'backward', variable = self.backwardVar) + self.backwardButton.pack () + self.revertokVar = IntVar () + self.revertokButton = Checkbutton (self.rightFrame, + text = 'revert ok', variable = self.revertokVar) + self.revertokButton.pack () + self.ftwButton = Button (self.rightFrame, text = 'FTW', + command = self.ftw) + self.ftwButton.pack () + + self.tableview.configure (cursor = 'crosshair') + self.tableview.bind ('<1>', self.button1) + self.tableview.bind ('<3>', self.button3) + + def read (self, file, mask): + """Handle mimot events.""" + self.a.proto.read () + self.a.proto.sync () + + def timeout (self): + self.a.proto.sync () + self.after (100, self.timeout) + + def pos (self): + p = self.a.position + self.tableview.robot.pos = p.pos + self.tableview.robot.angle = p.angle + self.tableview.robot.update () + self.update () + + def button1 (self, ev): + x, y = self.tableview.screen_coord ((ev.x, ev.y)) + action = self.actionVar.get () + if action == 'set_pos': + self.a.set_pos (x, y) + elif action == 'goto': + self.a.goto (x, y, self.backwardVar.get (), + self.revertokVar.get ()) + else: + assert 0 + + def button3 (self, ev): + x, y = self.tableview.screen_coord ((ev.x, ev.y)) + a = math.atan2 (y - self.tableview.robot.pos[1], + x - self.tableview.robot.pos[0]) + action = self.actionVar.get () + if action == 'set_pos': + self.a.set_pos (a = a) + elif action == 'goto': + self.a.goto_angle (a) + + def emergency_stop (self): + self.a.free () + + def ftw (self): + self.a.ftw (self.backwardVar.get ()) + +if __name__ == '__main__': + import sys + app = InterMimot (sys.argv[1:]) + try: + app.mainloop () + finally: + app.a.close () diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index 116cebb5..471a27bb 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -1,20 +1,21 @@ """Default parameters for asserv.""" host = dict ( - a0kp = 4, - a0a = 16, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, - a1kp = 4, - a1a = 16, a1sm = 0x60, a1ss = 0x10, - a1be = 256, a1bs = 0x18, a1bc = 5, + scale = 0.0395840674352314, f = 0xdd1, + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x20, tss = 0x10, + akp = 2, akd = 16, + aa = 0.25, asm = 0x20, ass = 0x10, E = 0x3ff, D = 0x1ff, + l = 0x1000, ) target = dict ( - a0kp = 4, - a0a = 16, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, - a1kp = 4, - a1a = 16, a1sm = 0x60, a1ss = 0x10, - a1be = 256, a1bs = 0x18, a1bc = 5, + scale = 0.0415178942124, f = 0xcef, + c = float (0x00ffbabf) / (1 << 24), + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x20, tss = 0x10, + akp = 2, akd = 16, + aa = 0.25, asm = 0x20, ass = 0x10, E = 0x3ff, D = 0x1ff, - w = 0x03, + l = 0x1000, + w = 0x00, ) diff --git a/digital/mimot/tools/mimot/mex.py b/digital/mimot/tools/mimot/mex.py index 5724189e..f39d3ebf 100644 --- a/digital/mimot/tools/mimot/mex.py +++ b/digital/mimot/tools/mimot/mex.py @@ -29,75 +29,43 @@ import simu.mex.msg class Mex: """Handle communications with simulated mimot.""" - class Aux (Observable): - """Auxiliary motor angle. + class Position (Observable): + """Robot position. + - pos: (x, y) millimeters. - angle: radian. """ - def __init__ (self): + def __init__ (self, node, instance): Observable.__init__ (self) + self.pos = None self.angle = None + node.register (instance + ':position', self.__handle) - class Pack: - """Handle reception of several Aux for one message.""" + def __handle (self, msg): + x, y, a = msg.pop ('hhl') + self.pos = (x, y) + self.angle = float (a) / 1024 + self.notify () - def __init__ (self, node, instance, list): - self.__list = list - node.register (instance + ':aux', self.__handle) + class PWM (Observable): + """Motor PWM. - def __handle (self, msg): - angles = msg.pop ('%dl' % len (self.__list)) - for aux, angle in zip (self.__list, angles): - aux.angle = float (angle) / 1024 - aux.notify () - - class Limits (Observable): - """Motor limits. - - - min, max: limits in radian. + - pwm: current PWM values (hardware unit). """ - def __init__ (self, pack, index): + def __init__ (self, node, instance): Observable.__init__ (self) - self.pack = pack - self.index = index - self.min = None - self.max = None - self.register (self.__notified) - - def __notified (self): - self.pack.set (self.index, self.min, self.max) - - class Pack: - """Handle emission of several limits for one message.""" - - def __init__ (self, node, instance): - self.node = node - self.mtype = node.reserve (instance + ':limits') - self.limits = [ None, None, None, None ] - - def set (self, index, min, max): - self.limits[index * 2] = min - self.limits[index * 2 + 1] = max - self.__send () + self.pwm = None + node.register (instance + ':pwm', self.__handle) - def __send (self): - m = simu.mex.msg.Msg (self.mtype) - for l in self.limits: - if l is None: - li = -1 - else: - li = int (l * 1024) - m.push ('l', li) - self.node.send (m) + def __handle (self, msg): + self.pwm = msg.pop ('hh') + self.notify () def __init__ (self, node, instance = 'mimot0'): - self.aux = (self.Aux (), self.Aux ()) - self.__aux_pack = self.Aux.Pack (node, instance, self.aux) - self.__limits_pack = self.Limits.Pack (node, instance) - for index, aux in enumerate (self.aux): - aux.limits = self.Limits (self.__limits_pack, index) + self.position = self.Position (node, instance) + self.pwm = self.PWM (node, instance) diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py index 013a85ff..665947f5 100644 --- a/digital/mimot/tools/mimot/mimot.py +++ b/digital/mimot/tools/mimot/mimot.py @@ -44,40 +44,62 @@ class Proto: # be in the same order than in mimot program. stats_order = 'CSPW' stats_items = { - 'a0c': ('C', 0), - 'a1c': ('C', 1), - 'a0s': ('S', 0), - 'a1s': ('S', 1), - 'a0e': ('P', 0), - 'a0i': ('P', 1), - 'a1e': ('P', 2), - 'a1i': ('P', 3), - 'a0w': ('W', 0), - 'a1w': ('W', 1), + 'lc': ('C', 0), + 'rc': ('C', 1), + 'ts': ('S', 0), + 'as': ('S', 1), + 'te': ('P', 0), + 'ti': ('P', 1), + 'ae': ('P', 2), + 'ai': ('P', 3), + 'lw': ('W', 0), + 'rw': ('W', 1), } - _index = dict (a0 = 0, a1 = 1) + _index = dict (t = ord ('t'), a = ord ('a')) + + class Position (Observable): + """An observable position. To be used with register_pos. + + - pos: (x, y) millimeters. + - angle: radian. + + """ + + def __init__ (self): + Observable.__init__ (self) + self.pos = None + self.angle = None + + def handle (self, x, y, a): + """Update position and notify observers.""" + self.pos = (x, y) + self.angle = a + self.notify () def __init__ (self, file, time = time.time, **param): """Initialise communication and send parameters to asserv.""" self.proto = proto.Proto (file, time, 0.1) self.async = False - self.aseq = [ 0, 0 ] - self.aseq_ack = [ 0, 0 ] - self.proto.register ('A', 'BB', self.__handle_ack) + self.mseq = 0 + self.mseq_ack = 0 + self.proto.register ('A', 'B', self.__handle_ack) def make_handle (s): return lambda *args: self.__handle_stats (s, *args) for (s, f) in self.stats_format.iteritems (): self.proto.register (s, f, make_handle (s)) self.stats_enabled = None self.param = dict ( - a0kp = 0, a0ki = 0, a0kd = 0, - a0a = 1, a0sm = 0, a0ss = 0, - a0be = 2048, a0bs = 0x10, a0bc = 20, - a1kp = 0, a1ki = 0, a1kd = 0, - a1a = 1, a1sm = 0, a1ss = 0, - a1be = 2048, a1bs = 0x10, a1bc = 20, + scale = 1, + tkp = 0, tki = 0, tkd = 0, + ta = 1, tsm = 0, tss = 0, + tbe = 2048, tbs = 0x10, tbc = 20, + akp = 0, aki = 0, akd = 0, + aa = 1, asm = 0, ass = 0, + abe = 2048, abs = 0x10, abc = 20, E = 1023, I = 1023, D = 1023, + c = 1, f = 0x1000, + l = 0x2000, w = 0x00, ) self.param.update (param) @@ -135,39 +157,114 @@ class Proto: def consign (self, w, c): """Consign offset.""" - self.proto.send ('c', 'Bh', self._index[w], c) + if w == 't': + self.proto.send ('c', 'hh', c, 0) + elif w == 'a': + self.proto.send ('c', 'hh', 0, c) + else: + raise IndexError def speed (self, w, s): """Speed consign.""" - self.proto.send ('s', 'Bb', self._index[w], s) + if w == 't': + self.proto.send ('s', 'bb', s, 0) + elif w == 'a': + self.proto.send ('s', 'bb', 0, s) + else: + raise IndexError def speed_pos (self, w, offset): """Speed controlled position consign.""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('s', 'BlB', i, offset, self.aseq[i]) + if w == 't': + self.mseq += 1 + self.proto.send ('s', 'llB', self._dist (offset), 0, self.mseq) + elif w == 'a': + self.mseq += 1 + self.proto.send ('s', 'llB', 0, self.dist (offset), self.mseq) + else: + raise IndexError + self.wait (self.finished, auto = True) + + def speed_angle (self, w, angle): + """Speed controlled angle consign.""" + if w == 'a': + self.mseq += 1 + self.proto.send ('s', 'llB', + 0, int (round (angle * self.param['f'])), self.mseq) + else: + raise IndexError + self.wait (self.finished, auto = True) + + def set_pos (self, x = None, y = None, a = None): + """Set current position.""" + if x is not None: + self.proto.send ('p', 'cl', 'X', self._dist_f248 (x)) + if y is not None: + self.proto.send ('p', 'cl', 'Y', self._dist_f248 (y)) + if a is not None: + self.proto.send ('p', 'cl', 'A', self._angle_f824 (a)) + + def goto (self, x, y, backward = False, revert_ok = False): + """Go to position.""" + self.mseq += 1 + b = 0 + if backward: + b |= 1 + if revert_ok: + b |= 2 + self.proto.send ('x', 'llBB', + self._dist_f248 (x), self._dist_f248 (y), b, self.mseq) self.wait (self.finished, auto = True) - def goto_pos (self, w, pos): - """Go to absolute position.""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('y', 'BhB', i, pos, self.aseq[i]) + def goto_angle (self, a): + """Go to angle.""" + self.mseq += 1 + self.proto.send ('x', 'HB', self._angle_f16 (a), self.mseq) self.wait (self.finished, auto = True) - def clamp (self, w, s, pwm): - """Clamp (speed control, then open loop PWM).""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i]) + def goto_xya (self, x, y, a, backward = False, revert_ok = False): + """Go to position, then angle.""" + self.mseq += 1 + b = 0 + if backward: + b |= 1 + if revert_ok: + b |= 2 + self.proto.send ('x', 'llHBB', + self._dist_f248 (x), self._dist_f248 (y), + self._angle_f16 (a), b, self.mseq) self.wait (self.finished, auto = True) + def ftw (self, backward = True): + """Go to the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) + self.wait (self.finished, auto = True) + + def set_simu_pos (self, x, y, a): + """Set simulated position.""" + self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), + int (round (a * 1024))) + + def register_pos (self, func = None, interval = 225 / 4): + """Will call func each time a position is received. If no function is + provided, use the Position observable object.""" + if func is None: + self.position = self.Position () + self.pos_func = self.position.handle + else: + self.pos_func = func + self.proto.register ('X', 'lll', self.__handle_pos) + self.proto.send ('X', 'B', interval) + def send_param (self): """Send all parameters.""" p = self.param def f88 (x): return int (round (x * (1 << 8))) - for m in ('a0', 'a1'): + def f824 (x): + return int (round (x * (1 << 24))) + for m in ('t', 'a'): index = self._index [m] self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp'])) self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki'])) @@ -179,6 +276,9 @@ class Proto: self.proto.send ('p', 'cH', 'E', p['E']) self.proto.send ('p', 'cH', 'I', p['I']) self.proto.send ('p', 'cH', 'D', p['D']) + self.proto.send ('p', 'cL', 'c', f824 (p['c'])) + self.proto.send ('p', 'cH', 'f', p['f']) + self.proto.send ('p', 'cH', 'l', p['l']) self.proto.send ('p', 'cB', 'w', p['w']) def write_eeprom (self): @@ -196,11 +296,17 @@ class Proto: self.stats_line = [ ] self.stats_count += 1 - def __handle_ack (self, a0seq, a1seq): + def __handle_ack (self, mseq): """Record current acknowledge level and acknowledge reception.""" - self.aseq_ack[0] = a0seq & 0x7f - self.aseq_ack[1] = a1seq & 0x7f - self.proto.send ('a', 'BB', a0seq, a1seq) + self.mseq_ack = mseq & 0x7f + self.proto.send ('a', 'B', mseq) + + def __handle_pos (self, x, y, a): + """Handle position report.""" + x = x / 256 * self.param['scale'] + y = y / 256 * self.param['scale'] + a = a * 2 * math.pi / (1 << 24) + self.pos_func (x, y, a) def wait (self, cond = None, auto = False): """Wait for a condition to become true, or for a number of recorded @@ -216,8 +322,7 @@ class Proto: def finished (self): """Return True if movement commands have been acknowledged.""" - return (self.aseq[0] == self.aseq_ack[0] - and self.aseq[1] == self.aseq_ack[1]) + return self.mseq == self.mseq_ack def free (self): """Coast motors.""" @@ -239,3 +344,15 @@ class Proto: """Return fileno for select() calls.""" return self.proto.fileno () + def _dist (self, d): + return int (round (d / self.param['scale'])) + + def _dist_f248 (self, d): + return int (round ((1 << 8) * d / self.param['scale'])) + + def _angle_f16 (self, a): + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + + def _angle_f824 (self, a): + return int (round ((1 << 24) * a / (2 * math.pi))) + -- cgit v1.2.3