From a86c71e0f36ab75489de3f6aa29fdcf31fa45710 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 2 May 2010 10:08:25 +0200 Subject: digital/io/src: add mimot interface, closes #134 --- digital/io/src/Makefile | 2 +- digital/io/src/mimot.c | 159 ++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/mimot.h | 87 ++++++++++++++++++++++++ digital/io/src/twi_master.c | 2 + 4 files changed, 249 insertions(+), 1 deletion(-) create mode 100644 digital/io/src/mimot.c create mode 100644 digital/io/src/mimot.h diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 8e51b416..c0299735 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -5,7 +5,7 @@ PROGS = io # Sources to compile. io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c main_timer.avr.c servo_pos.c \ - twi_master.c asserv.c \ + twi_master.c asserv.c mimot.c \ simu.host.c contact.c usdist.c radar.c \ path.c food.c # Modules needed for IO. diff --git a/digital/io/src/mimot.c b/digital/io/src/mimot.c new file mode 100644 index 00000000..ac94c517 --- /dev/null +++ b/digital/io/src/mimot.c @@ -0,0 +1,159 @@ +/* mimot.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 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 "mimot.h" + +#include "twi_master.h" + +#include "modules/utils/byte.h" +#include "io.h" + +/** Flag bit positions in mimot status byte. */ +enum mimot_status_flag_t +{ + /** Motor0 movement finished with success. */ + mimot_status_flag_motor0_succeed = 0, + /** Motor0 movement finished with failure. */ + mimot_status_flag_motor0_failed = 1, + /** Motor1 movement finished with success. */ + mimot_status_flag_motor1_succeed = 2, + /** Motor1 movement finished with failure. */ + mimot_status_flag_motor1_failed = 3, +}; + +/** Status structure. */ +struct mimot_status_t +{ + /** Status flags. */ + uint8_t status; + /** Mimot input port. */ + uint8_t input_port; + /** Motor0 position. */ + uint16_t motor0_position; + /** Motor1 position. */ + uint16_t motor1_position; +}; + +/** Current mimot status. */ +struct mimot_status_t mimot_status; + +void +mimot_init (void) +{ + /* Nothing to do. */ +} + +void +mimot_status_cb (uint8_t *status) +{ + /* Parse received data and store them. */ + mimot_status.status = status[0]; + mimot_status.input_port = status[1]; + mimot_status.motor0_position = v8_to_v16 (status[3], status[4]); + mimot_status.motor1_position = v8_to_v16 (status[5], status[6]); +} + +asserv_status_e +mimot_motor0_cmd_status (void) +{ + if (mimot_status.status & _BV (mimot_status_flag_motor0_succeed)) + return success; + else if (mimot_status.status & _BV (mimot_status_flag_motor0_failed)) + return failure; + else + return none; +} + +asserv_status_e +mimot_motor1_cmd_status (void) +{ + if (mimot_status.status & _BV (mimot_status_flag_motor1_succeed)) + return success; + else if (mimot_status.status & _BV (mimot_status_flag_motor1_failed)) + return failure; + else + return none; +} + +uint16_t +mimot_get_motor0_position (void) +{ + return mimot_status.motor0_position; +} + +uint16_t +mimot_get_motor1_position (void) +{ + return mimot_status.motor1_position; +} + +void +mimot_reset (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'z'; + twi_master_send (1); +} + +void +mimot_move_motor0_absolute (uint16_t position, uint8_t speed) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'b'; + buffer[1] = v16_to_v8 (position, 1); + buffer[2] = v16_to_v8 (position, 0); + buffer[3] = speed; + twi_master_send (4); +} + +void +mimot_move_motor1_absolute (uint16_t position, uint8_t speed) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'c'; + buffer[1] = v16_to_v8 (position, 1); + buffer[2] = v16_to_v8 (position, 0); + buffer[3] = speed; + twi_master_send (4); +} + +void +mimot_motor0_zero_position (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'B'; + buffer[1] = 0x10; + twi_master_send (2); +} + +void +mimot_motor1_zero_position (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'C'; + buffer[1] = 0x10; + twi_master_send (2); +} + diff --git a/digital/io/src/mimot.h b/digital/io/src/mimot.h new file mode 100644 index 00000000..58c7a7f9 --- /dev/null +++ b/digital/io/src/mimot.h @@ -0,0 +1,87 @@ +#ifndef mimot_h +#define mimot_h +/* mimot.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 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 "asserv.h" + +/** + * Provide interface to mimot board using the TWI protocol. + */ + +/** Slave number in the twi_master list. */ +#define MIMOT_SLAVE 1 + +/** Mimot TWI address. */ +#define MIMOT_TWI_ADDRESS 6 + +/** Length of status buffer (not including CRC). */ +#define MIMOT_STATUS_LENGTH 7 + +/** Initialise module. */ +void +mimot_init (void); + +/** Called when a new status buffer is received, update the mimot + * information. */ +void +mimot_status_cb (uint8_t *status); + +/** Return motor0 last command status. */ +asserv_status_e +mimot_motor0_cmd_status (void); + +/** Return motor1 last command status. */ +asserv_status_e +mimot_motor1_cmd_status (void); + +/** Get motor0 position in steps. */ +uint16_t +mimot_get_motor0_position (void); + +/** Get motor1 position in steps. */ +uint16_t +mimot_get_motor1_position (void); + +/** Reset mimot board. */ +void +mimot_reset (void); + +/** Move motor0 to absolute position in steps. */ +void +mimot_move_motor0_absolute (uint16_t position, uint8_t speed); + +/** Move motor1 to absolute position in steps. */ +void +mimot_move_motor1_absolute (uint16_t position, uint8_t speed); + +/** Reset motor0 to zero position. */ +void +mimot_motor0_zero_position (void); + +/** Reset motor1 to zero position. */ +void +mimot_motor1_zero_position (void); + +#endif /* mimot_h */ diff --git a/digital/io/src/twi_master.c b/digital/io/src/twi_master.c index 3b7541f3..fec09b10 100644 --- a/digital/io/src/twi_master.c +++ b/digital/io/src/twi_master.c @@ -26,6 +26,7 @@ #include "twi_master.h" #include "asserv.h" +#include "mimot.h" #include "modules/twi/twi.h" #include "modules/utils/utils.h" @@ -98,6 +99,7 @@ struct twi_master_slave_t /** Information on all slaves. */ static struct twi_master_slave_t twi_master_slaves[] = { { ASSERV_TWI_ADDRESS, 0, ASSERV_STATUS_LENGTH, asserv_status_cb }, + { MIMOT_TWI_ADDRESS, 0, MIMOT_STATUS_LENGTH, mimot_status_cb }, }; /** Send first pending message if available. */ -- cgit v1.2.3