summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
authorschodet2006-01-17 16:39:17 +0000
committerschodet2006-01-17 16:39:17 +0000
commit435b5a9adaa251e8d72ccf5b744db80296b30908 (patch)
treebbb366c0826073f4a4111b2a5af4a295180aee8c /n/asserv/src
parent1e15fde47f958a7564c45e852bbb7e9b2c31a3ce (diff)
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.
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/asserv/Makefile3
-rw-r--r--n/asserv/src/asserv/avrconfig.h4
-rw-r--r--n/asserv/src/asserv/counter.c8
-rw-r--r--n/asserv/src/asserv/eeprom.c2
-rw-r--r--n/asserv/src/asserv/main.c48
-rw-r--r--n/asserv/src/asserv/misc.h38
-rw-r--r--n/asserv/src/asserv/motor_model.c88
-rw-r--r--n/asserv/src/asserv/motor_model.h56
-rw-r--r--n/asserv/src/asserv/pos.c27
-rw-r--r--n/asserv/src/asserv/pwm.c8
-rw-r--r--n/asserv/src/asserv/speed.c21
-rw-r--r--n/asserv/src/asserv/test_motor_model.c78
-rw-r--r--n/asserv/src/asserv/timer.c6
13 files changed, 346 insertions, 41 deletions
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 <avr/eeprom.h>
-#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 <math.h>
+#include <stdio.h>
+
+/* 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. */