From a60c072e17cd502a06d9cea5de928b899d5cac16 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 9 Mar 2008 17:40:28 +0100 Subject: * digital/asserv/src/asserv: - cleaned up, separated compilation. - prepared auxialliary motor control. --- digital/asserv/src/asserv/Makefile | 3 +- digital/asserv/src/asserv/counter_ext.avr.c | 2 + digital/asserv/src/asserv/counter_tcc.avr.c | 1 + digital/asserv/src/asserv/eeprom.avr.c | 21 +++--- digital/asserv/src/asserv/eeprom.h | 40 ++++++++++ digital/asserv/src/asserv/main.c | 80 ++++++++------------ digital/asserv/src/asserv/pos.c | 28 ++++--- digital/asserv/src/asserv/pos.h | 47 ++++++++++++ digital/asserv/src/asserv/postrack.c | 33 +++------ digital/asserv/src/asserv/postrack.h | 41 +++++++++++ digital/asserv/src/asserv/pwm.avr.c | 32 ++++---- digital/asserv/src/asserv/pwm.h | 43 +++++++++++ digital/asserv/src/asserv/simu.host.c | 30 +++++++- digital/asserv/src/asserv/simu.host.h | 51 ------------- digital/asserv/src/asserv/speed.c | 17 +++-- digital/asserv/src/asserv/speed.h | 40 ++++++++++ digital/asserv/src/asserv/state.h | 110 ++++++++++++++++++++++++++++ digital/asserv/src/asserv/timer.avr.c | 29 ++------ digital/asserv/src/asserv/timer.h | 41 +++++++++++ digital/asserv/src/asserv/traj.c | 19 ++++- digital/asserv/src/asserv/traj.h | 33 +++++++++ digital/asserv/src/asserv/twi_proto.c | 1 + 22 files changed, 547 insertions(+), 195 deletions(-) create mode 100644 digital/asserv/src/asserv/eeprom.h create mode 100644 digital/asserv/src/asserv/pos.h create mode 100644 digital/asserv/src/asserv/postrack.h create mode 100644 digital/asserv/src/asserv/pwm.h create mode 100644 digital/asserv/src/asserv/speed.h create mode 100644 digital/asserv/src/asserv/state.h create mode 100644 digital/asserv/src/asserv/timer.h create mode 100644 digital/asserv/src/asserv/traj.h (limited to 'digital') diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile index 3ddd904c..9e3c10ac 100644 --- a/digital/asserv/src/asserv/Makefile +++ b/digital/asserv/src/asserv/Makefile @@ -1,7 +1,8 @@ BASE = ../../../avr PROGS = asserv HOST_PROGS = test_motor_model -asserv_SOURCES = main.c twi_proto.c counter_ext.avr.c \ +asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pos.c speed.c \ + postrack.c traj.c twi_proto.c eeprom.avr.c \ simu.host.c motor_model.host.c models.host.c test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c MODULES = proto uart utils math/fixed twi diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c index ff962496..89209a96 100644 --- a/digital/asserv/src/asserv/counter_ext.avr.c +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -24,6 +24,8 @@ * }}} */ #include "common.h" #include "counter.h" + +#include "modules/utils/utils.h" #include "io.h" /** diff --git a/digital/asserv/src/asserv/counter_tcc.avr.c b/digital/asserv/src/asserv/counter_tcc.avr.c index 9b64d916..030ebcc7 100644 --- a/digital/asserv/src/asserv/counter_tcc.avr.c +++ b/digital/asserv/src/asserv/counter_tcc.avr.c @@ -24,6 +24,7 @@ * }}} */ #include "common.h" #include "counter.h" + #include "modules/utils/utils.h" #include "io.h" diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 26e5c4b6..53d0af6a 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -1,4 +1,4 @@ -/* eeprom.c */ +/* eeprom.avr.c - Save parameters to internal EEPROM. */ /* asserv - Position & speed motor control on AVR. {{{ * * Copyright (C) 2005 Nicolas Schodet @@ -22,17 +22,20 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "eeprom.h" + #include -/* Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x45 -#define EEPROM_START 256 +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "postrack.h" -/* +AutoDec */ -/* -AutoDec */ +#define EEPROM_START 0 /* Read parameters from eeprom. */ -static void +void eeprom_read_params (void) { uint8_t *p8 = (uint8_t *) EEPROM_START; @@ -60,7 +63,7 @@ eeprom_read_params (void) } /* Write parameters to eeprom. */ -static void +void eeprom_write_params (void) { uint8_t *p8 = (uint8_t *) EEPROM_START; @@ -87,7 +90,7 @@ eeprom_write_params (void) } /* Clear eeprom parameters. */ -static void +void eeprom_clear_params (void) { uint8_t *p = (uint8_t *) EEPROM_START; diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h new file mode 100644 index 00000000..929d0da6 --- /dev/null +++ b/digital/asserv/src/asserv/eeprom.h @@ -0,0 +1,40 @@ +#ifndef eeprom_h +#define eeprom_h +/* eeprom.h - Save parameters to internal EEPROM. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/* Change the eeprom key each time you change eeprom format. */ +#define EEPROM_KEY 0x45 + +void +eeprom_read_params (void); + +void +eeprom_write_params (void); + +void +eeprom_clear_params (void); + +#endif /* eeprom_h */ diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 02d98701..385ac0e5 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -23,6 +23,7 @@ * * }}} */ #include "common.h" + #include "modules/uart/uart.h" #include "modules/proto/proto.h" #include "modules/utils/utils.h" @@ -31,36 +32,25 @@ #include "io.h" #include "misc.h" +#include "state.h" #include "counter.h" -#include "twi_proto.h" +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "postrack.h" +#include "traj.h" -/** Motor command sequence, do not use values above 127, do not use zero. */ -uint8_t main_sequence, main_sequence_ack, main_sequence_finish; +#include "twi_proto.h" +#include "eeprom.h" -/* This is implementation include. */ #ifndef HOST -# include "timer.avr.c" -# include "pwm.avr.c" +# include "timer.h" #else # include "simu.host.h" #endif -#include "pos.c" -#include "speed.c" -#include "postrack.c" -#include "traj.c" -#ifndef HOST -# include "eeprom.avr.c" -#endif -/** Motor control mode: - * 0: pwm setup. - * 1: shaft position control. - * 2: speed control. - * 3: trajectory control. */ -int8_t main_mode; - -/** Report trajectory end. */ +/** Report command completion. */ uint8_t main_sequence_ack_cpt = 2; /** Report of counters. */ @@ -93,14 +83,9 @@ uint8_t main_simu, main_simu_cpt; * analisys. */ uint8_t main_timer[6]; -/* +AutoDec */ - -/** Main loop. */ static void main_loop (void); -/* -AutoDec */ - /** Entry point. */ int main (int argc, char **argv) @@ -136,8 +121,8 @@ main_loop (void) /* Counter update. */ counter_update (); main_timer[0] = timer_read (); - /* Postion control. */ - if (main_mode >= 1) + /* Position control. */ + if (state_main.mode >= MODE_POS) pos_update (); main_timer[1] = timer_read (); /* Pwm setup. */ @@ -146,16 +131,17 @@ main_loop (void) /* Compute absolute position. */ postrack_update (); /* Compute trajectory. */ - if (main_mode >= 3) + if (state_main.mode >= MODE_TRAJ) traj_update (); /* Speed control. */ - if (main_mode >= 2) + if (state_main.mode >= MODE_SPEED) speed_update (); main_timer[3] = timer_read (); /* Stats. */ - if (main_sequence_ack != main_sequence_finish && !--main_sequence_ack_cpt) + if (state_main.sequence_ack != state_main.sequence_finish + && !--main_sequence_ack_cpt) { - proto_send1b ('A', main_sequence_finish); + proto_send1b ('A', state_main.sequence_finish); main_sequence_ack_cpt = 4; } if (main_stat_counter && !--main_stat_counter_cpt) @@ -227,7 +213,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('w', 0): /* Set zero pwm. */ pos_reset (); - main_mode = 0; + state_main.mode = MODE_PWM; pwm_left = 0; pwm_right = 0; break; @@ -236,7 +222,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: left pwm. * - w: right pwm. */ pos_reset (); - main_mode = 0; + state_main.mode = MODE_PWM; pwm_left = v8_to_v16 (args[0], args[1]); UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); pwm_right = v8_to_v16 (args[2], args[3]); @@ -246,13 +232,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Add to position consign. * - w: theta consign offset. * - w: alpha consign offset. */ - main_mode = 1; + state_main.mode = MODE_POS; pos_theta_cons += v8_to_v16 (args[0], args[1]); pos_alpha_cons += v8_to_v16 (args[2], args[3]); break; case c ('s', 0): /* Stop (set zero speed). */ - main_mode = 2; + state_main.mode = MODE_SPEED; speed_pos = 0; speed_theta_cons = 0; speed_alpha_cons = 0; @@ -261,7 +247,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set speed. * - b: theta speed. * - b: alpha speed. */ - main_mode = 2; + state_main.mode = MODE_SPEED; speed_pos = 0; speed_theta_cons = args[0] << 8; speed_alpha_cons = args[1] << 8; @@ -271,32 +257,32 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - d: theta consign offset. * - d: alpha consign offset. * - b: sequence number. */ - if (args[8] == main_sequence) + if (args[8] == state_main.sequence) break; - main_mode = 2; + state_main.mode = MODE_SPEED; speed_pos = 1; speed_theta_pos_cons = pos_theta_cons; speed_theta_pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); speed_alpha_pos_cons = pos_alpha_cons; speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); - main_sequence = args[8]; + state_start (&state_main, args[8]); break; case c ('f', 1): /* Go to the wall. * - b: sequence number. */ - if (args[0] == main_sequence) + if (args[0] == state_main.sequence) break; - main_mode = 3; + state_main.mode = MODE_TRAJ; speed_pos = 0; traj_mode = 10; - main_sequence = args[0]; + state_start (&state_main, args[0]); break; case c ('a', 1): /* Set acknoledge. * - b: ack sequence number. */ - main_sequence_ack = args[0]; - if (pos_blocked_state) + if (state_main.blocked) pos_reset (); + state_acknowledge (&state_main, args[0]); break; /* Stats. * - b: interval between stats. */ @@ -418,9 +404,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set PWM direction. * - b: inverse left direction. * - b: inverse right direction. */ - pwm_dir = 0; - if (args[1]) pwm_dir |= _BV (PWM_LEFT_DIR); - if (args[2]) pwm_dir |= _BV (PWM_RIGHT_DIR); + pwm_reverse (args[1], args[2]); break; case c ('E', 2): /* Write to eeprom. diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index eecd71f3..bfb6811e 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -22,6 +22,15 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "pos.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "counter.h" +#include "pwm.h" +#include "state.h" /** * This file is responsible for position motor control. The consign is a @@ -54,11 +63,6 @@ int32_t pos_blocked = 15000L; int32_t pos_theta_int, pos_alpha_int; /** Last error values. */ int32_t pos_theta_e_old, pos_alpha_e_old; -/** One if blocked. */ -uint8_t pos_blocked_state; - -/* +AutoDec */ -/* -AutoDec */ /** Compute a PID. * How to compute maximum numbers size: @@ -95,7 +99,7 @@ pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old, } /** Update PWM according to consign. */ -static void +void pos_update (void) { int16_t pid_theta, pid_alpha; @@ -106,15 +110,14 @@ pos_update (void) /* Compute PID. */ diff_theta = pos_theta_cons - pos_theta_cur; diff_alpha = pos_alpha_cons - pos_alpha_cur; - if (pos_blocked_state + if (state_main.blocked || diff_theta < -pos_blocked || pos_blocked < diff_theta || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) { /* Blocked. */ pwm_left = 0; pwm_right = 0; - pos_blocked_state = 1; - main_sequence_finish = main_sequence | 0x80; + state_blocked (&state_main); } else { @@ -132,13 +135,14 @@ pos_update (void) } } -/** Reset position control internal state. */ -static void +/** Reset position control internal state. To be called when the position + * control is deactivated. */ +void pos_reset (void) { pos_theta_int = pos_alpha_int = 0; pos_theta_cur = pos_alpha_cur = 0; pos_theta_cons = pos_alpha_cons = 0; pos_theta_e_old = pos_alpha_e_old = 0; - pos_blocked_state = 0; } + diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h new file mode 100644 index 00000000..ec6d93c9 --- /dev/null +++ b/digital/asserv/src/asserv/pos.h @@ -0,0 +1,47 @@ +#ifndef pos_h +#define pos_h +/* pos.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +extern uint32_t pos_theta_cur, pos_alpha_cur; +extern uint32_t pos_theta_cons, pos_alpha_cons; + +extern int32_t pos_e_sat; +extern int32_t pos_int_sat; +extern uint16_t pos_theta_kp, pos_alpha_kp; +extern uint16_t pos_theta_ki, pos_alpha_ki; +extern uint16_t pos_theta_kd, pos_alpha_kd; +extern int32_t pos_blocked; + +extern int32_t pos_theta_int, pos_alpha_int; +extern int32_t pos_theta_e_old, pos_alpha_e_old; + +void +pos_update (void); + +void +pos_reset (void); + +#endif /* pos_h */ diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c index 6da77592..14de6c09 100644 --- a/digital/asserv/src/asserv/postrack.c +++ b/digital/asserv/src/asserv/postrack.c @@ -1,4 +1,4 @@ -/* postrack.c */ +/* postrack.c - Compute current position from counters. */ /* asserv - Position & speed motor control on AVR. {{{ * * Copyright (C) 2006 Nicolas Schodet @@ -22,6 +22,12 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "postrack.h" + +#include "modules/math/fixed/fixed.h" + +#include "counter.h" /** Current position, f24.8. */ int32_t postrack_x, postrack_y; @@ -39,26 +45,10 @@ uint16_t postrack_footing; * a angle in f8.24 format. * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). * - there is a two factor because of the sum done in the motor control. */ -uint32_t postrack_footing_factor; - -/* +AutoDec */ +static uint32_t postrack_footing_factor; /** Initialise the position tracker. */ -static inline void -postrack_init (void); - -/** Update the current position. */ -static inline void -postrack_update (void); - -/** Change the footing value. */ -static inline void -postrack_set_footing (uint16_t footing); - -/* -AutoDec */ - -/** Initialise the position tracker. */ -static inline void +void postrack_init (void) { /* Prevent division by 0 by providing a default large value. */ @@ -68,7 +58,7 @@ postrack_init (void) #define M_PI 3.14159265358979323846 /* pi */ /** Update the current position. */ -static inline void +void postrack_update (void) { int32_t d, dd, da, na, dsc; @@ -107,10 +97,11 @@ postrack_update (void) #define M_1_PI 0.31830988618379067154 /* 1/pi */ /** Change the footing value. */ -static inline void +void postrack_set_footing (uint16_t footing) { postrack_footing = footing; postrack_footing_factor = (uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing; } + diff --git a/digital/asserv/src/asserv/postrack.h b/digital/asserv/src/asserv/postrack.h new file mode 100644 index 00000000..26fc4ee3 --- /dev/null +++ b/digital/asserv/src/asserv/postrack.h @@ -0,0 +1,41 @@ +#ifndef postrack_h +#define postrack_h +/* postrack.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +extern int32_t postrack_x, postrack_y; +extern int32_t postrack_a; +extern uint16_t postrack_footing; + +void +postrack_init (void); + +void +postrack_update (void); + +void +postrack_set_footing (uint16_t footing); + +#endif /* postrack_h */ diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c index b80483e0..58e18e9d 100644 --- a/digital/asserv/src/asserv/pwm.avr.c +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -22,6 +22,11 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "pwm.h" + +#include "modules/utils/utils.h" +#include "io.h" /** Define the PWM output used for left motor. */ #define PWM_LEFT_OCR OCR1B @@ -32,9 +37,6 @@ /** Define the direction output for right motor. */ #define PWM_RIGHT_DIR 5 -/** Define the absolute maximum PWM value. */ -#define PWM_MAX 0x3ff - /** PWM values, this is an error if absolute value is greater than the * maximum. */ int16_t pwm_left, pwm_right; @@ -42,20 +44,8 @@ int16_t pwm_left, pwm_right; * on port B. */ uint8_t pwm_dir = _BV (PWM_LEFT_DIR); -/* +AutoDec */ - /** Initialise PWM generator. */ -static inline void -pwm_init (void); - -/** Update the hardware PWM values. */ -static inline void -pwm_update (void); - -/* -AutoDec */ - -/** Initialise PWM generator. */ -static inline void +void pwm_init (void) { /* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic. @@ -71,7 +61,7 @@ pwm_init (void) } /** Update the hardware PWM values. */ -static inline void +void pwm_update (void) { uint16_t left, right; @@ -123,3 +113,11 @@ pwm_update (void) PWM_RIGHT_OCR = right; } +void +pwm_reverse (uint8_t left, uint8_t right) +{ + pwm_dir = 0; + if (left) pwm_dir |= _BV (PWM_LEFT_DIR); + if (right) pwm_dir |= _BV (PWM_RIGHT_DIR); +} + diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h new file mode 100644 index 00000000..2480f7eb --- /dev/null +++ b/digital/asserv/src/asserv/pwm.h @@ -0,0 +1,43 @@ +#ifndef pwm_h +#define pwm_h +/* pwm.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/** Define the absolute maximum PWM value. */ +#define PWM_MAX 0x3ff + +extern int16_t pwm_left, pwm_right; +extern uint8_t pwm_dir; + +void +pwm_init (void); + +void +pwm_update (void); + +void +pwm_reverse (uint8_t left, uint8_t right); + +#endif /* pwm_h */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 2041d67a..6e0a1cf8 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -21,16 +21,20 @@ * Web: http://perso.efrei.fr/~schodet/ * }}} */ #include "common.h" +#include "simu.host.h" + #include "modules/host/host.h" #include "modules/utils/utils.h" -#include "simu.host.h" -#include "motor_model.host.h" -#include "models.host.h" #include #include #include +#include "pwm.h" + +#include "motor_model.host.h" +#include "models.host.h" + /** Simulate some AVR regs. */ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; @@ -177,3 +181,23 @@ pwm_update (void) { } +void +eeprom_read_params (void) +{ +} + +void +eeprom_write_params (void) +{ +} + +void +eeprom_clear_params (void) +{ +} + +void +pwm_reverse (uint8_t left, uint8_t right) +{ +} + diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index 45778bd6..8d92c44b 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -23,68 +23,17 @@ * Web: http://perso.efrei.fr/~schodet/ * }}} */ -/** Simulate some AVR regs. */ extern uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; -/** Overall counter values. */ -extern uint16_t counter_left, counter_right; -/** Counter differences since last update. - * Maximum of 9 significant bits, sign included. */ -extern int16_t counter_left_diff, counter_right_diff; - -/** Define the direction output for left motor. */ -#define PWM_LEFT_DIR 4 -/** Define the direction output for right motor. */ -#define PWM_RIGHT_DIR 5 - -/** Define the absolute maximum PWM value. */ -#define PWM_MAX 0x3ff - -/** PWM values, this is an error if absolute value is greater than the - * maximum. */ -extern int16_t pwm_left, pwm_right; -/** PWM reverse direction, only set pwm dir bits or you will get weird results - * on port B. */ -extern uint8_t pwm_dir; - -/** Computed simulated position. */ extern double simu_pos_x, simu_pos_y, simu_pos_a; -#define EEPROM_KEY 0xa5 -#define eeprom_read_params() do { } while (0) -#define eeprom_write_params() do { } while (0) -#define eeprom_clear_params() do { } while (0) - -/* +AutoDec */ - -/** Initialise the timer. */ void timer_init (void); -/** Wait for timer overflow. */ void timer_wait (void); -/** Read timer value. Used for performance analysis. */ uint8_t timer_read (void); -/** Initialize the counters. */ -void -counter_init (void); - -/** Update overall counter values and compute diffs. */ -void -counter_update (void); - -/** Initialise PWM generator. */ -void -pwm_init (void); - -/** Update the hardware PWM values. */ -void -pwm_update (void); - -/* -AutoDec */ - #endif /* simu_host_h */ diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index 7e8ed558..be673273 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -22,6 +22,14 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "speed.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "pos.h" +#include "state.h" /** * This file is responsible for speed control. It changes the current shafts @@ -39,15 +47,12 @@ int8_t speed_theta_max, speed_alpha_max; int8_t speed_theta_slow, speed_alpha_slow; /** Consign position. */ uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; -/** Weither to use the consign position (1) or not (0). */ +/** Whether 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) @@ -104,11 +109,11 @@ speed_update_by_position (void) speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur, speed_alpha_acc, speed_alpha_max); if (speed_theta_cur == 0 && speed_alpha_cur == 0) - main_sequence_finish = main_sequence; + state_finish (&state_main); } /** Update shaft position consign according to consign. */ -static void +void speed_update (void) { /* Update speed. */ diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h new file mode 100644 index 00000000..82899871 --- /dev/null +++ b/digital/asserv/src/asserv/speed.h @@ -0,0 +1,40 @@ +#ifndef speed_h +#define speed_h +/* speed.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +extern int16_t speed_theta_cur, speed_alpha_cur; +extern int16_t speed_theta_cons, speed_alpha_cons; +extern int8_t speed_theta_max, speed_alpha_max; +extern int8_t speed_theta_slow, speed_alpha_slow; +extern uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; +extern uint8_t speed_pos; + +extern int16_t speed_theta_acc, speed_alpha_acc; + +void +speed_update (void); + +#endif /* speed_h */ diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h new file mode 100644 index 00000000..20000943 --- /dev/null +++ b/digital/asserv/src/asserv/state.h @@ -0,0 +1,110 @@ +#ifndef state_h +#define state_h +/* state.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/** There is two mechanism to acknowledge long lived command completion. + * + * The first one is dedicated to unreliable and full duplex channels like the + * serial port. It is based on sequence numbers. + * + * The second one is simpler, but can only be used with reliable and half + * duplex channels like the TWI bus. It just use a flag to remember command + * completion witch is reseted when a new command arrives. */ + +/** Control mode. */ +enum state_mode_t +{ + /** Simple PWM setup mode. */ + MODE_PWM, + /** Position control mode. */ + MODE_POS, + /** Speed control mode. */ + MODE_SPEED, + /** Trajectory control mode. */ + MODE_TRAJ, +}; + +/** Current motor state. */ +struct state_t +{ + /** Current motor control mode. */ + uint8_t mode; + /** Sequence number of the currently processed command, should be between + * 1 and 127. When a command is received on the serial port it is ignored + * if its sequence number is equal to the current sequence number. In + * this case a duplicated message is inferred. */ + uint8_t sequence; + /** Sequence number of the most recently finished command. When a command + * is finished, the current sequence number is copied to this variable. */ + uint8_t sequence_finish; + /** Sequence number of the most recently acknowledged command. Until this + * is not equal to the last finished command sequence number, a message is + * generated on the serial line. */ + uint8_t sequence_ack; + /** Simpler flag based mechanism, indicates if the last received command + * is finished. */ + uint8_t finished; + /** Wether the motor is blocked. */ + uint8_t blocked; +}; + +/** Main motors state. */ +struct state_t state_main; + +/** Start a new command execution. */ +static inline void +state_start (struct state_t *motor, uint8_t sequence) +{ + motor->sequence = sequence; + motor->finished = 0; +} + +/** Signal the current command completion. */ +static inline void +state_finish (struct state_t *motor) +{ + motor->sequence_finish = motor->sequence; + motor->finished = 1; +} + +/** Signal the current command completion. */ +static inline void +state_blocked (struct state_t *motor) +{ + motor->sequence_finish = motor->sequence | 0x80; + motor->blocked = 1; +} + +/** Acknowledge a command completion. */ +static inline void +state_acknowledge (struct state_t *motor, uint8_t sequence) +{ + motor->sequence_ack = sequence; + if (sequence == motor->sequence_finish) + motor->blocked = 0; +} + +#endif /* state_h */ diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c index dcad74d0..d0defdee 100644 --- a/digital/asserv/src/asserv/timer.avr.c +++ b/digital/asserv/src/asserv/timer.avr.c @@ -22,25 +22,13 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" -/* +AutoDec */ +#include "modules/utils/utils.h" +#include "io.h" /** Initialise the timer. */ -static inline void -timer_init (void); - -/** Wait for timer overflow. */ -static inline void -timer_wait (void); - -/** Read timer value. Used for performance analysis. */ -static inline uint8_t -timer_read (void); - -/* -AutoDec */ - -/** Initialise the timer. */ -static inline void +void timer_init (void) { TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, @@ -52,7 +40,7 @@ timer_init (void) } /** Wait for timer overflow. */ -static inline void +void timer_wait (void) { while (!(TIFR & _BV (TOV0))) @@ -61,10 +49,3 @@ timer_wait (void) TIFR = _BV (TOV0); } -/** Read timer value. Used for performance analysis. */ -static inline uint8_t -timer_read (void) -{ - return TCNT0; -} - diff --git a/digital/asserv/src/asserv/timer.h b/digital/asserv/src/asserv/timer.h new file mode 100644 index 00000000..754a2d46 --- /dev/null +++ b/digital/asserv/src/asserv/timer.h @@ -0,0 +1,41 @@ +#ifndef timer_h +#define timer_h +/* timer.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/** Read timer value. Used for performance analysis. */ +extern inline uint8_t +timer_read (void) +{ + return TCNT0; +} + +void +timer_init (void); + +void +timer_wait (void); + +#endif /* timer_h */ diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 926dcc5c..440c7be4 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -22,6 +22,19 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" +#include "traj.h" + +#include "io.h" + +#include "speed.h" +#include "state.h" + +#ifdef HOST +# include "simu.host.h" +#endif + +/* This file provides high level trajectories commands. */ /** Traj mode: * 10, 11: go to the wall. @@ -29,7 +42,7 @@ uint8_t traj_mode; /** Go to the wall mode. */ -void +static void traj_ftw (void) { int16_t speed; @@ -56,13 +69,13 @@ traj_ftw (void) speed_alpha_cons = 0; speed_theta_cur = 0; speed_alpha_cur = 0; - main_sequence_finish = main_sequence; + state_finish (&state_main); traj_mode = 11; } } /* Compute new speed according the defined trajectory. */ -static void +void traj_update (void) { switch (traj_mode) diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h new file mode 100644 index 00000000..fc127d60 --- /dev/null +++ b/digital/asserv/src/asserv/traj.h @@ -0,0 +1,33 @@ +#ifndef traj_h +#define traj_h +/* traj.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +extern uint8_t traj_mode; + +void +traj_update (void); + +#endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index bea56790..04ef34ae 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -24,6 +24,7 @@ * }}} */ #include "common.h" #include "twi_proto.h" + #include "modules/utils/utils.h" #include "modules/utils/byte.h" #include "modules/twi/twi.h" -- cgit v1.2.3