summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/Makefile4
-rw-r--r--digital/io/src/eeprom.avr.c21
-rw-r--r--digital/io/src/main.c52
-rw-r--r--digital/io/src/servo.avr.c5
-rw-r--r--digital/io/src/servo.h4
-rw-r--r--digital/io/src/servo_pos.c60
-rw-r--r--digital/io/src/servo_pos.h73
-rw-r--r--digital/io/src/trap.c147
-rw-r--r--digital/io/src/trap.h123
9 files changed, 161 insertions, 328 deletions
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 <avr/eeprom.h> /* 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 */