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/main.c | 4 +++ n/asserv/src/asserv/pos.c | 1 + n/asserv/src/asserv/speed.c | 85 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 90 insertions(+) create mode 100644 n/asserv/src/asserv/speed.c (limited to 'n/asserv/src/asserv') diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index 2f9c77b..3bb0a5d 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -34,6 +34,7 @@ #include "counter.c" #include "pwm.c" #include "pos.c" +#include "speed.c" #include "eeprom.c" /** Motor control mode: @@ -101,6 +102,9 @@ main_loop (void) pos_update (); /* Pwm setup. */ pwm_update (); + /* Speed control. */ + if (main_mode >= 2) + speed_update (); /* Stats. */ if (main_stat_counter && !--main_stat_counter_cpt) { diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c index 0a1af23..d1c7e8f 100644 --- a/n/asserv/src/asserv/pos.c +++ b/n/asserv/src/asserv/pos.c @@ -46,6 +46,7 @@ uint16_t pos_theta_kp = 0, pos_alpha_kp = 0; uint16_t pos_theta_ki = 0, pos_alpha_ki = 0; /** D coefficients. */ uint16_t pos_theta_kd = 0, pos_alpha_kd = 0; + /** Current integral values. */ int16_t pos_theta_int, pos_alpha_int; /** Last error values. */ 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