From 435b5a9adaa251e8d72ccf5b744db80296b30908 Mon Sep 17 00:00:00 2001 From: schodet Date: Tue, 17 Jan 2006 16:39:17 +0000 Subject: Ouh lala, plein de modifs, dont l'ajout d'un modèle de moteur à courant continue, le changement du PID, de la PWM et l'ajout de commandes dans main. --- n/asserv/src/asserv/Makefile | 3 ++ n/asserv/src/asserv/avrconfig.h | 4 +- n/asserv/src/asserv/counter.c | 8 ++-- n/asserv/src/asserv/eeprom.c | 2 +- n/asserv/src/asserv/main.c | 48 +++++++++++++++++-- n/asserv/src/asserv/misc.h | 38 +++++++++++++++ n/asserv/src/asserv/motor_model.c | 88 ++++++++++++++++++++++++++++++++++ n/asserv/src/asserv/motor_model.h | 56 ++++++++++++++++++++++ n/asserv/src/asserv/pos.c | 27 ++++++----- n/asserv/src/asserv/pwm.c | 8 ++-- n/asserv/src/asserv/speed.c | 21 ++++---- n/asserv/src/asserv/test_motor_model.c | 78 ++++++++++++++++++++++++++++++ n/asserv/src/asserv/timer.c | 6 +-- 13 files changed, 346 insertions(+), 41 deletions(-) create mode 100644 n/asserv/src/asserv/misc.h create mode 100644 n/asserv/src/asserv/motor_model.c create mode 100644 n/asserv/src/asserv/motor_model.h create mode 100644 n/asserv/src/asserv/test_motor_model.c diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile index 4e8f054..d618c0a 100644 --- a/n/asserv/src/asserv/Makefile +++ b/n/asserv/src/asserv/Makefile @@ -1,9 +1,12 @@ BASE = ../../../avr PROGS = asserv +HOST_PROGS = test_motor_model asserv_SOURCES = main.c +test_motor_model_SOURCES = test_motor_model.c motor_model.c DOC = EXTRACTDOC = MODULES = proto uart utils +test_motor_model_MODULES = CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... AVR_MCU = atmega128 diff --git a/n/asserv/src/asserv/avrconfig.h b/n/asserv/src/asserv/avrconfig.h index 461818a..2c2c103 100644 --- a/n/asserv/src/asserv/avrconfig.h +++ b/n/asserv/src/asserv/avrconfig.h @@ -32,7 +32,7 @@ /* uart - UART module. */ /** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ -#define AC_UART0_PORT 0 +#define AC_UART0_PORT 1 /** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, * 115200, 230400, 250000, 500000, 1000000. */ #define AC_UART0_BAUDRATE 115200 @@ -55,7 +55,7 @@ /** If the send buffer is full when putc: * - DROP: drop the new byte. * - WAIT: wait until there is room in the send buffer. */ -#define AC_UART0_SEND_BUFFER_FULL DROP +#define AC_UART0_SEND_BUFFER_FULL WAIT /** In HOST compilation: * - STDIO: use stdin/out. * - PTS: use pseudo terminal. */ diff --git a/n/asserv/src/asserv/counter.c b/n/asserv/src/asserv/counter.c index 6ac2cea..c4135b4 100644 --- a/n/asserv/src/asserv/counter.c +++ b/n/asserv/src/asserv/counter.c @@ -85,12 +85,12 @@ SIGNAL (SIG_INTERRUPT4) if (PINE & _BV (4)) { counter_left_rev += c - counter_left_old; - PORTD |= 0x40; + GREEN_LED (!COUNTER_REVERSE_LEFT); } else { counter_left_frw += c - counter_left_old; - PORTD &= ~0x40; + GREEN_LED (COUNTER_REVERSE_LEFT); } counter_left_old = c; } @@ -103,12 +103,12 @@ SIGNAL (SIG_INTERRUPT5) if (PINE & _BV (5)) { counter_right_rev += c - counter_right_old; - PORTD |= 0x20; + RED_LED (!COUNTER_REVERSE_RIGHT); } else { counter_right_frw += c - counter_right_old; - PORTD &= ~0x20; + RED_LED (COUNTER_REVERSE_RIGHT); } counter_right_old = c; } diff --git a/n/asserv/src/asserv/eeprom.c b/n/asserv/src/asserv/eeprom.c index 44f6ec2..34770bb 100644 --- a/n/asserv/src/asserv/eeprom.c +++ b/n/asserv/src/asserv/eeprom.c @@ -24,7 +24,7 @@ * }}} */ #include -#define EEPROM_KEY 0xa9 +#define EEPROM_KEY 0xa5 /* +AutoDec */ /* -AutoDec */ diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index 3bb0a5d..f56c662 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -29,6 +29,8 @@ #include "modules/utils/byte.h" #include "io.h" +#include "misc.h" + /* This is implementation include. */ #include "timer.c" #include "counter.c" @@ -61,7 +63,8 @@ uint8_t main_print_pin, main_print_pin_cpt; /** Record timer value at different stage of computing. Used for performance * analisys. */ -uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4; +uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4, + main_timer_5; /* +AutoDec */ @@ -73,11 +76,12 @@ main_loop (void); /** Entry point. */ int -main (void) +main (int argc, char **argv) { DDRD = 0x60; /* Pull-ups. */ - PORTA = _BV (7) | _BV (1) | _BV (0); + PORTA = 0xff; + PORTC = 0xff; eeprom_read_params (); pwm_init (); timer_init (); @@ -94,17 +98,22 @@ main (void) static void main_loop (void) { + main_timer_5 = timer_read (); timer_wait (); /* Counter update. */ counter_update (); + main_timer_0 = timer_read (); /* Postion control. */ if (main_mode >= 1) pos_update (); + main_timer_1 = timer_read (); /* Pwm setup. */ pwm_update (); + main_timer_2 = timer_read (); /* Speed control. */ if (main_mode >= 2) speed_update (); + main_timer_3 = timer_read (); /* Stats. */ if (main_stat_counter && !--main_stat_counter_cpt) { @@ -124,8 +133,8 @@ main_loop (void) } if (main_stat_timer && !--main_stat_timer_cpt) { - proto_send5b ('T', main_timer_4, main_timer_3, main_timer_2, - main_timer_1, main_timer_0); + proto_send6b ('T', main_timer_0, main_timer_2, main_timer_3, + main_timer_4, main_timer_4, main_timer_5); main_stat_timer_cpt = main_stat_timer; } if (main_print_pin && !--main_print_pin_cpt) @@ -136,6 +145,7 @@ main_loop (void) /* Misc. */ while (uart0_poll ()) proto_accept (uart0_getc ()); + main_timer_4 = timer_read (); } /** Handle incoming messages. */ @@ -165,6 +175,30 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_left = v8_to_v16 (args[0], args[1]); pwm_right = v8_to_v16 (args[2], args[3]); break; + case c ('c', 4): + /* Add to position consign. + * - w: theta consign offset. + * - w: alpha consign offset. */ + main_mode = 1; + 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; + speed_pos = 0; + speed_theta_cons = 0; + speed_alpha_cons = 0; + break; + case c ('s', 2): + /* Set speed. + * - b: theta speed. + * - b: alpha speed. */ + main_mode = 2; + speed_pos = 0; + speed_theta_cons = args[0] << 8; + speed_alpha_cons = args[1] << 8; + break; /* Stats. * - b: interval between stats. */ case c ('C', 1): @@ -193,6 +227,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { switch (c (args[0], size)) { + case c ('a', 5): + speed_theta_acc = v8_to_v16 (args[1], args[2]); + speed_alpha_acc = v8_to_v16 (args[3], args[4]); + break; case c ('p', 3): pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]); break; diff --git a/n/asserv/src/asserv/misc.h b/n/asserv/src/asserv/misc.h new file mode 100644 index 0000000..59f2513 --- /dev/null +++ b/n/asserv/src/asserv/misc.h @@ -0,0 +1,38 @@ +#ifndef misc_h +#define misc_h +/* misc.h - Miscellaneous definitions. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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. + * + * }}} */ + +#define RED_LED(x) do { \ + if (!(x)) PORTD |= 0x20; \ + else PORTD &= ~0x20; \ +} while (0) + +#define GREEN_LED(x) do { \ + if (!(x)) PORTD |= 0x40; \ + else PORTD &= ~0x40; \ +} while (0) + +#endif /* misc_h */ diff --git a/n/asserv/src/asserv/motor_model.c b/n/asserv/src/asserv/motor_model.c new file mode 100644 index 0000000..bff5cad --- /dev/null +++ b/n/asserv/src/asserv/motor_model.c @@ -0,0 +1,88 @@ +/* motor_model.c - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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. + * + * }}} */ +#include "common.h" +#include "motor_model.h" + +/** + * Switching to french for all thoses non english speaking people. + * + * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux + * parties, la modélisation électrique et la modélisation mécanique. + * + * On peut trouver de l'aide sur : + * - le site de maxon : http://www.maxonmotor.com/ + * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html + * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm + * + * Voici ce qui en résulte, des belles équations différentielles : + * u(t) = R i(t) + L di(t)/dt + Ke o(t) + * J do(t)/dt = Kt i(t) - Rf o(t) + * dth(t)/dt = o(t) + * + * Les variables sont décrites dans la structure motor_t. + * + * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en + * exercice. + * + * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il + * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive + * à : + * + * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t)) + * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t)) + * th(t+h) = th(t) + h o(t) + * + * C'est consternant de simplicité non ? + */ + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m) +{ + double i_, o_, th_; /* New computed values. */ + double h; /* Infinitesimal step... Well, not so infinite. */ + int d; + d = m->d; + h = m->h / d; + /* Make several small steps to increase precision. */ + for (; d; d--) + { + /* Ah, the mistical power of computation. */ + i_ = m->i + + h * (1.0 / m->L * m->u + - m->R / m->L * m->i + - 1.0 / m->Ke / m->L * m->o); + o_ = m->o + + h * m->i_G * m->i_G * m->ro_G / m->J + * (m->Kt * m->i - m->Rf * m->o); + th_ = m->th + h * m->o; + /* Ok, now store this step. */ + m->i = i_; + m->o = o_; + m->th = th_; + } + /* Damn! It's finished yet! */ + m->t += m->h; +} + diff --git a/n/asserv/src/asserv/motor_model.h b/n/asserv/src/asserv/motor_model.h new file mode 100644 index 0000000..a7446e1 --- /dev/null +++ b/n/asserv/src/asserv/motor_model.h @@ -0,0 +1,56 @@ +#ifndef motor_model_h +#define motor_model_h +/* motor_model.h - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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. + * + * }}} */ + +/** Motor and load caracteristics and current data. */ +struct motor_t +{ + /* Motor caracteristics. */ + double Ke; /* Speed constant ((rad/s)/V). */ + double Kt; /* Torque constant (N.m/A). */ + double Rf; /* Bearing friction (N.m/(rad/s)). */ + double R; /* Terminal resistance (Ohm). */ + double L; /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + double i_G; /* Gearbox ratio. */ + double ro_G;/* Gearbox efficiency. */ + /* Load caracteristics. */ + double J; /* Load (kg.m^2). */ + /* Simulation parameters. */ + double h; /* Simulation time step (s). */ + int d; /* Simulation time step division. */ + /* Simulation current state. */ + double t; /* Current time (not realy used) (s). */ + double u; /* Current input voltage (V). */ + double i; /* Current current (A). */ + double o; /* Current angular speed (o for omega) (rad/s). */ + double th; /* Current theta (th for theta) (rad). */ +}; + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m); + +#endif /* motor_model_h */ diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c index d1c7e8f..b6bee63 100644 --- a/n/asserv/src/asserv/pos.c +++ b/n/asserv/src/asserv/pos.c @@ -32,35 +32,35 @@ */ /** Current theta/alpha. */ -uint16_t pos_theta_cur, pos_alpha_cur; +uint32_t pos_theta_cur, pos_alpha_cur; /** Consign theta/alpha. */ -uint16_t pos_theta_cons, pos_alpha_cons; +uint32_t pos_theta_cons, pos_alpha_cons; /** Error saturation. */ -int16_t pos_e_sat = 1023; +int32_t pos_e_sat = 1023; /** Integral saturation. */ -int16_t pos_int_sat = 8191; +int32_t pos_int_sat = 1023; /** P coefficients. */ -uint16_t pos_theta_kp = 0, pos_alpha_kp = 0; +uint16_t pos_theta_kp = 1200, pos_alpha_kp = 1500; /** I coefficients. */ -uint16_t pos_theta_ki = 0, pos_alpha_ki = 0; +uint16_t pos_theta_ki = 120, pos_alpha_ki = 150; /** D coefficients. */ -uint16_t pos_theta_kd = 0, pos_alpha_kd = 0; +uint16_t pos_theta_kd = 2000, pos_alpha_kd = 2000; /** Current integral values. */ -int16_t pos_theta_int, pos_alpha_int; +int32_t pos_theta_int, pos_alpha_int; /** Last error values. */ -int16_t pos_theta_e_old, pos_alpha_e_old; +int32_t pos_theta_e_old, pos_alpha_e_old; /* +AutoDec */ /* -AutoDec */ /** Compute a PID. */ static inline int16_t -pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old, +pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old, uint16_t kp, uint16_t ki, uint16_t kd) { - int16_t diff, pid; + int32_t diff, pid; /* Saturate error. */ UTILS_BOUND (e, -pos_e_sat, pos_e_sat); /* Integral update. */ @@ -72,7 +72,7 @@ pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old, pid = e * kp + *i * ki + diff * kd; /* Save result. */ *e_old = e; - return pid; + return pid >> 12; } /** Update PWM according to consign. */ @@ -102,4 +102,7 @@ static 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; } diff --git a/n/asserv/src/asserv/pwm.c b/n/asserv/src/asserv/pwm.c index 4b5abc5..50f4842 100644 --- a/n/asserv/src/asserv/pwm.c +++ b/n/asserv/src/asserv/pwm.c @@ -33,7 +33,7 @@ #define PWM_RIGHT_DIR 2 /** Define the absolute maximum PWM value. */ -#define PWM_MAX 0x1ff +#define PWM_MAX 0x3ff /** PWM values, this is an error if absolute value is greater than the * maximum. */ @@ -58,12 +58,12 @@ pwm_update (void); static inline void pwm_init (void) { - /* Fast PWM, TOP = 0x1ff, OC1B & OC1C with positive logic. + /* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic. f_IO without prescaler. - Fpwm = f_IO / (prescaler * (1 + TOP)) = 28912 Hz. */ + Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */ TCCR1A = regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, - 0, 0, 1, 0, 1, 0, 1, 0); + 0, 0, 1, 0, 1, 0, 1, 1); TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, 0, 0, 0, 0, 1, 0, 0, 1); /* Enable pwm and direction outputs in DDRB. */ diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c index 87223e2..d354307 100644 --- a/n/asserv/src/asserv/speed.c +++ b/n/asserv/src/asserv/speed.c @@ -29,12 +29,12 @@ * shaft position. */ -/** Current speed, uf8.8. */ +/** Current speed, f8.8. */ int16_t speed_theta_cur, speed_alpha_cur; -/** Consign speed, uf8.8. */ +/** Consign speed, f8.8. */ int16_t speed_theta_cons, speed_alpha_cons; /** Consign position. */ -uint16_t speed_theta_pos_cons, speed_alpha_pos_cons; +uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; /** Weither to use the consign position (1) or not (0). */ uint8_t speed_pos; @@ -52,18 +52,15 @@ speed_update_by_speed (void) 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; + 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; + else + speed_alpha_cur -= speed_alpha_acc; } /** Update shaft position consign according to a position consign. */ @@ -77,9 +74,13 @@ speed_update_by_position (void) static void speed_update (void) { + /* Update speed. */ if (speed_pos) speed_update_by_position (); else speed_update_by_speed (); + /* Update shaft position. */ + pos_theta_cur += speed_theta_cur >> 8; + pos_alpha_cur += speed_alpha_cur >> 8; } diff --git a/n/asserv/src/asserv/test_motor_model.c b/n/asserv/src/asserv/test_motor_model.c new file mode 100644 index 0000000..14bab66 --- /dev/null +++ b/n/asserv/src/asserv/test_motor_model.c @@ -0,0 +1,78 @@ +/* test_motor_model.c - Test DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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. + * + * }}} */ +#include "common.h" +#include "motor_model.h" + +#include +#include + +/* Our robot model. */ +struct motor_t taz = +{ + /* Motor caracteristics. */ + 407 * M_2_PI / 60, /* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.18, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + 10, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load caracteristics. */ + 10 * 0.04 * 0.04, /* Load (kg.m^2). */ + /* Simulation parameters. */ + 4.444444 / 1000, /* Simulation time step (s). */ + 1000, /* Simulation time step division. */ + /* Simulation current state. */ + 0, /* Current time (not realy used) (s). */ + 0, /* Current input voltage (V). */ + 0, /* Current current (A). */ + 0, /* Current angular speed (o for omega) (rad/s). */ + 0 /* Current theta (th for theta) (rad). */ +}; + +void simu (struct motor_t *m, double t) +{ + int i, s; + s = t / m->h; + for (i = 0; i < s; i++) + { + motor_model_step (m); + printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); + } +} + +int +main (void) +{ + /* Make a step response simulation. */ + printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); + taz.u = 3.0; + printf ("%12f %12f %12f %12f %12f\n", taz.t, taz.u, taz.i, taz.o, taz.th); + simu (&taz, 1.0); + taz.u = 0.0; + simu (&taz, 1.0); + return 0; +} diff --git a/n/asserv/src/asserv/timer.c b/n/asserv/src/asserv/timer.c index c065317..e91d8a8 100644 --- a/n/asserv/src/asserv/timer.c +++ b/n/asserv/src/asserv/timer.c @@ -30,12 +30,12 @@ static inline void timer_init (void) { - /* 64 prescaler. */ TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, - 0, 0, 0, 0, 0, 1, 0, 0); + 0, 0, 0, 0, 0, 1, 1, 0); /* Fov = F_io / (prescaler * (TOP + 1)) * TOP = 0xff - * Tov = 1 / Fov = 1.111 ms */ + * prescaler = 256 + * Tov = 1 / Fov = 4.444 ms */ } /** Wait for timer overflow. */ -- cgit v1.2.3