From 872dfd49752b0fe5b7d4e8588c0abaac3829b7dc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 6 Apr 2012 23:20:37 +0200 Subject: digital/mimot: add asserv based program without aux motors --- digital/mimot/src/asserv/Makefile | 22 +++++ digital/mimot/src/asserv/avrconfig.h | 173 +++++++++++++++++++++++++++++++++ digital/mimot/src/asserv/contacts.h | 34 +++++++ digital/mimot/src/asserv/misc.h | 44 +++++++++ digital/mimot/src/asserv/models.host.c | 165 +++++++++++++++++++++++++++++++ digital/mimot/src/asserv/timer.avr.c | 68 +++++++++++++ 6 files changed, 506 insertions(+) create mode 100644 digital/mimot/src/asserv/Makefile create mode 100644 digital/mimot/src/asserv/avrconfig.h create mode 100644 digital/mimot/src/asserv/contacts.h create mode 100644 digital/mimot/src/asserv/misc.h create mode 100644 digital/mimot/src/asserv/models.host.c create mode 100644 digital/mimot/src/asserv/timer.avr.c (limited to 'digital/mimot/src/asserv') diff --git a/digital/mimot/src/asserv/Makefile b/digital/mimot/src/asserv/Makefile new file mode 100644 index 00000000..d8841db4 --- /dev/null +++ b/digital/mimot/src/asserv/Makefile @@ -0,0 +1,22 @@ +BASE = ../../../avr +PROGS = asserv +asserv_SOURCES = main.c timer.avr.c \ + postrack.c traj.c cs.c \ + twi_proto.c eeprom.avr.c seq.c \ + simu.host.c models.host.c +MODULES = proto uart utils math/fixed twi \ + motor/encoder motor/encoder/ext motor/output motor/output/pwm_ocr \ + motor/control_system motor/speed_control motor/pos_control \ + motor/blocking_detection motor/motor_model +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega32 +# -O2 : speed +# -Os : size +OPTIMIZE = -Os +HOST_LIBS = -lm + +vpath %.c ../../../asserv/src/asserv +INCLUDES += -I. -I../../../asserv/src/asserv + +include $(BASE)/make/Makefile.gen diff --git a/digital/mimot/src/asserv/avrconfig.h b/digital/mimot/src/asserv/avrconfig.h new file mode 100644 index 00000000..f6c13b83 --- /dev/null +++ b/digital/mimot/src/asserv/avrconfig.h @@ -0,0 +1,173 @@ +#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 128 +/** 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 0 +/** TWI address. */ +#define AC_ASSERV_TWI_ADDRESS 4 +/** Header file misc.h. */ +#define AC_ASSERV_MISC_H +/** Header file contacts.h. */ +#define AC_ASSERV_CONTACTS_H + +/* twi - TWI module. */ +/** Driver to implement TWI: HARD, SOFT, or USI. */ +#define AC_TWI_DRIVER HARD +/** Do not use interrupts. */ +#define AC_TWI_NO_INTERRUPT 0 +/** TWI frequency, should really be 100 kHz. */ +#define AC_TWI_FREQ 100000 +/** Enable slave part. */ +#define AC_TWI_SLAVE_ENABLE 1 +/** Enable master part. */ +#define AC_TWI_MASTER_ENABLE 0 +/** Use polled slave mode: received data is stored in a buffer which can be + * polled using twi_slave_poll. */ +#define AC_TWI_SLAVE_POLLED 1 +/** Slave reception callback to be defined by the user when not in polled + * mode. */ +#undef AC_TWI_SLAVE_RECV +/** Use internal pull up. */ +#define AC_TWI_PULL_UP 0 +/** Slave reception buffer size. */ +#define AC_TWI_SLAVE_RECV_BUFFER_SIZE 16 +/** Slave transmission buffer size. */ +#define AC_TWI_SLAVE_SEND_BUFFER_SIZE 16 + +/* motor/encoder - Encoder module. */ +/** Use external encoder module. */ +#define AC_ENCODER_USE_EXT 1 + +/* motor/encoder/ext - External encoder module. */ +/** Number of encoders. */ +#define AC_ENCODER_EXT_NB 2 +/** Use external memory hardware interface. */ +#define AC_ENCODER_EXT_USE_XMEM 0 +/** If not using XMEM, address/data bus, see io_bus.h. */ +#define AC_ENCODER_EXT_AD_BUS A, 4, 0, B, 4, 0 +/** If not using XMEM, address latch enable IO. */ +#define AC_ENCODER_EXT_ALE_IO B, 4 +/** If not using XMEM, read enable IO, valid low. */ +#define AC_ENCODER_EXT_RD_IO D, 6 +/** Reverse flag for each encoder (1 to reverse direction). */ +#define AC_ENCODER_EXT_REVERSE 0, 0 +/** Right shift for all encoders to lower resolution. */ +#define AC_ENCODER_EXT_SHIFT 1 +/** For debug purpose only! */ +#define AC_ENCODER_EXT_EXPORT_READ 0 + +/* motor/output - Output module. */ +/** Use Output Compare PWM output. */ +#define AC_OUTPUT_USE_PWM_OCR 1 +/** Use Motor Power PWM output. */ +#define AC_OUTPUT_USE_PWM_MP 0 +/** Define module and module index for each output. */ +#define AC_OUTPUT_LIST (pwm_ocr, 0), (pwm_ocr, 1) + +/* motor/output/pwm_ocr - Output Compare PWM output module. */ +/** For each output, define output parameters: + * + * (timer, ocr, pwm_io, dir_io[, brake_io]) + * + * With: + * - timer: timer number (ex: 1 for TIMER1) + * - ocr: output compare (ex: A for output compare A) + * - mode: compare output mode (ex: 2, see datasheet) + * - pwm_io: corresponding io port (ex: B, 1) + * - dir_io: io port used for direction (ex: B, 2) + * - brake_io: optional io port used for brake (ex: B, 3) + */ +#define AC_OUTPUT_PWM_OCR_LIST \ + (1, A, 2, D,5, D,3, A,4), \ + (1, B, 2, D,4, D,2, A,5) +/** Clock select for each used timer. */ +#define AC_OUTPUT_PWM_OCR_CS_1 0b0001 +/** Waveform Generation Mode for each used timer. */ +#define AC_OUTPUT_PWM_OCR_WGM_1 0b0011 +/** Offset added to PWM value to compensate for H-bridge weakness. */ +#define AC_OUTPUT_PWM_OCR_OFFSET 0x40 + +#endif /* avrconfig_h */ diff --git a/digital/mimot/src/asserv/contacts.h b/digital/mimot/src/asserv/contacts.h new file mode 100644 index 00000000..61f2a371 --- /dev/null +++ b/digital/mimot/src/asserv/contacts.h @@ -0,0 +1,34 @@ +#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_BACK_LEFT_IO C, 7 +#define CONTACT_BACK_RIGHT_IO C, 6 +#define CONTACT_FRONT_LEFT_IO C, 5 +#define CONTACT_FRONT_RIGHT_IO C, 4 + +#endif /* contacts_h */ diff --git a/digital/mimot/src/asserv/misc.h b/digital/mimot/src/asserv/misc.h new file mode 100644 index 00000000..8c1cefc6 --- /dev/null +++ b/digital/mimot/src/asserv/misc.h @@ -0,0 +1,44 @@ +#ifndef misc_h +#define misc_h +/* misc.h - Miscellaneous definitions. */ +/* 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 MISC_SETUP do { \ + /* Pull-ups. */ \ + PORTB = 0xe0; \ + PORTC = 0xfc; \ + PORTD = 0x80; \ +} while (0) + +#define LED_SETUP do { \ +} while (0) + +#define LED1(x) do { \ +} while (0) + +#define LED2(x) do { \ +} while (0) + +#endif /* misc_h */ diff --git a/digital/mimot/src/asserv/models.host.c b/digital/mimot/src/asserv/models.host.c new file mode 100644 index 00000000..27a696ea --- /dev/null +++ b/digital/mimot/src/asserv/models.host.c @@ -0,0 +1,165 @@ +/* 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. + * + * }}} */ +#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */ +#include "common.h" + +#include "modules/utils/utils.h" + +#include "io.h" + +#include "models.host.h" +#include "simu.host.h" + +#include "aux.h" + +#include AC_ASSERV_CONTACTS_H + +#include +#include + +#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } } + +static int +simu_table_test_guybrush (double p_x, double p_y); + +/* Guybrush, APBTeam 2012. */ +static const struct robot_t guybrush_robot = +{ + /* Main motors. */ + &motor_model_def_faulhaber_2657_x9_7, + /* Motors voltage (V). */ + 24.0, + /* Number of steps on the main motors encoders. */ + 2500, + /* Wheel radius (m). */ + 0.065 / 2, + /* Distance between the wheels (m). */ + 0.16, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 1, + /** Encoder wheel radius (m). */ + 0.063 / 2, + /** Distance between the encoders wheels (m). */ + 0.28, + /** No auxiliary motors. */ + { }, { }, { }, + /** Sensor update function. */ + NULL, + /** Table test function, return false if given robot point is not in + * table. */ + simu_table_test_guybrush, + /** Robot corners, from front left, then clockwise. */ + { { 150, 171.5 }, { 150, -171.5 }, { -130, -121.5 }, { -130, 121.5 } }, + /** Initialisation function. */ + NULL, +}; + +/* Table of models. */ +static const struct +{ + const char *name; + const struct robot_t *robot; +} models[] = { + { "guybrush", &guybrush_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, motor_model_t *main_motor_left, + motor_model_t *main_motor_right, motor_model_t aux_motor[]) +{ + int i; + if (main_motor_left) + { + main_motor_left->m = *robot->main_motor; + main_motor_left->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_left->h = ECHANT_PERIOD; + main_motor_left->d = 1000; + } + if (main_motor_right) + { + main_motor_right->m = *robot->main_motor; + main_motor_right->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_right->h = ECHANT_PERIOD; + main_motor_right->d = 1000; + } + 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].m.J = robot->aux_load[i]; + aux_motor[i].h = ECHANT_PERIOD; + aux_motor[i].d = 1000; + } + } + } + if (robot->init) + robot->init (robot, main_motor_left, main_motor_right, aux_motor); +} + +/* Table test for Guybrush. */ +int +simu_table_test_guybrush (double p_x, double p_y) +{ + static const double table_width = 3000.0, table_height = 2000.0; + double x, y; + simu_compute_absolute_position (p_x, p_y, &x, &y); + if (x < 0 || y < 0 || x >= table_width || y >= table_height) + return 0; + /* Totems. */ + if (((x >= 1500 - 400 - 125 && x < 1500 - 400 + 125) + || (x >= 1500 + 400 - 125 && x < 1500 + 400 + 125)) + && (y >= 1000 - 125 && y < 1000 + 125)) + return 0; + /* Boats. */ + double bx = 325 + (400 - 325) * y / 1500. + 22; + if ((x < bx || x >= 3000 - bx) && y < 740) + return 0; + return 1; +} + diff --git a/digital/mimot/src/asserv/timer.avr.c b/digital/mimot/src/asserv/timer.avr.c new file mode 100644 index 00000000..45079ccd --- /dev/null +++ b/digital/mimot/src/asserv/timer.avr.c @@ -0,0 +1,68 @@ +/* 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 "modules/motor/encoder/encoder.h" +#include "io.h" + +/** Top timer value. */ +#define TIMER_TOP 255 +/** Number of steps during wait. */ +#define TIMER_STEPS 4 +/** Size of step. */ +#define TIMER_STEP ((TIMER_TOP + 1) / TIMER_STEPS) + +/** 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) +{ + uint8_t i; + /* Make small steps with counter updates. */ + for (i = 1; i < TIMER_STEPS; i++) + { + while (TCNT0 < i * TIMER_STEP) + ; + encoder_update_step (); + } + /* Wait overflow. */ + while (!(TIFR & _BV (TOV0))) + ; + /* Write 1 to clear. */ + TIFR = _BV (TOV0); +} + -- cgit v1.2.3