summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/Makefile9
-rw-r--r--digital/io/src/avrconfig.h17
-rw-r--r--digital/io/src/eeprom.avr.c4
-rw-r--r--digital/io/src/main.c6
-rw-r--r--digital/io/src/move.c6
-rw-r--r--digital/io/src/radar.c189
-rw-r--r--digital/io/src/radar.h78
-rw-r--r--digital/io/src/radar_defs.c54
-rw-r--r--digital/io/src/radar_defs.h (renamed from digital/io/src/usdist.h)46
-rw-r--r--digital/io/src/servo.avr.c200
-rw-r--r--digital/io/src/servo.h96
-rw-r--r--digital/io/src/servo_pos.c60
-rw-r--r--digital/io/src/servo_pos.h73
-rw-r--r--digital/io/src/simu.host.c26
-rw-r--r--digital/io/src/usdist.c125
15 files changed, 111 insertions, 878 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 5cbea0ba..4ebc5a66 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -3,16 +3,17 @@ BASE = ../../avr
# Name of the program to build.
PROGS = io
# Sources to compile.
-io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \
- switch.avr.c chrono.c timer.avr.c servo_pos.c \
+io_SOURCES = main.c fsm_queue.c eeprom.avr.c pwm.c \
+ switch.avr.c chrono.c timer.avr.c \
twi_master.c asserv.c mimot.c \
- simu.host.c contact.c usdist.c radar.c \
+ simu.host.c contact.c radar.c radar_defs.c \
path.c food.c events.host.c \
fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c
# Modules needed for IO.
MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \
+ devices/usdist devices/servo \
trace flash spi
-AI_MODULES = twi_master common utils fsm
+AI_MODULES = twi_master common utils fsm move
# Configuration file.
CONFIGFILE = avrconfig.h
# IO board use an ATMega128.
diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h
index dd3f2e87..cb9061fd 100644
--- a/digital/io/src/avrconfig.h
+++ b/digital/io/src/avrconfig.h
@@ -117,6 +117,23 @@
/** Heuristic callback. */
#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback
+/* usdist - Analog US distance sensor. */
+/** Number of sensors. */
+#define AC_USDIST_NB 4
+/** Measuring period, in number of update call. */
+#define AC_USDIST_PERIOD 1
+/** List of space separated sensor definition, see usdist.h. */
+#define AC_USDIST_SENSORS \
+ USDIST_SENSOR (0, G, 3) \
+ USDIST_SENSOR (1, G, 1) \
+ USDIST_SENSOR (2, C, 7) \
+ USDIST_SENSOR (3, D, 4)
+
+/* servo - Servo module */
+/** All servos are currently connected to the same port. */
+#define AC_SERVO_PORT PORTA
+#define AC_SERVO_DDR DDRA
+
/* io - io/ai board. */
/** TWI address of the io board. */
#define AC_IO_TWI_ADDRESS 2
diff --git a/digital/io/src/eeprom.avr.c b/digital/io/src/eeprom.avr.c
index a32b9606..febba55c 100644
--- a/digital/io/src/eeprom.avr.c
+++ b/digital/io/src/eeprom.avr.c
@@ -26,8 +26,8 @@
#include "common.h"
#include "eeprom.h"
-#include "servo.h" /* SERVO_NUMBER */
-#include "servo_pos.h"
+#include "modules/devices/servo/servo.h"
+#include "modules/devices/servo/servo_pos.h"
#include <avr/eeprom.h> /* eeprom_{read,write}_byte */
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 22964ddd..5aa35fb7 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -28,6 +28,8 @@
#include "modules/proto/proto.h"
#include "modules/utils/utils.h"
#include "modules/path/path.h"
+#include "modules/devices/usdist/usdist.h"
+#include "modules/devices/servo/servo_pos.h"
#include "modules/flash/flash.h"
#include "modules/trace/trace.h"
#include "events.h"
@@ -50,8 +52,6 @@
#include "fsm.h"
#include "fsm_queue.h"
#include "bot.h"
-#include "servo_pos.h"
-#include "usdist.h"
#include "radar.h"
#include "chrono.h" /* chrono_end_match */
#include "pwm.h"
@@ -408,7 +408,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - 1b: servo id number;
* - 1b: pwm high time value (position).
*/
- servo_set_high_time (args[0], args[1]);
+ servo_set_position (args[0], args[1]);
break;
case c ('S', 0):
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 60db0272..81c554fc 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -397,7 +397,7 @@ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ()
position_t robot_pos;
asserv_get_position (&robot_pos);
vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == 1
+ int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD
? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
: -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
@@ -405,8 +405,8 @@ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY ()
path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
/* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction () == 1 ?
- - 300 : 300);
+ asserv_move_linearly (asserv_get_last_moving_direction ()
+ == DIRECTION_FORWARD ? -300 : 300);
}
FSM_TRANS (MOVE_MOVING,
diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c
deleted file mode 100644
index 1ac982c7..00000000
--- a/digital/io/src/radar.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/* radar.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 "radar.h"
-
-#include "playground_2010.h"
-#include "bot.h"
-#include "usdist.h"
-
-#include "modules/math/geometry/geometry.h"
-#include "modules/math/geometry/distance.h"
-#include "modules/utils/utils.h"
-
-/** Margin to be considered inside the playground. An obstacle can not be
- * exactly at the playground edge. */
-#define RADAR_MARGIN_MM 150
-
-/** Maximum distance for a sensor reading to be ignored if another sensor is
- * nearer. */
-#define RADAR_FAR_MM 250
-
-/** Describe a radar sensor. */
-struct radar_sensor_t
-{
- /** Distance updated by another module. */
- uint16_t *dist_mm;
- /** Position relative to the robot center. */
- vect_t pos;
- /** Angle relative to the robot X axis. */
- uint16_t a;
-};
-
-/** Define radar configuration. */
-struct radar_sensor_t radar_sensors[] = {
-#define RADAR_SENSOR_FRONT 0
- { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) },
-#define RADAR_SENSOR_LEFT 1
- { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) },
-#define RADAR_SENSOR_RIGHT 2
- { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) },
-#define RADAR_SENSOR_BACK 3
- { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) },
-};
-
-/** Define exclusion area (considered as invalid point). */
-static uint8_t
-radar_valid (vect_t p)
-{
- return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM
- && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM
- /* Ignore points on slope, no margin for the slope start. */
- && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2
- || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2
- || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2);
-}
-
-uint8_t
-radar_update (const position_t *robot_pos, vect_t *obs_pos)
-{
- uint8_t i, j;
- vect_t ray;
- uint8_t obs_nb = 0;
- uint8_t front_nb;
- vect_t front_center;
- /* Compute hit points for each sensor and eliminate invalid ones. */
- vect_t hit[UTILS_COUNT (radar_sensors)];
- uint8_t valid[UTILS_COUNT (radar_sensors)];
- uint16_t dist_mm[UTILS_COUNT (radar_sensors)];
- for (i = 0; i < UTILS_COUNT (radar_sensors); i++)
- {
- dist_mm[i] = *radar_sensors[i].dist_mm;
- if (dist_mm[i] != 0xffff)
- {
- hit[i] = radar_sensors[i].pos;
- vect_rotate_uf016 (&hit[i], robot_pos->a);
- vect_translate (&hit[i], &robot_pos->v);
- vect_from_polar_uf016 (&ray, dist_mm[i],
- robot_pos->a + radar_sensors[i].a);
- vect_translate (&hit[i], &ray);
- valid[i] = radar_valid (hit[i]);
- vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM,
- robot_pos->a + radar_sensors[i].a);
- vect_translate (&hit[i], &ray);
- }
- else
- valid[i] = 0;
- }
- /* Ignore sensor results too far from other sensors. */
- for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++)
- {
- for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++)
- {
- if (valid[j])
- {
- if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j])
- valid[j] = 0;
- else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i])
- valid[i] = 0;
- }
- }
- }
- /* Specific treatment about sensor topology. */
- if (valid[RADAR_SENSOR_BACK])
- obs_pos[obs_nb++] = hit[RADAR_SENSOR_BACK];
- front_nb = 0;
- front_center.x = 0; front_center.y = 0;
- for (i = RADAR_SENSOR_FRONT; i < RADAR_SENSOR_BACK; i++)
- {
- if (valid[i])
- {
- vect_add (&front_center, &hit[i]);
- front_nb++;
- }
- }
- if (front_nb)
- {
- vect_scale_f824 (&front_center, 0x1000000l / front_nb);
- obs_pos[obs_nb++] = front_center;
- }
- /* Done. */
- return obs_nb;
-}
-
-uint8_t
-radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
- const vect_t *obs_pos, uint8_t obs_nb)
-{
- uint8_t i;
- /* Stop here if no obstacle. */
- if (!obs_nb)
- return 0;
- vect_t vd = *dest_pos; vect_sub (&vd, robot_pos);
- uint16_t d = vect_norm (&vd);
- /* If destination is realy near, stop here. */
- if (d < RADAR_EPSILON_MM)
- return 0;
- /* If destination is near, use clearance to destination point instead of
- * stop length. */
- vect_t t;
- if (d < RADAR_STOP_MM)
- t = *dest_pos;
- else
- {
- vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM);
- t = *robot_pos;
- vect_translate (&t, &vd);
- }
- /* Now, look at obstacles. */
- for (i = 0; i < obs_nb; i++)
- {
- /* Vector from robot to obstacle. */
- vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos);
- /* Ignore if in our back. */
- int32_t dp = vect_dot_product (&vd, &vo);
- if (dp < 0)
- continue;
- /* Check distance. */
- int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]);
- if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2
- + RADAR_OBSTACLE_RADIUS_MM)
- continue;
- /* Else, obstacle is blocking. */
- return 1;
- }
- return 0;
-}
-
diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h
deleted file mode 100644
index b4611ec7..00000000
--- a/digital/io/src/radar.h
+++ /dev/null
@@ -1,78 +0,0 @@
-#ifndef radar_h
-#define radar_h
-/* radar.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 "defs.h"
-
-/**
- * Handle any distance sensors information to extract useful data. This
- * includes:
- * - combining several sensors information for a more precise obstacle
- * position,
- * - ignoring obstacles not in the playground,
- * - determining if an obstacle should make the robot stop.
- */
-
-/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is
- * added to position obstacle center. */
-#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40
-
-/** Estimated obstacle radius. The obstacle may be larger than at the
- * detected edge. */
-#define RADAR_OBSTACLE_RADIUS_MM 150
-
-/** Stop distance. Distance under which an obstacle is considered harmful when
- * moving. */
-#define RADAR_STOP_MM 350
-
-/** Clearance distance. Distance over which an obstacle should be to the side
- * when moving.
- *
- * OK, more explanations: when moving, a rectangle is placed in front of the
- * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM +
- * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered
- * in the way.
- *
- * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM),
- * this reduce the rectangle length.
- *
- * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */
-#define RADAR_CLEARANCE_MM 100
-
-/** Destination distance near enough so that obstacles could be ignored. */
-#define RADAR_EPSILON_MM 70
-
-/** Update radar view. Return the number of obstacles found. Obstacles
- * positions are returned in obs_pos. */
-uint8_t
-radar_update (const position_t *robot_pos, vect_t *obs_pos);
-
-/** Return non zero if there is a blocking obstacle near the robot while going
- * to a destination point. */
-uint8_t
-radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
- const vect_t *obs_pos, uint8_t obs_nb);
-
-#endif /* radar_h */
diff --git a/digital/io/src/radar_defs.c b/digital/io/src/radar_defs.c
new file mode 100644
index 00000000..2787ed97
--- /dev/null
+++ b/digital/io/src/radar_defs.c
@@ -0,0 +1,54 @@
+/* radar_defs.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 "radar.h"
+
+#include "modules/devices/usdist/usdist.h"
+#include "playground_2010.h"
+
+/** Define radar configuration. */
+struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = {
+#define RADAR_SENSOR_FRONT 0
+ { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) },
+#define RADAR_SENSOR_LEFT 1
+ { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) },
+#define RADAR_SENSOR_RIGHT 2
+ { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) },
+#define RADAR_SENSOR_BACK 3
+ { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) },
+};
+
+/** Define exclusion area (considered as invalid point). */
+uint8_t
+radar_valid (vect_t p)
+{
+ return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM
+ && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM
+ /* Ignore points on slope, no margin for the slope start. */
+ && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2
+ || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2
+ || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2);
+}
+
diff --git a/digital/io/src/usdist.h b/digital/io/src/radar_defs.h
index 9a4a6646..51bc5943 100644
--- a/digital/io/src/usdist.h
+++ b/digital/io/src/radar_defs.h
@@ -1,6 +1,6 @@
-#ifndef usdist_h
-#define usdist_h
-/* usdist.h */
+#ifndef radar_defs_h
+#define radar_defs_h
+/* radar_defs.h */
/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
*
* Copyright (C) 2010 Nicolas Schodet
@@ -25,35 +25,17 @@
*
* }}} */
-/*
- * Manage ultrasonic distance sensors using analog to digital converter.
- */
+#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40
+#define RADAR_OBSTACLE_RADIUS_MM 150
+#define RADAR_STOP_MM 350
+#define RADAR_CLEARANCE_MM 100
+#define RADAR_EPSILON_MM 70
-/** Number of sensors. */
-#define USDIST_NB 4
+#define RADAR_SENSOR_NB 4
-/** Minimal calibrated distance. */
-#define USDIST_MM_MIN 100
+#define RADAR_SENSOR_FRONT_FIRST 0
+#define RADAR_SENSOR_FRONT_NB 3
+#define RADAR_SENSOR_BACK_FIRST 3
+#define RADAR_SENSOR_BACK_NB 1
-/** Maximal calibrated distance. */
-#define USDIST_MM_MAX 700
-
-/** Distance considered as too far to be true. */
-#define USDIST_MM_TOO_FAR 650
-
-/** Measuring period in cycles. */
-#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / TIMER_PERIOD_MS)
-
-/** Array containing the last measures in millimeters. */
-extern uint16_t usdist_mm[USDIST_NB];
-
-/** Initialise module. */
-void
-usdist_init (void);
-
-/** To be called every cycle to update sensor measures.
- * - returns: non zero if sensor value has been updated. */
-uint8_t
-usdist_update (void);
-
-#endif /* usdist_h */
+#endif /* radar_defs_h */
diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c
deleted file mode 100644
index de50c819..00000000
--- a/digital/io/src/servo.avr.c
+++ /dev/null
@@ -1,200 +0,0 @@
-/* servo.avr.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 "servo.h"
-
-#include "modules/utils/utils.h" /* regv, set_bit */
-#include "modules/utils/byte.h" /* v16_to_v8 */
-#include "io.h" /* General defines of registers */
-
-/**
- * @defgroup ServoConfig Servo module configuration variables and defines.
- * @{
- */
-
-/**
- * All servos are connected to the PORTA.
- */
-#define SERVO_PORT PORTA
-#define SERVO_DDR DDRA
-
-/**
- * TOP of the timer/counter.
- */
-#define SERVO_TCNT_TOP 0xFF
-
-/**
- * Number of TIC the timer/counter need to do to make a whole cycle of servo
- * update.
- * It does not depend on the servo motors we manage but on the time we want a
- * whole cycle to last.
- * The formula used is:
- * time_of_a_cycle * AVR_frequency / timer_counter_prescaler
- * We want a time of 20ms (20/1000).
- * See @a servo_init to know the prescaler value of the timer/counter.
- */
-static const uint16_t servo_tic_cycle_ = AC_FREQ / 256 * 20 / 1000;
-
-/** @} */
-
-/**
- * @defgroup ServoPrivate Servo module private variables and functions
- * declarations
- * @{
- */
-
-/**
- * Identifier of the servo we are currently updating.
- * Note: -1 is a special value used by the servo module system to update the
- * low state of all the servos.
- */
-volatile int8_t servo_updating_id_;
-
-/**
- * A table for the time spent by each servo in high state.
- */
-volatile uint8_t servo_high_time_[SERVO_NUMBER];
-
-/**
- * Overflow of timer/counter 2 handler.
- */
-SIGNAL (SIG_OVERFLOW2);
-
-/** @} */
-
-/* Initialize servo module. */
-void
-servo_init (void)
-{
- /* Set-up all the pins of the servo to out direction */
- SERVO_DDR = 0xff;
- /* All pins are at low state by default */
-
- /* Set-up the timer/counter 2:
- - prescaler 256 => 4.44 ms TOP */
- TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
- 0, 0, 0, 0, 0, 1, 0, 0);
-
- /* The state machine start with the first servo */
- servo_updating_id_ = 0;
-
- /* 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). */
-void
-servo_set_high_time (uint8_t servo, uint8_t high_time)
-{
- uint8_t filtered = high_time;
- if (filtered != 0)
- UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX);
- /* Sanity check */
- if (servo < SERVO_NUMBER)
- /* Set new desired position (high value time) */
- servo_high_time_[servo] = filtered;
-}
-
-/* Get the high time of the servo. */
-uint8_t
-servo_get_high_time (uint8_t servo)
-{
- /* Sanity check */
- if (servo < SERVO_NUMBER)
- return servo_high_time_[servo];
- return 0;
-}
-
-/* Overflow of timer/counter 2 handler. */
-SIGNAL (SIG_OVERFLOW2)
-{
- /* Overflow count (used when we wait in the lower state).
- -1 is used for the first count where we wait less than a complete
- overflow */
- static int8_t servo_overflow_count = -1;
- /* Time spent by each servo motor at high state during a whole cycle */
- static uint16_t servo_high_time_cycle = servo_tic_cycle_;
-
- /* State machine actions */
- if (servo_updating_id_ >= 0)
- {
- /* Servos motor high state mode */
-
- /* Set to low state the previous servo motor pin if needed (not for
- * the first one) */
- if (servo_updating_id_ != 0)
- SERVO_PORT &= ~_BV (servo_updating_id_ - 1);
- /* Set to high state the current servo motor pin, unless is zero */
- if (servo_high_time_[servo_updating_id_])
- set_bit (SERVO_PORT, servo_updating_id_);
- /* Plan next timer overflow to the TOP minus the current configuration
- * of the servo motor */
- TCNT2 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_];
- /* Update the time spent at high state by all servo motors for this
- * cycle */
- servo_high_time_cycle += servo_high_time_[servo_updating_id_];
- /* Update the identifier of the current servo motor (and manage when
- * we are at the last one) */
- if (++servo_updating_id_ == SERVO_NUMBER)
- servo_updating_id_ = -1;
- }
- else
- {
- /* Sleeping time mode */
-
- /* Is it the first we are in this mode? */
- if (servo_overflow_count == -1)
- {
- /* Set to low state the previous servo motor pin */
- SERVO_PORT &= ~_BV (SERVO_NUMBER - 1);
- /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to
- * wait (division by SERVO_TCNT_TOP or >> 8) */
- servo_overflow_count = servo_high_time_cycle >> 8;
- /* Restart the counter from remaining TIC that are left and can
- * not be used to make a full overflow */
- TCNT2 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0);
- }
- else
- {
- /* We just have an overflow, are we at the last one needed? The -1
- * is normal: we do not count the first overflow of the sleeping
- * mode because it is not a full one */
- if (--servo_overflow_count == -1)
- {
- /* Restart with first servo motor */
- servo_updating_id_ = 0;
- /* Re-initialize the counter of time spent by each servo motor
- * at high state */
- servo_high_time_cycle = servo_tic_cycle_;
- }
- }
- }
-}
diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h
deleted file mode 100644
index b9f7e3ea..00000000
--- a/digital/io/src/servo.h
+++ /dev/null
@@ -1,96 +0,0 @@
-#ifndef servo_h
-#define servo_h
-/* servo.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.
- *
- * }}} */
-
-#include "common.h" // uint8_t
-
-/**
- * @file Module to control servo motors.
- * This module contains low-level functions to control the servo motors. If
- * you want to control the traps to store the balls at a specific place, have
- * a look at the trap module.
- * It uses the timer/counter 2 (8-bit).
- *
- * Servo motors can be controlled by the time the input signal spend at its
- * high state. For example, if the signal sent to the servo motor only spend
- * 1ms at the high state, it will have a 0° angle position. If the signal
- * stays for 1.5ms at high state, it will have a 90° angle position.
- * To manage all servo motors in a "one time shot", we need to use the
- * timer/counter 2 of the ATmega128 and its overflow.
- * We setup the timer/counter to the value of its overflow minus the time the
- * input signal of the servo need to spend at high state ; we put the input
- * signal of this servo motor to the high value. When the timer overflows, we
- * put it back to the low state. We go to the next servo motors and do the
- * 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 8
-
-/**
- * Minimum high time for servos.
- */
-#define SERVO_HIGH_TIME_MIN 0x24
-
-/**
- * Maximum high time for servos.
- */
-#define SERVO_HIGH_TIME_MAX 0x88
-
-/**
- * Initialize servo module.
- * This functions put the pins of the servos motor in the right direction,
- * initialize the timer/counter 2 and some internals stuff.
- */
-void
-servo_init (void);
-
-/**
- * Set the high time of the input signal of a servo (and its position).
- * @param servo the servo to change the position.
- * @param high_time the high time we want the input signal to spend at the
- * high state to set the servo motor to a position. A zero will let the servo
- * floating.
- */
-void
-servo_set_high_time (uint8_t servo, uint8_t high_time);
-
-/**
- * Get the high time of the servo.
- * @param servo the servo to get the position of.
- * @return the current position of the servo.
- */
-uint8_t
-servo_get_high_time (uint8_t servo);
-
-#endif /* servo_h */
diff --git a/digital/io/src/servo_pos.c b/digital/io/src/servo_pos.c
deleted file mode 100644
index d04077fd..00000000
--- a/digital/io/src/servo_pos.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/* 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
deleted file mode 100644
index c8e56d2b..00000000
--- a/digital/io/src/servo_pos.h
+++ /dev/null
@@ -1,73 +0,0 @@
-#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/simu.host.c b/digital/io/src/simu.host.c
index f1b7e0d2..a35b9d12 100644
--- a/digital/io/src/simu.host.c
+++ b/digital/io/src/simu.host.c
@@ -25,7 +25,7 @@
#include "common.h"
#include "simu.host.h"
-#include "servo.h"
+#include "modules/devices/servo/servo.h"
#include "pwm.h"
#include "modules/utils/utils.h"
@@ -36,10 +36,10 @@
#include "io.h"
/** Requested servo position. */
-uint8_t servo_high_time_[SERVO_NUMBER];
+uint8_t servo_position_[SERVO_NUMBER];
/** Current servo position. */
-uint8_t servo_high_time_current_[SERVO_NUMBER];
+uint8_t servo_position_current_[SERVO_NUMBER];
/** Servo speed is about 120 ms for 60 degrees. This means about 360 ms for
* the full swing. */
@@ -112,13 +112,13 @@ simu_step (void)
/* Update servos. */
for (i = 0; i < SERVO_NUMBER; i++)
{
- if (UTILS_ABS (servo_high_time_current_[i] - servo_high_time_[i]) <
+ if (UTILS_ABS (servo_position_current_[i] - servo_position_[i]) <
SERVO_SPEED)
- servo_high_time_current_[i] = servo_high_time_[i];
- else if (servo_high_time_current_[i] < servo_high_time_[i])
- servo_high_time_current_[i] += SERVO_SPEED;
+ servo_position_current_[i] = servo_position_[i];
+ else if (servo_position_current_[i] < servo_position_[i])
+ servo_position_current_[i] += SERVO_SPEED;
else
- servo_high_time_current_[i] -= SERVO_SPEED;
+ servo_position_current_[i] -= SERVO_SPEED;
}
/* Send servos. */
if (simu_servo_update && !--simu_servo_update_cpt)
@@ -126,7 +126,7 @@ simu_step (void)
simu_servo_update_cpt = simu_servo_update;
m = mex_msg_new (simu_mex_servo);
for (i = 0; i < SERVO_NUMBER; i++)
- mex_msg_push (m, "B", servo_high_time_current_[i]);
+ mex_msg_push (m, "B", servo_position_current_[i]);
mex_node_send (m);
}
/* Update switches. */
@@ -180,15 +180,15 @@ servo_init (void)
}
void
-servo_set_high_time (uint8_t servo, uint8_t high_time)
+servo_set_position (uint8_t servo, uint8_t position)
{
- servo_high_time_[servo] = high_time;
+ servo_position_[servo] = position;
}
uint8_t
-servo_get_high_time (uint8_t servo)
+servo_get_position (uint8_t servo)
{
- return servo_high_time_[servo];
+ return servo_position_[servo];
}
void
diff --git a/digital/io/src/usdist.c b/digital/io/src/usdist.c
deleted file mode 100644
index d6fd9730..00000000
--- a/digital/io/src/usdist.c
+++ /dev/null
@@ -1,125 +0,0 @@
-/* usdist.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 "usdist.h"
-#include "timer.h"
-
-#include "modules/adc/adc.h"
-#include "modules/utils/utils.h"
-#include "io.h"
-
-#include "simu.host.h"
-
-uint16_t usdist_mm[USDIST_NB];
-
-/** Describe a sensor. */
-struct usdist_sensor_t
-{
- /** ADC number. */
- uint8_t adc;
- /** Sync PORT. */
- volatile uint8_t *sync_port;
- /** Sync DDR. */
- volatile uint8_t *sync_ddr;
- /** Sync bit mask. */
- uint8_t sync_bv;
-};
-
-/** Define sensors configuration. */
-#define USDIST_SENSOR(adc, p, n) \
- { adc, &IO_PORT_ (p, n), &IO_DDR_ (p, n), IO_BV_ (p, n) }
-struct usdist_sensor_t usdist_sensors[USDIST_NB] = {
- USDIST_SENSOR (0, G, 3),
- USDIST_SENSOR (1, G, 1),
- USDIST_SENSOR (2, C, 7),
- USDIST_SENSOR (3, D, 4),
-};
-
-void
-usdist_init (void)
-{
- uint8_t i;
- adc_init ();
- for (i = 0; i < USDIST_NB; i++)
- {
- usdist_mm[i] = 0xffff;
- *usdist_sensors[i].sync_port &= ~usdist_sensors[i].sync_bv;
- *usdist_sensors[i].sync_ddr |= usdist_sensors[i].sync_bv;
- }
-}
-
-uint8_t
-usdist_update (void)
-{
- /* Current measuring sensor. */
- static uint8_t current;
- /* Number of cycles until the measure ends. */
- static uint8_t wait;
- /* Set to one once first cycle is done. */
- static uint8_t init;
- /* Time to measure? */
- if (wait == 0)
- {
- if (init)
- {
- /* Stop sensor. */
- *usdist_sensors[current].sync_port &=
- ~usdist_sensors[current].sync_bv;
- /* Read ADC value. */
- adc_start (usdist_sensors[current].adc);
- while (!adc_checkf ())
- ;
- uint16_t v = adc_read ();
- /* Our sensors return a value between 1 and 5 V proportional to
- * the distance between calibrated values. Ignore faulty sensors. */
- if (v <= 1024 / 5 / 4)
- usdist_mm[current] = 0xffff;
- else if (v <= 1024 / 5)
- usdist_mm[current] = USDIST_MM_MIN;
- else
- usdist_mm[current] = USDIST_MM_MIN
- + ((uint32_t) (v - 1024 / 5)
- * (USDIST_MM_MAX - USDIST_MM_MIN)
- / (4 * 1024 / 5));
- if (usdist_mm[current] >= USDIST_MM_TOO_FAR)
- usdist_mm[current] = 0xffff;
- /* Next. */
- current = (current + 1) % USDIST_NB;
- }
- init = 1;
- /* Prepare next measure. */
- *usdist_sensors[current].sync_port |=
- usdist_sensors[current].sync_bv;
- wait = USDIST_PERIOD_CYCLE;
- /* New mesure done. */
- return 1;
- }
- else
- {
- wait--;
- return 0;
- }
-}
-