From 73c73c4a2c56ce76299c7a0631d4e1c3bae9b720 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Fri, 8 May 2009 17:17:44 +0200 Subject: * digital/io: - remove the old trap module (for the position of the servo motors), - add a new module for the position of the servo motors, - increase number of servo motors supported, - new version of eeprom data structure. --- digital/io/src/Makefile | 4 +- digital/io/src/eeprom.avr.c | 21 +++---- digital/io/src/main.c | 52 +++------------- digital/io/src/servo.avr.c | 5 ++ digital/io/src/servo.h | 4 +- digital/io/src/servo_pos.c | 60 ++++++++++++++++++ digital/io/src/servo_pos.h | 73 ++++++++++++++++++++++ digital/io/src/trap.c | 147 -------------------------------------------- digital/io/src/trap.h | 123 ------------------------------------ 9 files changed, 161 insertions(+), 328 deletions(-) create mode 100644 digital/io/src/servo_pos.c create mode 100644 digital/io/src/servo_pos.h delete mode 100644 digital/io/src/trap.c delete mode 100644 digital/io/src/trap.h diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index c5465f12..48511605 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -3,8 +3,8 @@ BASE = ../../avr # Name of the program to build. PROGS = io # Sources to compile. -io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c pwm.c \ - switch.avr.c chrono.c main_timer.avr.c \ +io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c sharp.c pwm.c \ + switch.avr.c chrono.c main_timer.avr.c servo_pos.c \ simu.host.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed path diff --git a/digital/io/src/eeprom.avr.c b/digital/io/src/eeprom.avr.c index 04aa2b7a..4e4eff39 100644 --- a/digital/io/src/eeprom.avr.c +++ b/digital/io/src/eeprom.avr.c @@ -27,7 +27,7 @@ #include "eeprom.h" #include "servo.h" /* SERVO_NUMBER */ -#include "trap.h" /* trap_high_time_pos */ +#include "servo_pos.h" #include "sharp.h" /* sharp_threshold */ #include /* eeprom_{read,write}_byte */ @@ -48,7 +48,7 @@ * @warning: you must update this value every time you change the structure of * data stored into the EEPROM. */ -#define EEPROM_PARAM_KEY 0x04 +#define EEPROM_PARAM_KEY 0x05 /** @} */ @@ -57,6 +57,7 @@ void eeprom_load_param () { uint8_t compt; + uint8_t pos; uint16_t *ptr16; /* The parameters start at the given address */ @@ -66,12 +67,11 @@ eeprom_load_param () /* Error, stop here */ return; - /* Load trap module data */ - /* Extreme position of the servos motor (high time value of the PWM) */ + /* Load servo pos module data */ for (compt = 0; compt < SERVO_NUMBER; compt++) { - trap_high_time_pos[0][compt] = eeprom_read_byte(ptr8++); - trap_high_time_pos[1][compt] = eeprom_read_byte(ptr8++); + for (pos = 0; pos < SERVO_POS_NUMBER; pos++) + servo_pos_high_time[compt][pos] = eeprom_read_byte(ptr8++); } /* Load sharp module data */ @@ -88,7 +88,7 @@ eeprom_load_param () void eeprom_save_param () { - uint8_t compt; + uint8_t compt, pos; uint16_t *ptr16; /* The parameters start at the given address */ @@ -96,12 +96,11 @@ eeprom_save_param () /* Store the key */ eeprom_write_byte (ptr8++, EEPROM_PARAM_KEY); - /* Store trap module data */ - /* Extreme position of the servos motor (high time value of the PWM) */ + /* Store servo pos module data */ for (compt = 0; compt < SERVO_NUMBER; compt++) { - eeprom_write_byte (ptr8++, trap_high_time_pos[0][compt]); - eeprom_write_byte (ptr8++, trap_high_time_pos[1][compt]); + for (pos = 0; pos < SERVO_POS_NUMBER; pos++) + eeprom_write_byte (ptr8++, servo_pos_high_time[compt][pos]); } /* Store sharp module data */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index a7c9030d..3e1c8ec3 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -39,10 +39,10 @@ #include "asserv.h" /* Functions to control the asserv board */ #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ -#include "trap.h" /* Trap module (trap_* functions) */ #include "fsm.h" /* fsm_* */ #include "giboulee.h" /* team_color */ /* #include "top.h" */ /* top_* */ +#include "servo_pos.h" #include "chrono.h" /* chrono_end_match */ #include "sharp.h" /* sharp module */ #include "pwm.h" @@ -132,8 +132,6 @@ main_init (void) utils_delay_ms (500); /* Asserv communication */ asserv_init (); - /* Trap module */ - trap_init (); /* Switch module */ switch_init (); /* Path module */ @@ -356,39 +354,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) utils_reset (); break; - /* Servo/trap commands */ - case c ('t', 3): - /* Set the high time values of a servo for the vertical and horizontal - * positions using trapdoor module. + /* Servo pos commands */ + case c ('p', (SERVO_POS_NUMBER + 1)): + /* Set the high time values of a servo for the positions * - 1b: servo id number; - * - 1b: high time value (horizontal); - * - 1b: high time value (vertical). + * - 1b: high time value for position 0; + * - ... */ - trap_set_high_time (args[0], args[1], args[2]); - break; - - case c ('T', 1): - { - /* Setup traps to open a path to a destination box. - * - 1b: box identification - */ - switch (args[0]) - /*FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? - TOP_EVENT_jack_removed_from_bot : - TOP_EVENT_jack_inserted_into_bot); - */ - { - case 'o': - trap_open_rear_panel (); - break; - case 'c': - trap_close_rear_panel (); - break; - default: - trap_setup_path_to_box (args[0]); - break; - } - } break; case c ('s', 2): @@ -470,8 +442,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Trap */ for (compt = 0; compt < SERVO_NUMBER; compt++) { - proto_send3b ('t', compt, trap_high_time_pos[0][compt], - trap_high_time_pos[1][compt]); + proto_send ('p', SERVO_POS_NUMBER, + servo_pos_high_time[compt]); } /* Sharp */ for (compt = 0; compt < SHARP_NUMBER; compt++) @@ -489,14 +461,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; } /* FSM commands */ - case c ('g', 2): - /* Start the get samples FSM - * - 1b: the approach angle to face the distributor ; - * - 1b: how many and where to put collected samples ; - */ - /* getsamples_start (args[0] << 8, args[1], 0); */ - break; - case c ('A', 1): { /* Get position stats diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c index 3ba43be5..6689f06a 100644 --- a/digital/io/src/servo.avr.c +++ b/digital/io/src/servo.avr.c @@ -103,6 +103,11 @@ servo_init (void) /* Enable overflow interrupt */ set_bit (TIMSK, TOIE2); + + /* By default, servo init disable all servo. */ + uint8_t i; + for (i = 0; i < SERVO_NUMBER; i++) + servo_set_high_time (i, 0); } /* Set the high time of the input signal of a servo (and its position). */ diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h index e433fba0..b9f7e3ea 100644 --- a/digital/io/src/servo.h +++ b/digital/io/src/servo.h @@ -47,13 +47,15 @@ * same algorithm. When the all servos motor have been taken care of, we * set-up the timer to overflow a certain number of times to wait before * restarting the whole cycle. + * + * All servos are connected to the PORTA of the ATmega. */ /** * Number of servos motor managed by this module. * If you change it, you _must_ update the key of the eeprom module! */ -#define SERVO_NUMBER 6 +#define SERVO_NUMBER 8 /** * Minimum high time for servos. diff --git a/digital/io/src/servo_pos.c b/digital/io/src/servo_pos.c new file mode 100644 index 00000000..d04077fd --- /dev/null +++ b/digital/io/src/servo_pos.c @@ -0,0 +1,60 @@ +/* servo_pos.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2009 Dufour Jérémy + * + * 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 "servo_pos.h" + +uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +void +servo_pos_init (void) +{ + /* Initialize under layer, servo module. */ + servo_init (); +} + +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]) +{ + uint8_t i; + /* If servo exists. */ + if (servo_id < SERVO_NUMBER) + { + /* For each position. */ + for (i = 0; i < SERVO_POS_NUMBER; i++) + servo_pos_high_time[servo_id][i] = high_times[i]; + } +} + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position) +{ + servo_set_high_time (servo_id, servo_pos_high_time[servo_id][position]); +} diff --git a/digital/io/src/servo_pos.h b/digital/io/src/servo_pos.h new file mode 100644 index 00000000..c8e56d2b --- /dev/null +++ b/digital/io/src/servo_pos.h @@ -0,0 +1,73 @@ +#ifndef servo_pos_h +#define servo_pos_h +/* servo_pos.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2009 Dufour Jérémy + * + * 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. + * + * }}} */ + +/** + * Module to control two positions of the servo motor. + * It's an higher interface that uses the servo module but remember the + * positions (for example, two positions, open and close) that they can take. + */ + +#include "servo.h" + +/** + * Number of positions the servo can take. + * @warning if you change this define, you must update the key value in the + * EEPROM module. Otherwise, everything will fail! + */ +#define SERVO_POS_NUMBER 2 + +/** + * Table to store the correspondence between the positions and the high time. + */ +extern uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +/** + * Initialize the servo with positions module. + * @note it also initialize the servo module for you. + */ +void +servo_pos_init (void); + +/** + * Set the high times of a servo for positions. + * @param servo_id the ID of the servo. + * @param high_times the different high times for the positions of the + * servo. + */ +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]); + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position); + +#endif /* servo_pos_h */ diff --git a/digital/io/src/trap.c b/digital/io/src/trap.c deleted file mode 100644 index ed22d2f5..00000000 --- a/digital/io/src/trap.c +++ /dev/null @@ -1,147 +0,0 @@ -/* trap.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Dufour Jérémy - * - * 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 "trap.h" - -/** - * @todo - * - how to manage traps collision for the 3 and 4. - * - who map servo/pin to trap? - */ - -/** - * @defgroup TrapPrivate Trap module private variables and functions - * declarations - * @{ - */ - -/** - * Possible positions of a trap. - * It is used for the index of the table @see trap_high_time_pos. - */ -typedef enum trap_position_e -{ - /** Horizontal. */ - horizontal = 0, - /** Vertical. */ - vertical, - - /** Length of the enum, always left it at the end. */ - lenght -} trap_position_e; - -/** - * Trap high time values for the different positions (horizontal and vertical). - * In this two dimensions table, the first index represent horizontal or - * vertical values. - */ -uint8_t trap_high_time_pos[lenght][SERVO_NUMBER]; - -/** @} */ - -/* Trap module initialization. */ -void -trap_init (void) -{ - /* Initialize servo module */ - servo_init (); - - /* Ensure servo motor are in the right state */ - trap_setup_path_to_box (out_left_box); -} - -/* Configure traps to open a path to a box. */ -void -trap_setup_path_to_box (trap_box_id_e box) -{ - switch (box) - { - case garbage: - /* 0:H, 1:H, 2:H, 3:V, 4:? */ - servo_set_high_time (0, trap_high_time_pos[horizontal][0]); - servo_set_high_time (1, trap_high_time_pos[horizontal][1]); - servo_set_high_time (2, trap_high_time_pos[horizontal][2]); - servo_set_high_time (3, trap_high_time_pos[vertical][3]); - break; - case out_left_box: - /* 0:V, 1:H, 2:H, 3:V, 4:? */ - servo_set_high_time (0, trap_high_time_pos[vertical][0]); - servo_set_high_time (1, trap_high_time_pos[horizontal][1]); - servo_set_high_time (2, trap_high_time_pos[horizontal][2]); - servo_set_high_time (3, trap_high_time_pos[vertical][3]); - break; - case middle_left_box: - /* 0:?, 1:V, 2:H, 3:V, 4:? */ - servo_set_high_time (1, trap_high_time_pos[vertical][1]); - servo_set_high_time (2, trap_high_time_pos[horizontal][2]); - servo_set_high_time (3, trap_high_time_pos[vertical][3]); - break; - case middle_box: - /* 0:?, 1:?, 2:V, 3:V, 4:? */ - servo_set_high_time (2, trap_high_time_pos[vertical][2]); - servo_set_high_time (3, trap_high_time_pos[vertical][3]); - break; - case middle_right_box: - /* 0:?, 1:?, 2:V, 3:H, 4:V */ - servo_set_high_time (2, trap_high_time_pos[vertical][2]); - servo_set_high_time (3, trap_high_time_pos[horizontal][3]); - servo_set_high_time (4, trap_high_time_pos[vertical][4]); - break; - case out_right_box: - /* 0:?, 1:?, 2:V, 3:H, 4:H */ - servo_set_high_time (2, trap_high_time_pos[vertical][2]); - servo_set_high_time (3, trap_high_time_pos[horizontal][3]); - servo_set_high_time (4, trap_high_time_pos[horizontal][4]); - break; - default: - return; - } -} - -/* Set high time value for horizontal and vertical position of a trap. */ -void -trap_set_high_time (uint8_t servo_id, uint8_t h, uint8_t v) -{ - if (servo_id < SERVO_NUMBER) - { - trap_high_time_pos[horizontal][servo_id] = h; - trap_high_time_pos[vertical][servo_id] = v; - } -} - -/* Open the rear pannel to eject the balls. */ -void -trap_open_rear_panel (void) -{ - servo_set_high_time (5, trap_high_time_pos[horizontal][5]); -} - -/* Close the rear pannel to eject the balls. */ -void -trap_close_rear_panel (void) -{ - servo_set_high_time (5, trap_high_time_pos[vertical][5]); -} diff --git a/digital/io/src/trap.h b/digital/io/src/trap.h deleted file mode 100644 index 0c199516..00000000 --- a/digital/io/src/trap.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef trap_h -#define trap_h -/* trap.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Dufour Jérémy - * - * 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. - * - * }}} */ - -/** - * @file Module to control the traps (using servo module). - * It is a higher interface to the servo module more simple to use. - * The traps and the servos motor are organized in the following ways: - * @verbatim - * +---+----+---+----+---+ - * G | L | ML | M | MR | R | - * +---+----+---+----+---+ - * 0 1 2 ^ 3 4 - * @endverbatim - * In the scheme, the target boxes for the balls are identified by letters, - * and the servo motors that control the traps are identified by a number: - * - L: left; - * - ML: middle left; - * - M: middle; - * - MR: middle right; - * - R: right. - * Balls are entering in the system between the 2 and 3 traps, where the ^ - * symbol is. - * If the ball want to go M, traps 2 and 3 must be vertical. - * To go to ML, trap 4 must be vertical, 2 must be horizontal and 1 must be - * vertical. - * To go to the garbage, trap 3 must be vertical and traps 0, 1 and 2 must be - * horizontal. - * There is also a sixth trap to eject all the balls. It is used to put the - * balls contained in the boxes into the gutter (unlike the garbage). - */ - -#include "servo.h" /* SERVO_NUMBER */ - -/** - * Trap high time values for the different positions (horizontal and - * vertical). - * We need to declare it as external to be able to use it from the EEPROM - * module. - */ -extern uint8_t trap_high_time_pos[2][SERVO_NUMBER]; - -/** - * Trap module initialization. - * It initializes the sub-module servo. - */ -void -trap_init (void); - -/** - * List of boxes where to store balls. - */ -typedef enum trap_box_id_e -{ - /** Throw away ball. */ - garbage = 0, - /** Most left box. */ - out_left_box, - /** Middle left box. */ - middle_left_box, - /** Box for the middle slot. */ - middle_box, - /** Middle right box. */ - middle_right_box, - /** Most right box. */ - out_right_box, - /** Count of trap boxes. */ - trap_count, -} trap_box_id_e; - -/** - * Configure traps to open a path to a box. - * This function will setup all the servo open a path to the desired box. - * @param box the box where you want the path to lead to. - */ -void -trap_setup_path_to_box (trap_box_id_e box); - -/** - * Set high time value for horizontal and vertical position of a trap. - * @param servo_id the servo identification number. - * @param h the horizontal high time value. - * @param v the vertical high time value. - */ -void -trap_set_high_time (uint8_t servo_id, uint8_t h, uint8_t v); - -/** - * Open the rear panel to eject the balls. - */ -void -trap_open_rear_panel (void); - -/** - * Close the rear panel to eject the balls. - */ -void -trap_close_rear_panel (void); - -#endif /* trap_h */ -- cgit v1.2.3