From 4cd9c4458eaa49c269171145f4bc2a184948eae1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 Apr 2011 21:26:15 +0200 Subject: digital/mimot: switch to differential drive --- digital/mimot/src/dirty/traj.c | 349 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 349 insertions(+) create mode 100644 digital/mimot/src/dirty/traj.c (limited to 'digital/mimot/src/dirty/traj.c') diff --git a/digital/mimot/src/dirty/traj.c b/digital/mimot/src/dirty/traj.c new file mode 100644 index 00000000..a12b6f5b --- /dev/null +++ b/digital/mimot/src/dirty/traj.c @@ -0,0 +1,349 @@ +/* traj.c - Trajectories. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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 "traj.h" + +#include "modules/math/fixed/fixed.h" +#include "modules/math/math.h" +#include "modules/utils/utils.h" +#include "io.h" + +#include + +#include "state.h" + +#include "pos.h" +#include "speed.h" +#include "postrack.h" + +#include "contacts.h" + +#ifdef HOST +# include "simu.host.h" +#endif + +/* This file provides high level trajectories commands. */ + +/** Angle after which a backward movement is considered. */ +#define TRAJ_GOTO_BACKWARD_THRESHOLD ((int32_t) (0.55 * M_PI * (1L << 24))) + +/** The famous number. */ +#define PI_F824 ((int32_t) (M_PI * (1L << 24))) + +/** Traj mode enum. */ +enum +{ + /* Go to the wall. */ + TRAJ_FTW, + /* Go to position. */ + TRAJ_GOTO, + /* Go to angle. */ + TRAJ_GOTO_ANGLE, + /* Go to position, then angle. */ + TRAJ_GOTO_XYA, + /* Everything done. */ + TRAJ_DONE, +}; + +/** Traj mode. */ +uint8_t traj_mode; + +/** Epsilon, distance considered to be small enough. */ +int16_t traj_eps = 500; + +/** Angle epsilon, angle considered to be small enough (f0.16). */ +int16_t traj_aeps = 0x0100; + +/** Angle at which to start going forward (f0.16). */ +uint16_t traj_angle_limit = 0x2000; + +/** Angle at which to start going forward (rad, f8.24). */ +int32_t traj_angle_limit_rad; + +/** Go backward. */ +static uint8_t traj_backward; + +/** Go to position. */ +static uint32_t traj_goto_x, traj_goto_y; + +/** Go to angle. */ +static uint32_t traj_goto_a; + +/** Initialise computed factors. */ +void +traj_init (void) +{ + traj_set_angle_limit (traj_angle_limit); +} + +/** Angle offset. Directly handled to speed layer. */ +void +traj_angle_offset_start (int32_t angle, uint8_t seq) +{ + int32_t a = fixed_mul_f824 (angle, 2 * PI_F824); + uint32_t f = postrack_footing; + int32_t arc = fixed_mul_f824 (f, a); + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + speed_alpha.pos_cons += arc; + state_start (&state_main, MODE_SPEED, seq); +} + +/** Go to the wall mode. */ +static void +traj_ftw (void) +{ + uint8_t left, right; + int16_t speed; + speed = speed_theta.slow; + speed *= 256; + if (!traj_backward) + { + left = !IO_GET (CONTACT_FRONT_LEFT_IO); + right = !IO_GET (CONTACT_FRONT_RIGHT_IO); + } + else + { + speed = -speed; + left = !IO_GET (CONTACT_BACK_LEFT_IO); + right = !IO_GET (CONTACT_BACK_RIGHT_IO); + } + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 0; + if (!left && !right) + { + /* Backward. */ +#ifndef HOST + /* No angular control. */ + state_main.variant = 2; +#endif + } + else if (!left || !right) + { +#ifndef HOST + /* No angular control. */ + state_main.variant = 2; +#else + /* On host, we must do the job. */ + speed_theta.cons = speed / 4; + if (left) + speed_alpha.cons = speed / 2; + else + speed_alpha.cons = -speed / 2; +#endif + } + else + { + /* Stay here. */ + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cur; + speed_alpha.pos_cons = pos_alpha.cur; + speed_theta.cur = 0; + speed_alpha.cur = 0; + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start go to the wall mode. */ +void +traj_ftw_start (uint8_t backward, uint8_t seq) +{ + traj_mode = TRAJ_FTW; + traj_backward = backward; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to position mode. */ +static void +traj_goto (void) +{ + int32_t dx = traj_goto_x - postrack_x; + int32_t dy = traj_goto_y - postrack_y; + if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8 + && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8) + { + /* Near enough, stop, let speed terminate the movement. */ + state_main.mode = MODE_SPEED; + traj_mode = TRAJ_DONE; + } + else + { + /* Projection of destination in robot base. */ + int32_t c = fixed_cos_f824 (postrack_a); + int32_t s = fixed_sin_f824 (postrack_a); + int32_t dt = fixed_mul_f824 (dx, c) + fixed_mul_f824 (dy, s); + int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s); + /* Compute arc length. */ + int32_t arad = atan2 (da, dt) * (1L << 24); + if (traj_backward & TRAJ_BACKWARD) + { + if (arad > 0) + arad = - PI_F824 + arad; + else + arad = PI_F824 + arad; + } + if (traj_backward & TRAJ_REVERT_OK) + { + if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = - PI_F824 + arad; + else if (arad < - TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = PI_F824 + arad; + } + int32_t arc = fixed_mul_f824 (arad, postrack_footing); + /* Compute consign. */ + speed_alpha.pos_cons = pos_alpha.cur; + speed_alpha.pos_cons += arc; + if (UTILS_ABS (arad) < traj_angle_limit_rad) + { + speed_theta.pos_cons = pos_theta.cur; + speed_theta.pos_cons += dt >> 8; + } + else + { + speed_theta.pos_cons = pos_theta.cons; + } + } +} + +/** Start go to position mode (x, y: f24.8). */ +void +traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq) +{ + traj_mode = TRAJ_GOTO; + traj_goto_x = x; + traj_goto_y = y; + traj_backward = backward; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to angle mode. */ +static void +traj_goto_angle (void) +{ + /* There is some tricky parts to handle rotation direction. */ + int16_t da = (uint16_t) (traj_goto_a >> 8) - (uint16_t) (postrack_a >> 8); + if (UTILS_ABS (da) < traj_aeps) + { + /* Near enough, stop, let speed terminate the movement. */ + state_main.mode = MODE_SPEED; + traj_mode = TRAJ_DONE; + } + else + { + /* Compute arc length. */ + int32_t arad = fixed_mul_f824 (((int32_t) da) << 8, + 2 * PI_F824); + int32_t arc = fixed_mul_f824 (arad, postrack_footing); + /* Compute consign. */ + speed_alpha.pos_cons = pos_alpha.cur; + speed_alpha.pos_cons += arc; + } +} + +/** Start go to angle mode (a: f8.24). */ +void +traj_goto_angle_start (uint32_t a, uint8_t seq) +{ + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_a = a; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Go to position, then angle mode. */ +static void +traj_goto_xya (void) +{ + int32_t dx = traj_goto_x - postrack_x; + int32_t dy = traj_goto_y - postrack_y; + if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8 + && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8) + { + /* Near enough, now do a go to angle. */ + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_angle (); + } + else + { + traj_goto (); + } +} + +/** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */ +void +traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward, + uint8_t seq) +{ + traj_mode = TRAJ_GOTO_XYA; + traj_goto_x = x; + traj_goto_y = y; + traj_goto_a = a; + traj_backward = backward; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + +/* Compute new speed according the defined trajectory. */ +void +traj_update (void) +{ + switch (traj_mode) + { + case TRAJ_FTW: + traj_ftw (); + break; + case TRAJ_GOTO: + traj_goto (); + break; + case TRAJ_GOTO_ANGLE: + traj_goto_angle (); + break; + case TRAJ_GOTO_XYA: + traj_goto_xya (); + break; + case TRAJ_DONE: + break; + } +} + +/* Set angle limit. */ +void +traj_set_angle_limit (uint16_t a) +{ + traj_angle_limit = a; + traj_angle_limit_rad = (uint32_t) a * (uint32_t) ((1 << 8) * M_PI * 2); +} + -- cgit v1.2.3