From df7d0d94ecc69fa9aadd2ab90db89b5ef1a74030 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 29 Apr 2010 23:03:56 +0200 Subject: digital/mimot/src/dirty: add copy/pasted firmware for mimot, refs #123 This is a quick and dirty firmware copied from asserv. This will be improved once #124 is closed. --- digital/mimot/src/dirty/Makefile | 16 + digital/mimot/src/dirty/aux.c | 196 +++++++++++++ digital/mimot/src/dirty/aux.h | 68 +++++ digital/mimot/src/dirty/avrconfig.h | 104 +++++++ digital/mimot/src/dirty/contacts.h | 32 ++ digital/mimot/src/dirty/counter.h | 37 +++ digital/mimot/src/dirty/counter_ext.avr.c | 142 +++++++++ digital/mimot/src/dirty/eeprom.avr.c | 116 ++++++++ digital/mimot/src/dirty/eeprom.h | 40 +++ digital/mimot/src/dirty/main.c | 453 +++++++++++++++++++++++++++++ digital/mimot/src/dirty/models.host.c | 104 +++++++ digital/mimot/src/dirty/models.host.h | 50 ++++ digital/mimot/src/dirty/motor_model.host.c | 88 ++++++ digital/mimot/src/dirty/motor_model.host.h | 65 +++++ digital/mimot/src/dirty/pos.c | 145 +++++++++ digital/mimot/src/dirty/pos.h | 61 ++++ digital/mimot/src/dirty/pwm.avr.c | 57 ++++ digital/mimot/src/dirty/pwm.h | 98 +++++++ digital/mimot/src/dirty/pwm_config.h | 41 +++ digital/mimot/src/dirty/pwm_ocr.avr.c | 161 ++++++++++ digital/mimot/src/dirty/pwm_ocr.avr.h | 37 +++ digital/mimot/src/dirty/simu.host.c | 261 +++++++++++++++++ digital/mimot/src/dirty/simu.host.h | 48 +++ digital/mimot/src/dirty/speed.c | 130 +++++++++ digital/mimot/src/dirty/speed.h | 57 ++++ digital/mimot/src/dirty/state.c | 29 ++ digital/mimot/src/dirty/state.h | 124 ++++++++ digital/mimot/src/dirty/timer.avr.c | 51 ++++ digital/mimot/src/dirty/timer.h | 41 +++ digital/mimot/src/dirty/twi_proto.c | 173 +++++++++++ digital/mimot/src/dirty/twi_proto.h | 34 +++ 31 files changed, 3059 insertions(+) create mode 100644 digital/mimot/src/dirty/Makefile create mode 100644 digital/mimot/src/dirty/aux.c create mode 100644 digital/mimot/src/dirty/aux.h create mode 100644 digital/mimot/src/dirty/avrconfig.h create mode 100644 digital/mimot/src/dirty/contacts.h create mode 100644 digital/mimot/src/dirty/counter.h create mode 100644 digital/mimot/src/dirty/counter_ext.avr.c create mode 100644 digital/mimot/src/dirty/eeprom.avr.c create mode 100644 digital/mimot/src/dirty/eeprom.h create mode 100644 digital/mimot/src/dirty/main.c create mode 100644 digital/mimot/src/dirty/models.host.c create mode 100644 digital/mimot/src/dirty/models.host.h create mode 100644 digital/mimot/src/dirty/motor_model.host.c create mode 100644 digital/mimot/src/dirty/motor_model.host.h create mode 100644 digital/mimot/src/dirty/pos.c create mode 100644 digital/mimot/src/dirty/pos.h create mode 100644 digital/mimot/src/dirty/pwm.avr.c create mode 100644 digital/mimot/src/dirty/pwm.h create mode 100644 digital/mimot/src/dirty/pwm_config.h create mode 100644 digital/mimot/src/dirty/pwm_ocr.avr.c create mode 100644 digital/mimot/src/dirty/pwm_ocr.avr.h create mode 100644 digital/mimot/src/dirty/simu.host.c create mode 100644 digital/mimot/src/dirty/simu.host.h create mode 100644 digital/mimot/src/dirty/speed.c create mode 100644 digital/mimot/src/dirty/speed.h create mode 100644 digital/mimot/src/dirty/state.c create mode 100644 digital/mimot/src/dirty/state.h create mode 100644 digital/mimot/src/dirty/timer.avr.c create mode 100644 digital/mimot/src/dirty/timer.h create mode 100644 digital/mimot/src/dirty/twi_proto.c create mode 100644 digital/mimot/src/dirty/twi_proto.h (limited to 'digital/mimot/src') diff --git a/digital/mimot/src/dirty/Makefile b/digital/mimot/src/dirty/Makefile new file mode 100644 index 00000000..979aae7e --- /dev/null +++ b/digital/mimot/src/dirty/Makefile @@ -0,0 +1,16 @@ +BASE = ../../../avr +PROGS = dirty +dirty_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c \ + pwm_ocr.avr.c pos.c speed.c aux.c \ + twi_proto.c eeprom.avr.c state.c \ + simu.host.c motor_model.host.c models.host.c +MODULES = proto uart utils math/fixed twi +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega32 +# -O2 : speed +# -Os : size +OPTIMIZE = -Os +HOST_LIBS = -lm + +include $(BASE)/make/Makefile.gen diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c new file mode 100644 index 00000000..d262cba0 --- /dev/null +++ b/digital/mimot/src/dirty/aux.c @@ -0,0 +1,196 @@ +/* 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 "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, + /* 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, + /* 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].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].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 (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; +} + +/** 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); +} + +/** 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_FIND_ZERO_NOT: + case AUX_TRAJ_FIND_ZERO: + aux_traj_find_zero (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 new file mode 100644 index 00000000..65456ec9 --- /dev/null +++ b/digital/mimot/src/dirty/aux.h @@ -0,0 +1,68 @@ +#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; + /** Absolute position. */ + int16_t pos; + /** Trajectory mode. */ + uint8_t traj_mode; + /** Goto position position. */ + uint32_t goto_pos; + /** 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_find_zero_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 new file mode 100644 index 00000000..c7cc73f3 --- /dev/null +++ b/digital/mimot/src/dirty/avrconfig.h @@ -0,0 +1,104 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 0 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 38400 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL DROP +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER STDIO +/** Same thing for secondary port. */ +#define AC_UART1_PORT -1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#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. */ +#define AC_ASSERV_TWI_ADDRESS 6 + +/* twi - TWI module. */ +/** Activate master part. */ +#define AC_TWI_MASTER_ENABLE 0 +/** Activate slave part. */ +#define AC_TWI_SLAVE_ENABLE 1 +/** Slave recv buffer size. */ +#define AC_TWI_SL_RECV_BUFFER_SIZE 16 +/** Slave send buffer size. */ +#define AC_TWI_SL_SEND_BUFFER_SIZE 16 + +#endif /* avrconfig_h */ diff --git a/digital/mimot/src/dirty/contacts.h b/digital/mimot/src/dirty/contacts.h new file mode 100644 index 00000000..1de0c57d --- /dev/null +++ b/digital/mimot/src/dirty/contacts.h @@ -0,0 +1,32 @@ +#ifndef contacts_h +#define contacts_h +/* contacts.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2009 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 contacts. */ +#define CONTACT_AUX0_ZERO_IO C, 0 +#define CONTACT_AUX1_ZERO_IO C, 1 + +#endif /* contacts_h */ diff --git a/digital/mimot/src/dirty/counter.h b/digital/mimot/src/dirty/counter.h new file mode 100644 index 00000000..39d596c5 --- /dev/null +++ b/digital/mimot/src/dirty/counter.h @@ -0,0 +1,37 @@ +#ifndef counter_h +#define counter_h +/* counter.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +extern uint16_t counter_aux[AC_ASSERV_AUX_NB]; +extern int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; + +void +counter_init (void); + +void +counter_update (void); + +#endif /* counter_h */ diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c new file mode 100644 index 00000000..1bbe9a38 --- /dev/null +++ b/digital/mimot/src/dirty/counter_ext.avr.c @@ -0,0 +1,142 @@ +/* counter_ext.avr.c - External counter support. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "counter.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" +#include "io.h" + +/** + * This file add support for an external counter like the hdlcounter or + * avrcounter project. This can be better in order not to loose steps and + * support more counters. + */ + +/** Define the first auxiliary counter address. */ +#define COUNTER_AUX0 0 +/** Define the second auxiliary counter address. */ +#define COUNTER_AUX1 1 + +/** Define to 1 to reverse the first auxiliary counter. */ +#define COUNTER_AUX0_REVERSE 0 +/** Define to 1 to reverse the second auxiliary counter. */ +#define COUNTER_AUX1_REVERSE 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 uint8_t counter_aux_old[AC_ASSERV_AUX_NB]; +/** Overall counter values. */ +uint16_t counter_aux[AC_ASSERV_AUX_NB]; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; + +#if !COUNTER_USE_XMEM +# define COUNTER_ALE_IO B, 4 +# define COUNTER_RD_IO D, 6 +#endif + +/** Read an external counter. */ +static inline uint8_t +counter_read (uint8_t n) +{ +#if COUNTER_USE_XMEM + uint8_t * const ext = (void *) 0x1100; + return ext[n]; +#else + uint8_t v; + PORTA = (PORTA & 0xf0) | (n & 0x0f); + IO_CLR (COUNTER_ALE_IO); + PORTA &= 0xf0; + DDRA &= 0xf0; + DDRB &= 0xf0; + IO_CLR (COUNTER_RD_IO); + utils_nop (); + utils_nop (); + v = (PINA & 0x0f) | (PINB & 0x0f) << 4; + IO_SET (COUNTER_RD_IO); + IO_SET (COUNTER_ALE_IO); + DDRA |= 0x0f; + DDRB |= 0x0f; + return v; +#endif +} + +/** Initialize the counters. */ +void +counter_init (void) +{ +#if COUNTER_USE_XMEM + /* Long wait-states. */ + XMCRA = _BV (SRW11); + /* Do not use port C for address. */ + XMCRB = _BV (XMM2) | _BV (XMM1) | _BV (XMM0); + /* Long wait-states and enable. */ + MCUCR |= _BV (SRE) | _BV (SRW10); +#else + IO_SET (COUNTER_ALE_IO); + IO_SET (COUNTER_RD_IO); + IO_OUTPUT (COUNTER_ALE_IO); + IO_OUTPUT (COUNTER_RD_IO); + PORTA &= 0xf0; + PORTB &= 0xf0; + DDRA |= 0x0f; + DDRB |= 0x0f; +#endif + /* Begin with safe values. */ + counter_aux_old[0] = counter_read (COUNTER_AUX0); + counter_aux_old[1] = counter_read (COUNTER_AUX1); +} + +/** Update overall counter values and compute diffs. */ +void +counter_update (void) +{ + uint8_t aux0, aux1; + /* Sample counters. */ + aux0 = counter_read (COUNTER_AUX0); + aux1 = counter_read (COUNTER_AUX1); + /* First auxiliary counter. */ +#if !COUNTER_AUX0_REVERSE + counter_aux_diff[0] = (int8_t) (aux0 - counter_aux_old[0]); +#else + counter_aux_diff[0] = (int8_t) (counter_aux_old[0] - aux0); +#endif + counter_aux_old[0] = aux0; + counter_aux[0] += counter_aux_diff[0]; + /* Second auxiliary counter. */ +#if !COUNTER_AUX1_REVERSE + counter_aux_diff[1] = (int8_t) (aux1 - counter_aux_old[1]); +#else + counter_aux_diff[1] = (int8_t) (counter_aux_old[1] - aux1); +#endif + counter_aux_old[1] = aux1; + counter_aux[1] += counter_aux_diff[1]; +} + diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c new file mode 100644 index 00000000..a20032d1 --- /dev/null +++ b/digital/mimot/src/dirty/eeprom.avr.c @@ -0,0 +1,116 @@ +/* eeprom.avr.c - Save parameters to internal EEPROM. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "eeprom.h" + +#include "modules/utils/byte.h" + +#include + +#include "counter.h" +#include "pwm.h" +#include "pos.h" +#include "speed.h" + +#define EEPROM_START 0 + +/* WARNING: + * If you change EEPROM format, be sure to change the EEPROM_KEY in header if + * your new format is not compatible with the old one or you will load + * garbages in parameters. */ + +/* Read parameters from eeprom. */ +void +eeprom_read_params (void) +{ + uint8_t *p8 = (uint8_t *) EEPROM_START; + uint16_t *p16; + if (eeprom_read_byte (p8++) != EEPROM_KEY) + return; + speed_aux[0].max = eeprom_read_byte (p8++); + speed_aux[1].max = eeprom_read_byte (p8++); + speed_aux[0].slow = eeprom_read_byte (p8++); + speed_aux[1].slow = eeprom_read_byte (p8++); + pwm_set_reverse (eeprom_read_byte (p8++)); + p16 = (uint16_t *) p8; + speed_aux[0].acc = eeprom_read_word (p16++); + speed_aux[1].acc = eeprom_read_word (p16++); + pos_aux[0].kp = eeprom_read_word (p16++); + pos_aux[1].kp = eeprom_read_word (p16++); + pos_aux[0].ki = eeprom_read_word (p16++); + pos_aux[1].ki = eeprom_read_word (p16++); + pos_aux[0].kd = eeprom_read_word (p16++); + pos_aux[1].kd = eeprom_read_word (p16++); + pos_aux[0].blocked_error_limit = eeprom_read_word (p16++); + pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++); + pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_error_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++); + pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++); + pos_e_sat = eeprom_read_word (p16++); + pos_i_sat = eeprom_read_word (p16++); + pos_d_sat = eeprom_read_word (p16++); +} + +/* Write parameters to eeprom. */ +void +eeprom_write_params (void) +{ + uint8_t *p8 = (uint8_t *) EEPROM_START; + uint16_t *p16; + eeprom_write_byte (p8++, EEPROM_KEY); + eeprom_write_byte (p8++, speed_aux[0].max); + eeprom_write_byte (p8++, speed_aux[1].max); + eeprom_write_byte (p8++, speed_aux[0].slow); + eeprom_write_byte (p8++, speed_aux[1].slow); + eeprom_write_byte (p8++, pwm_reverse); + p16 = (uint16_t *) p8; + eeprom_write_word (p16++, speed_aux[0].acc); + eeprom_write_word (p16++, speed_aux[1].acc); + eeprom_write_word (p16++, pos_aux[0].kp); + eeprom_write_word (p16++, pos_aux[1].kp); + eeprom_write_word (p16++, pos_aux[0].ki); + eeprom_write_word (p16++, pos_aux[1].ki); + eeprom_write_word (p16++, pos_aux[0].kd); + eeprom_write_word (p16++, pos_aux[1].kd); + eeprom_write_word (p16++, pos_aux[0].blocked_error_limit); + eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit); + eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_error_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit); + eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit); + eeprom_write_word (p16++, pos_e_sat); + eeprom_write_word (p16++, pos_i_sat); + eeprom_write_word (p16++, pos_d_sat); +} + +/* Clear eeprom parameters. */ +void +eeprom_clear_params (void) +{ + uint8_t *p = (uint8_t *) EEPROM_START; + eeprom_write_byte (p, 0xff); +} + diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h new file mode 100644 index 00000000..d061a9fd --- /dev/null +++ b/digital/mimot/src/dirty/eeprom.h @@ -0,0 +1,40 @@ +#ifndef eeprom_h +#define eeprom_h +/* eeprom.h - Save parameters to internal EEPROM. */ +/* 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. + * + * }}} */ + +/** Change the eeprom key each time you change eeprom format. */ +#define EEPROM_KEY 0x01 + +void +eeprom_read_params (void); + +void +eeprom_write_params (void); + +void +eeprom_clear_params (void); + +#endif /* eeprom_h */ diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c new file mode 100644 index 00000000..9b674461 --- /dev/null +++ b/digital/mimot/src/dirty/main.c @@ -0,0 +1,453 @@ +/* main.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/math/fixed/fixed.h" +#include "io.h" + +#include "state.h" + +#include "counter.h" +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "aux.h" + +#include "twi_proto.h" +#include "eeprom.h" + +#ifndef HOST +# include "timer.h" +#else +# include "simu.host.h" +#endif + +/** Report command completion. */ +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; + +/** Statistics about speed control. */ +uint8_t main_stat_speed, main_stat_speed_cpt; + +/** Statistics about shaft position control. */ +uint8_t main_stat_pos, main_stat_pos_cpt; + +/** Statistics about pwm values. */ +uint8_t main_stat_pwm, main_stat_pwm_cpt; + +/** Report of timer. */ +uint8_t main_stat_timer, main_stat_timer_cpt; + +/** Print input port. */ +uint8_t main_print_pin, main_print_pin_cpt; + +/** Record timer value at different stage of computing. Used for performance + * analisys. */ +uint8_t main_timer[6]; + +static void +main_loop (void); + +/** Entry point. */ +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + /* Pull-ups. */ + PORTB = 0xe0; + PORTC = 0xfc; + PORTD = 0x80; + pwm_init (); + timer_init (); + counter_init (); + uart0_init (); + twi_proto_init (); + speed_init (); + aux_init (); + eeprom_read_params (); + proto_send0 ('z'); + sei (); + while (1) + main_loop (); + return 0; +} + +/** Main loop. */ +static void +main_loop (void) +{ + main_timer[5] = timer_read (); + timer_wait (); + /* Counter update. */ + counter_update (); + main_timer[0] = timer_read (); + /* Position control. */ + pos_update (); + main_timer[1] = timer_read (); + /* Pwm setup. */ + pwm_update (); + main_timer[2] = timer_read (); + /* Compute absolute position. */ + aux_pos_update (); + /* Compute trajectory. */ + aux_traj_update (); + /* Speed control. */ + speed_update (); + main_timer[3] = timer_read (); + /* 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) + && !--main_sequence_ack_cpt) + { + proto_send2b ('A', state_aux[0].sequence_finish, + state_aux[1].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]); + main_stat_counter_cpt = main_stat_counter; + } + if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) + { + proto_send2w ('Y', aux[0].pos, aux[1].pos); + main_stat_aux_pos_cpt = main_stat_aux_pos; + } + if (main_stat_speed && !--main_stat_speed_cpt) + { + proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].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); + main_stat_pos_cpt = main_stat_pos; + } + if (main_stat_pwm && !--main_stat_pwm_cpt) + { + proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur); + main_stat_pwm_cpt = main_stat_pwm; + } + if (main_stat_timer && !--main_stat_timer_cpt) + { + proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3], + main_timer[4], main_timer[4], main_timer[5]); + main_stat_timer_cpt = main_stat_timer; + } + if (main_print_pin && !--main_print_pin_cpt) + { + proto_send1b ('I', PINC & 0xfc); + main_print_pin_cpt = main_print_pin; + } + /* Misc. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); + twi_proto_update (); + main_timer[4] = timer_read (); +} + +/** Handle incoming messages. */ +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)) + { + case c ('z', 0): + /* Reset. */ + utils_reset (); + 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); + 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])); + 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]); + 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; + 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; + break; + case c ('s', 6): + /* Set auxiliary speed controlled position consign. + * - b: aux index. + * - d: consign offset. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[5] == state->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]); + break; + case c ('y', 4): + /* Auxiliary go to position. + * - b: aux index. + * - w: pos, i16. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[3] == state->sequence) + break; + aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]); + break; + case c ('y', 3): + /* Auxiliary find zero. + * - b: aux index. + * - b: speed. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[2] == state->sequence) + break; + aux_traj_find_zero_start (auxp, args[1], args[2]); + 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[1]); + state_acknowledge (&state_aux[1], args[2]); + break; + /* Stats. + * - b: interval between stats. */ + case c ('A', 1): + /* Command completion report. */ + main_sequence_ack_cpt = main_sequence_ack = args[0]; + break; + case c ('C', 1): + /* 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]; + 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. */ + main_stat_pos_cpt = main_stat_pos = args[0]; + break; + case c ('W', 1): + /* Pwm stats. */ + main_stat_pwm_cpt = main_stat_pwm = args[0]; + break; + case c ('T', 1): + /* Timing stats. */ + main_stat_timer_cpt = main_stat_timer = args[0]; + break; + case c ('I', 1): + /* Input port stats. */ + main_print_pin_cpt = main_print_pin = args[0]; + break; + default: + /* Params. */ + if (cmd == 'p') + { + /* 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]]; + break; + default: + pos = 0; + speed = 0; + break; + } + switch (c (args[0], size)) + { + case c ('a', 4): + /* Set acceleration. + * - b: index. + * - w: acceleration. */ + if (!speed) { proto_send0 ('?'); return; } + speed->acc = v8_to_v16 (args[2], args[3]); + break; + case c ('s', 4): + /* Set maximum and slow speed. + * - b: index. + * - b: max speed. + * - b: slow speed. */ + if (!speed) { proto_send0 ('?'); return; } + speed->max = args[2]; + speed->slow = args[3]; + break; + case c ('p', 4): + /* Set proportional coefficient. + * - b: index. + * - w: P coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kp = v8_to_v16 (args[2], args[3]); + break; + case c ('i', 4): + /* Set integral coefficient. + * - b: index. + * - w: I coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->ki = v8_to_v16 (args[2], args[3]); + break; + case c ('d', 4): + /* Set differential coefficient. + * - b: index. + * - w: D coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kd = v8_to_v16 (args[2], args[3]); + break; + case c ('b', 7): + /* Set blocking detection parameters. + * - b: index. + * - w: error limit. + * - w: speed limit. + * - b: counter limit. */ + if (!pos) { proto_send0 ('?'); return; } + pos->blocked_error_limit = v8_to_v16 (args[2], args[3]); + pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]); + pos->blocked_counter_limit = args[6]; + break; + case c ('E', 3): + pos_e_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('I', 3): + pos_i_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('D', 3): + pos_d_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('w', 2): + /* Set PWM direction. + * - b: bits: 0000[aux1][aux0][right][left]. */ + pwm_set_reverse (args[1]); + break; + case c ('E', 2): + /* Write to eeprom. + * - b: 00: clear config, 01: write config. */ + if (args[1]) + eeprom_write_params (); + else + eeprom_clear_params (); + break; + case c ('P', 1): + /* Print current settings. */ + proto_send1b ('E', EEPROM_KEY); + proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc); + proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow); + proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow); + proto_send3w ('b', pos_aux[0].blocked_error_limit, + pos_aux[0].blocked_speed_limit, + pos_aux[0].blocked_counter_limit); + proto_send3w ('b', pos_aux[1].blocked_error_limit, + pos_aux[1].blocked_speed_limit, + pos_aux[1].blocked_counter_limit); + proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp); + proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki); + proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd); + proto_send1w ('E', pos_e_sat); + proto_send1w ('I', pos_i_sat); + proto_send1w ('D', pos_d_sat); + proto_send1b ('w', pwm_reverse); + break; + default: + proto_send0 ('?'); + return; + } + } + else + { + proto_send0 ('?'); + return; + } + break; + } + proto_send (cmd, size, args); +#undef c +} + diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c new file mode 100644 index 00000000..ab7242db --- /dev/null +++ b/digital/mimot/src/dirty/models.host.c @@ -0,0 +1,104 @@ +/* models.host.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "motor_model.host.h" +#include "models.host.h" +#include "simu.host.h" + +#include +#include + +/* Marcel clamp, with a Faulhaber 2342 and 23/1 3.71:1 gearbox model. */ +static const struct motor_def_t marcel_clamp_f2342_model = +{ + /* Motor characteristics. */ + 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 26.10 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 7.10, /* Terminal resistance (Ohm). */ + 0.265 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 3.71, /* Gearbox ratio. */ + 0.88, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */ + /* This is a pifometric estimation. */ +}; + +/* Marcel, APBTeam 2010. */ +static const struct robot_t marcel_robot = +{ + /** Auxiliary motors, NULL if not present. */ + { &marcel_clamp_f2342_model, &marcel_clamp_f2342_model }, + /** Number of steps for each auxiliary motor encoder. */ + { 512, 512 }, + /** Sensor update function. */ + simu_sensor_update_marcel, +}; + +/* Table of models. */ +static const struct +{ + const char *name; + const struct robot_t *robot; +} models[] = { + { "marcel", &marcel_robot }, + { 0, 0 } +}; + +/** Get a pointer to a model by name, or return 0. */ +const struct robot_t * +models_get (const char *name) +{ + int i; + for (i = 0; models[i].name; i++) + { + if (strcmp (models[i].name, name) == 0) + return models[i].robot; + } + return 0; +} + +/** Initialise simulation models. */ +void +models_init (const struct robot_t *robot, struct motor_t aux_motor[]) +{ + int i; + if (aux_motor) + { + 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; + } + } + } +} + diff --git a/digital/mimot/src/dirty/models.host.h b/digital/mimot/src/dirty/models.host.h new file mode 100644 index 00000000..0e53aaea --- /dev/null +++ b/digital/mimot/src/dirty/models.host.h @@ -0,0 +1,50 @@ +#ifndef models_host_h +#define models_host_h +/* models.host.h */ +/* 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. + * + * }}} */ + +#define ECHANT_PERIOD (1.0 / ((double) AC_FREQ / 256 / 256)) + +/** Define a robot and its peripherals. + * 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]; + /** Sensor update function. */ + void (*sensor_update) (void); +}; + +/** Get a pointer to a model by name, or return 0. */ +const struct robot_t * +models_get (const char *name); + +/** Initialise simulation models. */ +void +models_init (const struct robot_t *robot, struct motor_t aux_motor[]); + +#endif /* models_host_h */ diff --git a/digital/mimot/src/dirty/motor_model.host.c b/digital/mimot/src/dirty/motor_model.host.c new file mode 100644 index 00000000..37171c32 --- /dev/null +++ b/digital/mimot/src/dirty/motor_model.host.c @@ -0,0 +1,88 @@ +/* motor_model.c - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "motor_model.host.h" + +/** + * Switching to french for all those non english speaking people. + * + * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux + * parties, la modélisation électrique et la modélisation mécanique. + * + * On peut trouver de l'aide sur : + * - le site de maxon : http://www.maxonmotor.com/ + * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html + * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm + * + * Voici ce qui en résulte, des belles équations différentielles : + * u(t) = R i(t) + L di(t)/dt + 1/Ke o(t) + * J do(t)/dt = Kt i(t) - Rf o(t) + * dth(t)/dt = o(t) + * + * Les variables sont décrites dans la structure motor_t. + * + * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en + * exercice. + * + * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il + * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive + * à : + * + * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t)) + * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t)) + * th(t+h) = th(t) + h o(t) + * + * C'est consternant de simplicité non ? + */ + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m) +{ + double i_, o_, th_; /* New computed values. */ + double h; /* Infinitesimal step... Well, not so infinite. */ + int d; + d = m->d; + h = m->h / d; + /* Make several small steps to increase precision. */ + for (; d; d--) + { + /* Ah, the mystical power of computation. */ + i_ = m->i + + h * (1.0 / m->m.L * m->u + - m->m.R / m->m.L * m->i + - 1.0 / m->m.Ke / m->m.L * m->o); + o_ = m->o + + h * m->m.i_G * m->m.i_G * m->m.ro_G / m->m.J + * (m->m.Kt * m->i - m->m.Rf * m->o); + th_ = m->th + h * m->o; + /* Ok, now store this step. */ + m->i = i_; + m->o = o_; + m->th = th_; + } + /* Damn! It's finished yet! */ + m->t += m->h; +} + diff --git a/digital/mimot/src/dirty/motor_model.host.h b/digital/mimot/src/dirty/motor_model.host.h new file mode 100644 index 00000000..3a04e15a --- /dev/null +++ b/digital/mimot/src/dirty/motor_model.host.h @@ -0,0 +1,65 @@ +#ifndef motor_model_host_h +#define motor_model_host_h +/* motor_model.host.h - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Motor and load characteristics. */ +struct motor_def_t +{ + /* Motor characteristics. */ + double Ke; /* Speed constant ((rad/s)/V). */ + double Kt; /* Torque constant (N.m/A). */ + double Rf; /* Bearing friction (N.m/(rad/s)). */ + double R; /* Terminal resistance (Ohm). */ + double L; /* Terminal inductance (H). */ + double u_max;/* Maximum voltage (V). */ + /* Gearbox characteristics. */ + double i_G; /* Gearbox ratio. */ + double ro_G;/* Gearbox efficiency. */ + /* Load characteristics. */ + double J; /* Load at gearbox output (kg.m^2). */ +}; + +/** Motor and load characteristics and current data. Angular speed and theta + * are at motor output, not gearbox output. */ +struct motor_t +{ + /* Motor and load characteristics. */ + struct motor_def_t m; + /* Simulation parameters. */ + double h; /* Simulation time step (s). */ + int d; /* Simulation time step division. */ + /* Simulation current state. */ + double t; /* Current time (not really used) (s). */ + double u; /* Current input voltage (V). */ + double i; /* Current current (A). */ + double o; /* Current angular speed (o for omega) (rad/s). */ + double th; /* Current theta (th for theta) (rad). */ +}; + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m); + +#endif /* motor_model_host_h */ diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c new file mode 100644 index 00000000..ef6475c2 --- /dev/null +++ b/digital/mimot/src/dirty/pos.c @@ -0,0 +1,145 @@ +/* pos.c - Position motor control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pos.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "counter.h" +#include "pwm.h" +#include "state.h" + +/** + * This file is responsible for position motor control. The consign is a + * position of the motor shafts, as theta/alpha. Theta is the sum of right + * and left position, alpha is the difference between the right and the left + * position. + * 16 bits are enough as long as there is no long blocking (see 2005 cup!). + */ + +/** Auxiliaries control states. */ +struct pos_t pos_aux[AC_ASSERV_AUX_NB]; + +/** Error saturation. */ +int32_t pos_e_sat = 1023; +/** Integral saturation. */ +int32_t pos_i_sat = 1023; +/** Differential saturation. */ +int32_t pos_d_sat = 1023; + +/** Compute a PID. + * How to compute maximum numbers size: + * Result is 24 bits (16 bits kept after shift). + * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff + * max is 12 bits (can be saturated with d_sat). + * If i_sat == 1023, i max is 11 bits. + * In the final addition, let's give 23 bits to the p part, and 22 bits to the + * i and d part (23b + 22b + 22b => 23b + 23b => 24b). + * Therefore, kp can be 23 - 11 = 12 bits (f4.8). + * ki can be 22 - 11 = 11 bits (f3.8). + * kd can be 22 - 12 = 10 bits (f2.8). + * How to increase this number: + * - lower the shift. + * - bound the value returned. + * - lower e, i & d saturation. */ +static inline int16_t +pos_compute_pid (int32_t e, struct pos_t *pos) +{ + int32_t diff, pid; + /* Saturate error. */ + UTILS_BOUND (e, -pos_e_sat, pos_e_sat); + /* Integral update. */ + pos->i += e; + UTILS_BOUND (pos->i, -pos_i_sat, pos_i_sat); + /* Differential value. */ + diff = e - pos->e_old; + UTILS_BOUND (diff, -pos_d_sat, pos_d_sat); + /* Compute PID. */ + pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd; + /* Save result. */ + pos->e_old = e; + return pid >> 8; +} + +/** Update PWM for a single motor system. */ +static void +pos_update_single (struct state_t *state, struct pos_t *pos, + int16_t counter_diff, struct pwm_t *pwm) +{ + if (state->mode >= MODE_POS) + { + int16_t pid; + int32_t error; + /* Update current shaft position. */ + pos->cur += counter_diff; + /* Compute error. */ + error = pos->cons - pos->cur; + /* Test or blocking. */ + if (UTILS_ABS (error) > pos->blocked_error_limit + && UTILS_ABS (counter_diff) < pos->blocked_speed_limit) + pos->blocked_counter++; + else + pos->blocked_counter = 0; + if (!(state->variant & 4) + && pos->blocked_counter > pos->blocked_counter_limit) + { + /* Blocked. */ + pos_reset (pos); + state_blocked (state); + pwm_set (pwm, 0); + } + else + { + /* Compute PID. */ + pid = pos_compute_pid (error, pos); + /* Update PWM. */ + pwm_set (pwm, pid); + } + } +} + +/** Update PWM according to consign. */ +void +pos_update (void) +{ + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i], + &pwm_aux[i]); +} + +/** Reset position control state. To be called when the position control is + * deactivated. */ +void +pos_reset (struct pos_t *pos) +{ + pos->i = 0; + pos->cur = 0; + pos->cons = 0; + pos->e_old = 0; + pos->blocked_counter = 0; +} + diff --git a/digital/mimot/src/dirty/pos.h b/digital/mimot/src/dirty/pos.h new file mode 100644 index 00000000..514b091c --- /dev/null +++ b/digital/mimot/src/dirty/pos.h @@ -0,0 +1,61 @@ +#ifndef pos_h +#define pos_h +/* pos.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Position control state. */ +struct pos_t +{ + /** Current position. */ + uint32_t cur; + /** Consign position. */ + uint32_t cons; + /** PID coefficients (f8.8, maximum depends on saturation values). */ + uint16_t kp, ki, kd; + /** Current integral value. */ + int32_t i; + /** Last error value. */ + int32_t e_old; + /** Blocking detection: error limit. */ + int32_t blocked_error_limit; + /** Blocking detection: speed limit. */ + int16_t blocked_speed_limit; + /** Blocking detection: counter limit. */ + uint8_t blocked_counter_limit; + /** Count the number of blocked detection. */ + uint8_t blocked_counter; +}; + +extern struct pos_t pos_aux[AC_ASSERV_AUX_NB]; + +extern int32_t pos_e_sat, pos_i_sat, pos_d_sat; + +void +pos_update (void); + +void +pos_reset (struct pos_t *pos); + +#endif /* pos_h */ diff --git a/digital/mimot/src/dirty/pwm.avr.c b/digital/mimot/src/dirty/pwm.avr.c new file mode 100644 index 00000000..37ec34b0 --- /dev/null +++ b/digital/mimot/src/dirty/pwm.avr.c @@ -0,0 +1,57 @@ +/* pwm.avr.c - Handle all PWM generators. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pwm.h" +#include "pwm_ocr.avr.h" + +/** PWM control states. */ +struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { + PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) +}; +/** PWM reverse directions. */ +uint8_t pwm_reverse; + +/** Initialise PWM generators. */ +void +pwm_init (void) +{ + pwm_ocr_init (); +} + +/** Update the hardware PWM values. */ +void +pwm_update (void) +{ + pwm_ocr_update (); +} + +/** Set which PWM is reversed. */ +void +pwm_set_reverse (uint8_t reverse) +{ + pwm_reverse = reverse; + pwm_ocr_set_reverse (reverse); +} + diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h new file mode 100644 index 00000000..a751ee9e --- /dev/null +++ b/digital/mimot/src/dirty/pwm.h @@ -0,0 +1,98 @@ +#ifndef pwm_h +#define pwm_h +/* pwm.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Define the absolute maximum PWM value. */ +#define PWM_MAX 0x3ff + +/** PWM control state. */ +struct pwm_t +{ + /** Current PWM value. */ + int16_t cur; + /** Maximum value. */ + int16_t max; + /** Minimum value (dead zone). */ + int16_t min; +}; + +extern struct pwm_t pwm_aux[AC_ASSERV_AUX_NB]; + +extern uint8_t pwm_reverse; + +/** Define current PWM value for each output. */ +#define PWM_VALUE(x) PWM_VALUE_ (x) +#define PWM_VALUE_(x) PWM_VALUE_ ## x +#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur +#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur + +/** Define maximum PWM value for each output. */ +#define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x) +#define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x +#define PWM_MAX_FOR_pwm_aux0 PWM_MAX +#define PWM_MAX_FOR_pwm_aux1 PWM_MAX + +/** Define minimum PWM value for each output, if the value is less than the + * minimum, use 0. */ +#define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x) +#define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x +#define PWM_MIN_FOR_pwm_aux0 0x10 +#define PWM_MIN_FOR_pwm_aux1 0x10 + +/** Define which bit controls the PWM inversion. */ +#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x) +#define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x +#define PWM_REVERSE_BIT_pwm_aux0 _BV (0) +#define PWM_REVERSE_BIT_pwm_aux1 _BV (1) + +/** State init macro. */ +#define PWM_INIT_FOR(x) \ + { 0, PWM_MAX_FOR (x), PWM_MIN_FOR (x) } + +/** Set PWM value. */ +static inline void +pwm_set (struct pwm_t *pwm, int16_t value) +{ + if (value > pwm->max) + pwm->cur = pwm->max; + else if (value < -pwm->max) + pwm->cur = -pwm->max; + else if (value > -pwm->min && value < pwm->min) + pwm->cur = 0; + else + pwm->cur = value; +} + +void +pwm_init (void); + +void +pwm_update (void); + +void +pwm_set_reverse (uint8_t reverse); + +#endif /* pwm_h */ diff --git a/digital/mimot/src/dirty/pwm_config.h b/digital/mimot/src/dirty/pwm_config.h new file mode 100644 index 00000000..e937002e --- /dev/null +++ b/digital/mimot/src/dirty/pwm_config.h @@ -0,0 +1,41 @@ +#ifndef pwm_config_h +#define pwm_config_h +/* pwm_config.h - Helper for PWM configuration. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* Simplify conditionnal compilation. */ +#define PWM1or2 (defined (PWM1) || defined (PWM2)) +#ifdef PWM1 +# define PWM1c(x) x +#else +# define PWM1c(x) 0 +#endif +#ifdef PWM2 +# define PWM2c(x) x +#else +# define PWM2c(x) 0 +#endif + +#endif /* pwm_config_h */ diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c new file mode 100644 index 00000000..c4cd5279 --- /dev/null +++ b/digital/mimot/src/dirty/pwm_ocr.avr.c @@ -0,0 +1,161 @@ +/* pwm_ocr.avr.c - PWM using internal generator. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pwm_ocr.avr.h" +#include "pwm.h" + +#include "modules/utils/utils.h" +#include "io.h" + +/** Assign PWM outputs. */ +#define PWM1 pwm_aux0 +#define PWM2 pwm_aux1 + +#define PWM1_OCR OCR1A +#define PWM1_OCR_BIT 5 +#define PWM1_DIR 3 +#define PWM2_OCR OCR1B +#define PWM2_OCR_BIT 4 +#define PWM2_DIR 2 + +#define PWM1_BRK_IO A, 4 +#define PWM2_BRK_IO A, 5 + +#include "pwm_config.h" + +/** PWM reverse direction, port D. */ +static uint8_t pwm_ocr_dir_reverse_d; + +/** Initialise PWM generator. */ +void +pwm_ocr_init (void) +{ + /* Phase Correct PWM, TOP = 0x3ff, OCnA & OCnB with positive logic. + * f_IO without prescaler. + * Fpwm = f_IO / (2 * prescaler * (1 + TOP)) = 7200 Hz. */ +#if PWM1or2 + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, FOC1A, FOC1B, WGM11, WGM10, + 1, 0, 1, 0, 0, 0, 1, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 0, 0, 0, 1); + /* Enable PWM and direction outputs in DDRD. */ + DDRD |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR)) + | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR)); + /* Will activate output at first non zero PWM. */ +# ifdef PWM1 + IO_OUTPUT (PWM1_BRK_IO); +# endif +# ifdef PWM2 + IO_OUTPUT (PWM2_BRK_IO); +# endif +#endif +} + +/** Update the hardware PWM values. */ +void +pwm_ocr_update (void) +{ +#if PWM1or2 + uint8_t dir_d; + /* Sample port D. */ + dir_d = PORTD & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR))); +# ifdef PWM1 + uint16_t pwm1; + /* Set PWM1. */ + if (PWM_VALUE (PWM1) == 0) + { + pwm1 = 0; + } + else + { + IO_SET (PWM1_BRK_IO); + if (PWM_VALUE (PWM1) < 0) + { + pwm1 = -PWM_VALUE (PWM1); + } + else + { + dir_d |= _BV (PWM1_DIR); + pwm1 = PWM_VALUE (PWM1); + } + } +# endif /* PWM1 */ +# ifdef PWM2 + uint16_t pwm2; + /* Set PWM2. */ + if (PWM_VALUE (PWM2) == 0) + { + pwm2 = 0; + } + else + { + IO_SET (PWM2_BRK_IO); + if (PWM_VALUE (PWM2) < 0) + { + pwm2 = -PWM_VALUE (PWM2); + } + else + { + dir_d |= _BV (PWM2_DIR); + pwm2 = PWM_VALUE (PWM2); + } + } +# endif /* PWM2 */ +#endif /* PWM1or2 */ + /* Setup registers. */ + /* Here, there could be a problem because OCRx are double buffered, not + * PORTx! */ + /* Another problem arise if the OCR sampling is done between left and + * right OCR: the right PWM is one cycle late. */ + /* A solution could be to use interrupts to update PWM or to synchronise + * general timer with PWM. */ +#if PWM1or2 + dir_d ^= pwm_ocr_dir_reverse_d; + PORTD = dir_d; +# ifdef PWM1 + PWM1_OCR = pwm1; +# endif +# ifdef PWM2 + PWM2_OCR = pwm2; +# endif +#endif /* PWM1or2 */ +} + +void +pwm_ocr_set_reverse (uint8_t reverse) +{ + pwm_reverse = reverse; + pwm_ocr_dir_reverse_d = 0; +#ifdef PWM1 + if (reverse & PWM_REVERSE_BIT (PWM1)) + pwm_ocr_dir_reverse_d |= _BV (PWM1_DIR); +#endif +#ifdef PWM2 + if (reverse & PWM_REVERSE_BIT (PWM2)) + pwm_ocr_dir_reverse_d |= _BV (PWM2_DIR); +#endif +} + diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.h b/digital/mimot/src/dirty/pwm_ocr.avr.h new file mode 100644 index 00000000..cd013824 --- /dev/null +++ b/digital/mimot/src/dirty/pwm_ocr.avr.h @@ -0,0 +1,37 @@ +#ifndef pwm_ocr_avr_h +#define pwm_ocr_avr_h +/* pwm_ocr.avr.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +void +pwm_ocr_init (void); + +void +pwm_ocr_update (void); + +void +pwm_ocr_set_reverse (uint8_t reverse); + +#endif /* pwm_ocr_avr_h */ diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c new file mode 100644 index 00000000..1f0d6394 --- /dev/null +++ b/digital/mimot/src/dirty/simu.host.c @@ -0,0 +1,261 @@ +/* simu.host.c */ +/* 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 "simu.host.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "io.h" + +#include +#include +#include +#include + +#include "pwm.h" +#include "aux.h" + +#include "contacts.h" + +#include "motor_model.host.h" +#include "models.host.h" + +/** Simulate some AVR regs. */ +uint8_t PORTB, PORTC, PORTD, PINC; + +/** Overall counter values. */ +uint16_t counter_aux[AC_ASSERV_AUX_NB]; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +int16_t counter_aux_diff[AC_ASSERV_AUX_NB]; + +/** PWM control states. */ +struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = { + PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1) +}; +/** PWM reverse directions. */ +uint8_t pwm_reverse; + +/* Robot model. */ +const struct robot_t *simu_robot; + +/** Motor models. */ +struct motor_t simu_aux_model[AC_ASSERV_AUX_NB]; + +/** Full counter values. */ +uint32_t simu_counter_aux[AC_ASSERV_AUX_NB]; + +/** Use mex. */ +int simu_mex; + +/** Counter to limit the interval between information is sent. */ +int simu_send_cpt; + +/** Initialise simulation. */ +static void +simu_init (void) +{ + int argc; + char **argv; + host_get_program_arguments (&argc, &argv); + if (argc == 2 && strncmp (argv[0], "-m", 2) == 0) + { + simu_mex = atoi (argv[0] + 2); + if (!simu_mex) simu_mex = 1; + simu_send_cpt = simu_mex; + mex_node_connect (); + argc--; argv++; + } + if (argc != 1) + { + fprintf (stderr, "Syntax: dirty.host [-m[interval]] model\n"); + exit (1); + } + simu_robot = models_get (argv[0]); + if (!simu_robot) + { + fprintf (stderr, "unknown model name: %s\n", argv[0]); + exit (1); + } + models_init (simu_robot, simu_aux_model); +} + +/** Update sensors for Marcel. */ +void +simu_sensor_update_marcel (void) +{ +} + +/** Do a simulation step. */ +static void +simu_step (void) +{ + int i; + double old_aux_th[AC_ASSERV_AUX_NB]; + /* Convert pwm value into voltage. */ + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + simu_aux_model[i].u = simu_aux_model[i].m.u_max + * ((double) pwm_aux[i].cur / (PWM_MAX + 1)); + /* Make one step. */ + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + old_aux_th[i] = simu_aux_model[i].th; + if (simu_robot->aux_motor[i]) + motor_model_step (&simu_aux_model[i]); + } + /* Update auxiliary counter. */ + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + if (simu_robot->aux_motor[i]) + { + uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI) + * 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; + } + } + /* Update sensors. */ + if (simu_robot->sensor_update) + simu_robot->sensor_update (); +} + +/** Send information to the other nodes. */ +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++) + { + 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; + } + if (first || simu_aux_model_changed) + { + m = mex_msg_new (0xc8); + 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]; + } + mex_node_send (m); + } + /* First send done. */ + first = 0; +} + +/** Initialise the timer. */ +void +timer_init (void) +{ + simu_init (); +} + +/** Wait for timer overflow. */ +void +timer_wait (void) +{ + if (simu_mex) + mex_node_wait_date (mex_node_date () + 4); + simu_step (); + if (simu_mex && !--simu_send_cpt) + { + simu_send_cpt = simu_mex; + simu_send (); + } +} + +/** Read timer value. Used for performance analysis. */ +uint8_t +timer_read (void) +{ + return 0; +} + +/** Initialize the counters. */ +void +counter_init (void) +{ +} + +/** Update overall counter values and compute diffs. */ +void +counter_update (void) +{ +} + +/** Initialise PWM generator. */ +void +pwm_init (void) +{ +} + +/** Update the hardware PWM values. */ +void +pwm_update (void) +{ +} + +void +eeprom_read_params (void) +{ +} + +void +eeprom_write_params (void) +{ +} + +void +eeprom_clear_params (void) +{ +} + +void +pwm_set_reverse (uint8_t reverse) +{ + pwm_reverse = reverse; +} + diff --git a/digital/mimot/src/dirty/simu.host.h b/digital/mimot/src/dirty/simu.host.h new file mode 100644 index 00000000..6250bed1 --- /dev/null +++ b/digital/mimot/src/dirty/simu.host.h @@ -0,0 +1,48 @@ +#ifndef simu_host_h +#define simu_host_h +/* simu.host.h */ +/* 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. + * + * }}} */ + +extern uint8_t PORTB, PORTC, PORTD, PINC; + +void +timer_init (void); + +void +timer_wait (void); + +uint8_t +timer_read (void); + +void +simu_sensor_update_giboulee (void); + +void +simu_sensor_update_aquajim (void); + +void +simu_sensor_update_marcel (void); + +#endif /* simu_host_h */ diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c new file mode 100644 index 00000000..f9d4193e --- /dev/null +++ b/digital/mimot/src/dirty/speed.c @@ -0,0 +1,130 @@ +/* speed.c - Speed control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "speed.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "pos.h" +#include "state.h" + +/** + * This file is responsible for speed control. It changes the current shafts + * positions using ramps. It can be controlled by a wanted speed or wanted + * shaft position. + */ + +/** Auxiliaries speed control states. */ +struct speed_t speed_aux[AC_ASSERV_AUX_NB]; + +/** Initialise speed control states. */ +void +speed_init (void) +{ + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_aux[i].pos = &pos_aux[i]; +} + +/** Update current speed according to a speed consign. */ +static void +speed_update_by_speed (struct speed_t *speed) +{ + /* Update current speed. */ + if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) + speed->cur = speed->cons; + else if (speed->cons > speed->cur) + speed->cur += speed->acc; + else + speed->cur -= speed->acc; +} + +/** Compute maximum allowed speed according to: distance left, maximum speed, + * current speed and acceleration. */ +static int16_t +speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) +{ + int16_t s; + /* Compute maximum speed in order to be able to brake in time. + * The "+ 0xff" is to ceil result. + * s = sqrt (2 * a * d) */ + s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8); + /* Apply consign. */ + s = UTILS_MIN (max, s); + /* Apply sign. */ + if (d < 0) + s = -s; + /* Convert to f8.8 and check acceleration. */ + s = s << 8; + UTILS_BOUND (s, cur - acc, cur + acc); + return s; +} + +/** Update current speed according to a position consign. */ +static void +speed_update_by_position (struct speed_t *speed) +{ + int32_t diff = speed->pos_cons - speed->pos->cons; + speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc, + speed->max); +} + +/** Update shaft position consign according to its consign type. */ +static void +speed_update_by (struct speed_t *speed) +{ + if (speed->use_pos) + speed_update_by_position (speed); + else + speed_update_by_speed (speed); + /* Update shaft position. */ + speed->pos->cons += speed->cur >> 8; +} + +/** Update shaft position consign for one motor system. */ +static void +speed_update_single (struct state_t *state, struct speed_t *speed) +{ + if (state->mode >= MODE_SPEED) + { + /* Update speed. */ + speed_update_by (speed); + /* Check for completion. */ + if (state->mode == MODE_SPEED + && speed->use_pos && speed->cur == 0) + state_finish (state); + } +} + +/** Update shaft position consign according to consign. */ +void +speed_update (void) +{ + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_update_single (&state_aux[i], &speed_aux[i]); +} + diff --git a/digital/mimot/src/dirty/speed.h b/digital/mimot/src/dirty/speed.h new file mode 100644 index 00000000..10a0da2b --- /dev/null +++ b/digital/mimot/src/dirty/speed.h @@ -0,0 +1,57 @@ +#ifndef speed_h +#define speed_h +/* speed.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Speed control state. */ +struct speed_t +{ + /** Controlled position. */ + struct pos_t *pos; + /** Current speed, f8.8. */ + int16_t cur; + /** Consign speed, f8.8. */ + int16_t cons; + /** Maximum speed for position consign, u8. */ + int8_t max; + /** Slow speed for position consign, u8. */ + int8_t slow; + /** Acceleration, f8.8. */ + int16_t acc; + /** Consign position. */ + uint32_t pos_cons; + /** Whether to use the consign position (1) or not (0). */ + uint8_t use_pos; +}; + +extern struct speed_t speed_aux[AC_ASSERV_AUX_NB]; + +void +speed_init (void); + +void +speed_update (void); + +#endif /* speed_h */ diff --git a/digital/mimot/src/dirty/state.c b/digital/mimot/src/dirty/state.c new file mode 100644 index 00000000..c6c90409 --- /dev/null +++ b/digital/mimot/src/dirty/state.c @@ -0,0 +1,29 @@ +/* state.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "state.h" + +struct state_t state_aux[AC_ASSERV_AUX_NB]; + diff --git a/digital/mimot/src/dirty/state.h b/digital/mimot/src/dirty/state.h new file mode 100644 index 00000000..4916e691 --- /dev/null +++ b/digital/mimot/src/dirty/state.h @@ -0,0 +1,124 @@ +#ifndef state_h +#define state_h +/* state.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** There is two mechanism to acknowledge long lived command completion. + * + * The first one is dedicated to unreliable and full duplex channels like the + * serial port. It is based on sequence numbers. + * + * The second one is simpler, but can only be used with reliable and half + * duplex channels like the TWI bus. It just use a flag to remember command + * completion witch is reseted when a new command arrives. */ + +/** Control mode. */ +enum state_mode_t +{ + /** Simple PWM setup mode. */ + MODE_PWM, + /** Position control mode. */ + MODE_POS, + /** Speed control mode. */ + MODE_SPEED, + /** Trajectory control mode. */ + MODE_TRAJ, +}; + +/** Current motor state. */ +struct state_t +{ + /** Current motor control mode. */ + uint8_t mode; + /** Control mode variant. + * Used for main motors: + * - bit 0: disable theta position control. + * - bit 1: disable alpha position control. + * - bit 2: disable blocking detection. */ + uint8_t variant; + /** Sequence number of the currently processed command, should be between + * 1 and 127. When a command is received on the serial port it is ignored + * if its sequence number is equal to the current sequence number. In + * this case a duplicated message is inferred. */ + uint8_t sequence; + /** Sequence number of the most recently finished command. When a command + * is finished, the current sequence number is copied to this variable. */ + uint8_t sequence_finish; + /** Sequence number of the most recently acknowledged command. Until this + * is not equal to the last finished command sequence number, a message is + * generated on the serial line. */ + uint8_t sequence_ack; + /** Simpler flag based mechanism, indicates if the last received command + * is finished. */ + uint8_t finished; + /** Wether the motor is blocked. */ + uint8_t blocked; +}; + +/** Auxiliary motor states. */ +extern struct state_t state_aux[AC_ASSERV_AUX_NB]; + +/** Start a new command execution. */ +static inline void +state_start (struct state_t *motor, uint8_t mode, uint8_t sequence) +{ + motor->mode = mode; + motor->variant = 0; + motor->sequence = sequence; + motor->finished = 0; + motor->blocked = 0; +} + +/** Signal the current command completion. */ +static inline void +state_finish (struct state_t *motor) +{ + motor->sequence_finish = motor->sequence; + motor->finished = 1; +} + +/** Signal the current command is blocked, disable motor control until a new + * command is given. */ +static inline void +state_blocked (struct state_t *motor) +{ + motor->sequence_finish = motor->sequence | 0x80; + motor->blocked = 1; + motor->mode = MODE_PWM; +} + +/** Acknowledge a command completion and blocked state. */ +static inline void +state_acknowledge (struct state_t *motor, uint8_t sequence) +{ + motor->sequence_ack = sequence; + if (sequence == motor->sequence_finish) + { + motor->finished = 0; + motor->blocked = 0; + } +} + +#endif /* state_h */ diff --git a/digital/mimot/src/dirty/timer.avr.c b/digital/mimot/src/dirty/timer.avr.c new file mode 100644 index 00000000..eb313603 --- /dev/null +++ b/digital/mimot/src/dirty/timer.avr.c @@ -0,0 +1,51 @@ +/* timer.avr.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/utils/utils.h" +#include "io.h" + +/** Initialise the timer. */ +void +timer_init (void) +{ + TCCR0 = regv (FOC0, WGM00, COM01, COM00, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 0, 0); + /* Fov = F_io / (prescaler * (TOP + 1)) + * TOP = 0xff + * prescaler = 256 + * Tov = 1 / Fov = 4.444 ms */ +} + +/** Wait for timer overflow. */ +void +timer_wait (void) +{ + while (!(TIFR & _BV (TOV0))) + ; + /* Write 1 to clear. */ + TIFR = _BV (TOV0); +} + diff --git a/digital/mimot/src/dirty/timer.h b/digital/mimot/src/dirty/timer.h new file mode 100644 index 00000000..754a2d46 --- /dev/null +++ b/digital/mimot/src/dirty/timer.h @@ -0,0 +1,41 @@ +#ifndef timer_h +#define timer_h +/* timer.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. + * + * }}} */ + +/** Read timer value. Used for performance analysis. */ +extern inline uint8_t +timer_read (void) +{ + return TCNT0; +} + +void +timer_init (void); + +void +timer_wait (void); + +#endif /* timer_h */ diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c new file mode 100644 index 00000000..614536e6 --- /dev/null +++ b/digital/mimot/src/dirty/twi_proto.c @@ -0,0 +1,173 @@ +/* twi_proto.c - Implement the protocol over TWI. */ +/* 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 "twi_proto.h" + +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/utils/crc.h" +#include "modules/twi/twi.h" +#include "io.h" + +#include "state.h" + +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "aux.h" + +#ifdef HOST +# include "simu.host.h" +#endif + +struct twi_proto_t +{ + u8 seq; +}; + +struct twi_proto_t twi_proto; + +static void +twi_proto_callback (u8 *buf, u8 size); + +static u8 +twi_proto_params (u8 *buf, u8 size); + +/** Initialise. */ +void +twi_proto_init (void) +{ + twi_init (AC_ASSERV_TWI_ADDRESS); + twi_proto_update (); +} + +/** Handle received commands and update status. */ +void +twi_proto_update (void) +{ + u8 buf[AC_TWI_SL_RECV_BUFFER_SIZE]; + u8 read_data; + /* Handle incoming command. */ + while ((read_data = twi_sl_poll (buf, sizeof (buf)))) + twi_proto_callback (buf, read_data); + /* Update status. */ + u8 status_with_crc[8]; + u8 *status = &status_with_crc[1]; + status[0] = 0 + | (state_aux[1].blocked << 3) + | (state_aux[1].finished << 2) + | (state_aux[0].blocked << 1) + | (state_aux[0].finished << 0); + 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); + /* Compute CRC. */ + status_with_crc[0] = crc_compute (&status_with_crc[1], + sizeof (status_with_crc) - 1); + twi_sl_update (status_with_crc, sizeof (status_with_crc)); +} + +/** Handle one command. */ +static void +twi_proto_callback (u8 *buf, u8 size) +{ + /* Check CRC. */ + if (crc_compute (buf + 1, size - 1) != buf[0]) + return; + else + { + /* Remove the CRC of the buffer. */ + buf += 1; + size -= 1; + } + + if (buf[0] == twi_proto.seq) + return; +#define c(cmd, size) (cmd) + switch (c (buf[1], 0)) + { + case c ('z', 0): + /* 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); + break; + case c ('B', 1): + /* Find the zero position of the aux0. + * - b: speed. */ + aux_traj_find_zero_start (&aux[0], buf[2], 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); + break; + case c ('C', 1): + /* Find the zero position of the aux1. + * - b: speed. */ + aux_traj_find_zero_start (&aux[1], buf[2], 0); + break; + case c ('p', x): + /* Set parameters. */ + if (twi_proto_params (&buf[2], size - 2) != 0) + buf[0] = 0; + break; + default: + buf[0] = 0; + break; + } + /* Acknowledge. */ + twi_proto.seq = buf[0]; +} + +/* Handle a parameter list of change. */ +static u8 +twi_proto_params (u8 *buf, u8 size) +{ + u8 eat; + while (*buf && size) + { + size--; + switch (*buf++) + { + default: + return 1; + } + buf += eat; + size -= eat; + } + return 0; +} + diff --git a/digital/mimot/src/dirty/twi_proto.h b/digital/mimot/src/dirty/twi_proto.h new file mode 100644 index 00000000..a0746be8 --- /dev/null +++ b/digital/mimot/src/dirty/twi_proto.h @@ -0,0 +1,34 @@ +#ifndef twi_proto_h +#define twi_proto_h +/* twi_proto.h - Implement the protocol over TWI. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +void +twi_proto_init (void); + +void +twi_proto_update (void); + +#endif /* twi_proto_h */ -- cgit v1.2.3