From a9c80bb5dbc4c07c0babcc3152005170892dfa84 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 6 Mar 2008 00:32:06 +0100 Subject: * digital/asserv/src/asserv: - TWI protocol skeleton. --- digital/asserv/src/asserv/Makefile | 4 +- digital/asserv/src/asserv/avrconfig.h | 6 ++ digital/asserv/src/asserv/twi_proto.c | 169 ++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/twi_proto.h | 40 ++++++++ 4 files changed, 217 insertions(+), 2 deletions(-) create mode 100644 digital/asserv/src/asserv/twi_proto.c create mode 100644 digital/asserv/src/asserv/twi_proto.h (limited to 'digital') diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile index 023a8fd1..50ad719a 100644 --- a/digital/asserv/src/asserv/Makefile +++ b/digital/asserv/src/asserv/Makefile @@ -1,9 +1,9 @@ BASE = ../../../avr PROGS = asserv HOST_PROGS = test_motor_model -asserv_SOURCES = main.c simu.host.c motor_model.host.c models.host.c +asserv_SOURCES = main.c twi_proto.c simu.host.c motor_model.host.c models.host.c test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c -MODULES = proto uart utils math/fixed +MODULES = proto uart utils math/fixed twi test_motor_model_MODULES = CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h index fa82ba71..f8ced464 100644 --- a/digital/asserv/src/asserv/avrconfig.h +++ b/digital/asserv/src/asserv/avrconfig.h @@ -86,5 +86,11 @@ /* asserv. */ /** Use external counters. */ #define AC_ASSERV_COUNTER_EXTERNAL 1 +/** TWI address. */ +#define AV_ASSERV_TWI_ADDRESS 4 + +#define TWI_SLAVE_ENABLE 1 +#define TWI_SL_RCPT_SIZE 16 +#define TWI_SL_SEND_SIZE 16 #endif /* avrconfig_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c new file mode 100644 index 00000000..aa5c9fad --- /dev/null +++ b/digital/asserv/src/asserv/twi_proto.c @@ -0,0 +1,169 @@ +/* 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/twi/twi.h" + +struct twi_proto_t +{ + u8 seq; +}; + +struct twi_proto_t twi_proto; + +/** Initialise. */ +void +twi_proto_init (void) +{ + twi_init (AV_ASSERV_TWI_ADDRESS); + twi_proto_update (); +} + +/** Handle received commands and update status. */ +void +twi_proto_update (void) +{ + u8 buf[TWI_SL_RCPT_SIZE]; + /* Handle incoming command. */ + while (twi_sl_poll (buf, sizeof (buf))) + twi_proto_callback (buf, sizeof (buf)); + /* Update status. */ + u8 status[12]; + status[0] = 0; + status[1] = twi_proto.seq; + status[2] = v32_to_v8 (0, 3); + status[3] = v32_to_v8 (0, 2); + status[4] = v32_to_v8 (0, 1); + status[5] = v32_to_v8 (0, 3); + status[6] = v32_to_v8 (0, 2); + status[7] = v32_to_v8 (0, 1); + status[8] = v32_to_v8 (0, 2); + status[9] = v32_to_v8 (0, 1); + status[10] = v16_to_v8 (0, 1); + status[11] = v16_to_v8 (0, 0); + twi_sl_update (status, sizeof (status)); +} + +/** Handle one command. */ +void +twi_proto_callback (u8 *buf, u8 size) +{ +#define c(cmd, size) (cmd) + switch (c (buf[1], 0)) + { + case c ('z', 0): + /* Reset. */ + utils_reset (); + break; + case c ('w', 0): + /* Set zero pwm. */ + break; + case c ('s', 0): + /* Stop (set zero speed). */ + break; + case c ('l', 3): + /* Set linear speed controlled position consign. + * - 3b: theta consign offset. */ + break; + case c ('a', 2): + /* Set linear speed controlled position consign. + * - w: theta consign offset. */ + break; + case c ('f', 0): + /* Go to the wall. */ + break; + case c ('F', 0): + /* Go to the dispenser. */ + break; + case c ('b', 3): + /* Move the arm. + * - w: new position. + * - b: speed. */ + 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. */ +u8 +twi_proto_params (u8 *buf, u8 size) +{ + u8 eat; + while (*buf && size) + { + size--; + switch (*buf++) + { + case 'X': + /* Set current X position. + * - 3b: X position. */ + if (size < 3) + return 1; + eat = 3; + break; + case 'Y': + /* Set current Y position. + * - 3b: Y position. */ + if (size < 3) + return 1; + eat = 3; + break; + case 'A': + /* Set current angle. + * - w: angle. */ + if (size < 2) + return 1; + eat = 2; + break; + case 's': + /* Set maximum and slow speed. + * - b: theta max. + * - b: alpha max. + * - b: theta slow. + * - b: alpha slow. */ + if (size < 4) + return 1; + eat = 4; + break; + default: + return 1; + } + buf += eat; + size -= eat; + } + return 0; +} + diff --git a/digital/asserv/src/asserv/twi_proto.h b/digital/asserv/src/asserv/twi_proto.h new file mode 100644 index 00000000..d7a43abf --- /dev/null +++ b/digital/asserv/src/asserv/twi_proto.h @@ -0,0 +1,40 @@ +#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); + +void +twi_proto_callback (u8 *buf, u8 size); + +u8 +twi_proto_params (u8 *buf, u8 size); + +#endif /* twi_proto_h */ -- cgit v1.2.3