From 4e9742f7aa92f690429e4b6be4f53268f43374e4 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Nov 2005 21:30:42 +0000 Subject: Ajout de speed. --- n/asserv/src/asserv/speed.c | 85 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 n/asserv/src/asserv/speed.c (limited to 'n/asserv/src/asserv/speed.c') diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c new file mode 100644 index 0000000..87223e2 --- /dev/null +++ b/n/asserv/src/asserv/speed.c @@ -0,0 +1,85 @@ +/* speed.c - Speed control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * 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. + * + * }}} */ + +/** + * This file is responsible for speed control. It changes the current shafts + * positions using ramps. It can be controlled by a wanted speed or wanted + * shaft position. + */ + +/** Current speed, uf8.8. */ +int16_t speed_theta_cur, speed_alpha_cur; +/** Consign speed, uf8.8. */ +int16_t speed_theta_cons, speed_alpha_cons; +/** Consign position. */ +uint16_t speed_theta_pos_cons, speed_alpha_pos_cons; +/** Weither to use the consign position (1) or not (0). */ +uint8_t speed_pos; + +/** Acceleration, uf8.8. */ +int16_t speed_theta_acc, speed_alpha_acc; + +/* +AutoDec */ +/* -AutoDec */ + +/** Update shaft position consign according to a speed consign. */ +static void +speed_update_by_speed (void) +{ + /* Update current speed. */ + if (UTILS_ABS (speed_theta_cons - speed_theta_cur) < speed_theta_acc) + speed_theta_cur = speed_theta_cons; + else if (speed_theta_cons > speed_theta_cur) + speed_theta_cur -= speed_theta_acc; + else + speed_theta_cur += speed_theta_acc; + if (UTILS_ABS (speed_alpha_cons - speed_alpha_cur) < speed_alpha_acc) + speed_alpha_cur = speed_alpha_cons; + else if (speed_alpha_cons > speed_alpha_cur) + speed_alpha_cur -= speed_alpha_acc; + else + speed_alpha_cur += speed_alpha_acc; + /* Update shaft position. */ + pos_theta_cur += speed_theta_cur; + pos_alpha_cur += speed_alpha_cur; +} + +/** Update shaft position consign according to a position consign. */ +static void +speed_update_by_position (void) +{ + // TODO +} + +/** Update shaft position consign according to consign. */ +static void +speed_update (void) +{ + if (speed_pos) + speed_update_by_position (); + else + speed_update_by_speed (); +} + -- cgit v1.2.3