summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/Makefile9
-rw-r--r--n/asserv/src/avrconfig.h11
-rw-r--r--n/asserv/src/counter.c103
-rw-r--r--n/asserv/src/counter.h43
-rw-r--r--n/asserv/src/dsp.c41
-rw-r--r--n/asserv/src/dsp.h (renamed from n/asserv/src/pwm.h)32
-rw-r--r--n/asserv/src/dsp_check.pl53
-rw-r--r--n/asserv/src/main.c184
-rw-r--r--n/asserv/src/motor.c402
-rw-r--r--n/asserv/src/motor.h74
-rw-r--r--n/asserv/src/pwm.c77
-rw-r--r--n/asserv/src/speed.c159
-rw-r--r--n/asserv/src/test_dsp.c89
-rw-r--r--n/asserv/src/test_pwm.c18
-rw-r--r--n/asserv/src/timer.c38
-rw-r--r--n/asserv/src/timer.h39
16 files changed, 688 insertions, 684 deletions
diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile
index 1ba351e..35ce9b5 100644
--- a/n/asserv/src/Makefile
+++ b/n/asserv/src/Makefile
@@ -1,6 +1,7 @@
-PROGS = test_pwm asserv
-asserv_OBJECTS = motor.o main.o rs232.o proto.o
-test_pwm_OBJECTS = test_pwm.o pwm.o rs232.o proto.o
+PROGS = asserv test_pwm test_dsp
+asserv_OBJECTS = main.o rs232.o proto.o
+test_pwm_OBJECTS = test_pwm.o rs232.o proto.o
+test_dsp_OBJECTS = test_dsp.o rs232.o proto.o
DOC =
EXTRACTDOC =
MODULES = n/avr/rs232 n/avr/proto n/avr/utils
@@ -19,3 +20,5 @@ include Makefile.avr
asserv.elf: $(asserv_OBJECTS)
test_pwm.elf: $(test_pwm_OBJECTS)
+
+test_dsp.elf: $(test_dsp_OBJECTS)
diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h
index cf92dd8..16d55a7 100644
--- a/n/asserv/src/avrconfig.h
+++ b/n/asserv/src/avrconfig.h
@@ -1,10 +1,14 @@
#ifndef avrconfig_h
#define avrconfig_h
/* avrconfig.h - config file for avr projects. */
-/* n.asserv - Asservissement moteur continu. {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -19,9 +23,6 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
/* global */
@@ -54,7 +55,7 @@
/* proto - Protocol module. */
/** Maximum argument number. */
-#define AC_PROTO_MAX_ARGS 3
+#define AC_PROTO_MAX_ARGS 4
/** Protocol arguments type. */
#define AC_PROTO_ARG_TYPE uint8_t
diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c
index a9b5760..cfdf2e6 100644
--- a/n/asserv/src/counter.c
+++ b/n/asserv/src/counter.c
@@ -1,8 +1,12 @@
/* counter.c */
-/* {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,23 +21,40 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "counter.h"
-#include <n/avr/utils/utils.h>
+
+/** Forward and reverse counter values. */
+static uint8_t counter_left_frw, counter_left_rev,
+ counter_right_frw, counter_right_rev;
+/** Last TCNT values. */
+static uint8_t counter_left_old, counter_right_old;
+/** Overall counter values. */
+static uint16_t counter_left, counter_right;
+/** Counter differences since last update. */
+static int16_t counter_left_diff, counter_right_diff;
/* +AutoDec */
-/* -AutoDec */
-/** Forward and reverse counter value. */
-static uint8_t counter_l_frw, counter_l_rev, counter_r_frw, counter_r_rev;
-/** Last TCNT value. */
-static uint8_t counter_l_old, counter_r_old;
+/** Initialize the counters. */
+static inline void
+counter_init (void);
+
+/** Poll counters, should be called as often as possible. */
+static inline void
+counter_poll (void);
+
+/** Update overall counter values and compute diffs. */
+static inline void
+counter_update (void);
+
+/** Restart counting. */
+static inline void
+counter_restart (void);
+
+/* -AutoDec */
/** Initialize the counters. */
-inline void
+static inline void
counter_init (void)
{
/* Left counter. */
@@ -47,51 +68,67 @@ counter_init (void)
TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
0, 0, 0, 0, 0, 1, 1, 1);
/* Begin with safe values. */
- counter_reset ();
+ counter_restart ();
}
-/* Update counter, should be called as often as possible. */
-inline void
-counter_update (void)
+/** Poll counters, should be called as often as possible. */
+static inline void
+counter_poll (void)
{
uint8_t c;
- /* Update left counter. */
+ /* Read left counter. */
c = TCNT2;
if (PINE & _BV (4))
{
PORTD &= ~0x40;
- counter_l_frw += c - counter_l_old;
+ counter_left_frw += c - counter_left_old;
}
else
{
PORTD |= 0x40;
- counter_l_rev += c - counter_l_old;
+ counter_left_rev += c - counter_left_old;
}
- counter_l_old = c;
- /* Update right counter. */
+ counter_left_old = c;
+ /* Read right counter. */
c = TCNT3L;
if (PINE & _BV (5))
{
PORTD &= ~0x20;
- counter_r_frw += c - counter_r_old;
+ counter_right_frw += c - counter_right_old;
}
else
{
PORTD |= 0x20;
- counter_r_rev += c - counter_r_old;
+ counter_right_rev += c - counter_right_old;
}
- counter_r_old = c;
+ counter_right_old = c;
+}
+
+/** Update overall counter values and compute diffs. */
+static inline void
+counter_update (void)
+{
+ counter_left_diff = counter_left_frw;
+ counter_left_frw = 0;
+ counter_left_diff -= counter_left_rev;
+ counter_left_rev = 0;
+ counter_left += counter_left_diff;
+ counter_right_diff = counter_right_frw;
+ counter_right_frw = 0;
+ counter_right_diff -= counter_right_rev;
+ counter_right_rev = 0;
+ counter_right += counter_right_diff;
}
-/* Reset the counters. */
-inline void
-counter_reset (void)
+/** Restart counting. */
+static inline void
+counter_restart (void)
{
- counter_l_frw = 0;
- counter_l_rev = 0;
- counter_l_old = TCNT2;
- counter_r_frw = 0;
- counter_r_rev = 0;
- counter_r_old = TCNT3L;
+ counter_left_frw = 0;
+ counter_left_rev = 0;
+ counter_left_old = TCNT2;
+ counter_right_frw = 0;
+ counter_right_rev = 0;
+ counter_right_old = TCNT3L;
}
diff --git a/n/asserv/src/counter.h b/n/asserv/src/counter.h
deleted file mode 100644
index 1f8ed88..0000000
--- a/n/asserv/src/counter.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef counter_h
-#define counter_h
-/* counter.h */
-/* {{{
- *
- * Copyright (C) 2004 Nicolas Schodet
- *
- * 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.
- *
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
- * }}} */
-
-/* +AutoDec */
-
-/** Initialize the counters. */
-inline void
-counter_init (void);
-
-/* Update counter, should be called as often as possible. */
-inline void
-counter_update (void);
-
-/* Reset the counters. */
-inline void
-counter_reset (void);
-
-/* -AutoDec */
-
-#endif /* counter_h */
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 71317a5..9520513 100644
--- a/n/asserv/src/dsp.c
+++ b/n/asserv/src/dsp.c
@@ -1,8 +1,12 @@
/* dsp.c */
-/* {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,18 +21,15 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
#include "dsp.h"
/* +AutoDec */
/* -AutoDec */
-/* Add two signed words and saturate. */
-inline int16_t
-dsp_i16i16_add_sat (int16_t a, int16_t b)
+/** Add two signed words and saturate. UNTESTED. */
+extern inline int16_t
+dsp_add_sat_i16i16 (int16_t a, int16_t b)
{
asm ("add %A0, %A1\n\t"
"adc %B0, %B1\n\t"
@@ -42,14 +43,28 @@ dsp_i16i16_add_sat (int16_t a, int16_t b)
/* Load Min. */
"ldi %A0, 0x00\n\t"
"ldi %B0, 0x80\n\t"
- "1f" "\n\t"
- : "=r" a : "0" a, "r" b);
+ "1:" "\n\t"
+ : "=r" (a) : "0" (a), "r" (b));
return a;
}
-/* Multiply and saturate. */
-inline int16_t
-dsp_i16u8_mul_sat (int16_t a, uint8_t b)
+/** Multiply signed 16 by fixed 8.8. */
+extern inline int16_t
+dsp_mul_i16f88 (int16_t a, uint16_t b)
{
- asm ("mul
+ int16_t r;
+ asm ("mulsu %B1, %B2\n\t"
+ "mov %B0, r0\n\t"
+ "mul %A1, %A2\n\t"
+ "mov %A0, r1\n\t"
+ "mul %A1, %B2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "mulsu %B1, %A2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "clr r1\n\t"
+ : "=&r" (r) : "a" (a), "a" (b) : "r0", "r1");
+ return r;
}
+
diff --git a/n/asserv/src/pwm.h b/n/asserv/src/dsp.h
index 7ae4902..e498e1d 100644
--- a/n/asserv/src/pwm.h
+++ b/n/asserv/src/dsp.h
@@ -1,10 +1,14 @@
-#ifndef pwm_h
-#define pwm_h
-/* pwm.h */
-/* {{{
+#ifndef dsp_h
+#define dsp_h
+/* dsp.h */
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -19,24 +23,22 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
#include <inttypes.h>
/* +AutoDec */
-inline void
-pwm_init (void);
-
-inline void
-pwm_set_left (int16_t v);
+/** Add two signed words and saturate. UNTESTED. */
+extern inline int16_t
+dsp_add_sat_i16i16 (int16_t a, int16_t b);
-inline void
-pwm_set_right (int16_t v);
+/** Multiply signed 16 by fixed 8.8. */
+extern inline int16_t
+dsp_mul_i16f88 (int16_t a, uint16_t b);
/* -AutoDec */
-#endif /* pwm_h */
+#include "dsp.c"
+
+#endif /* dsp_h */
diff --git a/n/asserv/src/dsp_check.pl b/n/asserv/src/dsp_check.pl
new file mode 100644
index 0000000..69384e2
--- /dev/null
+++ b/n/asserv/src/dsp_check.pl
@@ -0,0 +1,53 @@
+#!/usr/bin/perl -w
+use strict;
+use POSIX qw(floor);
+
+my $fail = 0;
+
+sub check_mul
+{
+ my ($a, $b, $r) = @_;
+ $b = $b / 256;
+ my $m = floor ($a * $b);
+ if ($m > (1 << 15) - 1 || $m < -(1 << 15))
+ {
+ print "overflow $a * $b = $r ($m)\n";
+ }
+ elsif ($m == $r)
+ {
+ print "pass $a * $b = $r\n";
+ }
+ else
+ {
+ print "fail $a * $b = $r ($m)\n";
+ $fail = 1;
+ }
+}
+
+while (<>)
+{
+ chomp;
+ if (/^m (-?\d+) (\d+)$/)
+ {
+ my ($a, $b) = ($1, $2);
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ my $r = $1;
+ $_ = <>; chomp;
+ next unless (/^R (-?\d+)$/);
+ my $R = $1;
+ check_mul $a, $b, $r;
+ check_mul -$a, $b, $R;
+ }
+}
+
+if ($fail)
+{
+ print "test failled\n";
+ exit 1;
+}
+else
+{
+ print "test passed\n";
+ exit 0;
+}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 2a25fd7..ae44774 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -1,8 +1,12 @@
/* main.c */
-/* APBTasserv - asservissement Robot 2005 {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,20 +21,184 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "motor.h"
-
+#include "dsp.h"
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
#include <avr/interrupt.h>
+#include <avr/io.h>
+
+#include "pwm.c"
+#include "timer.c"
+#include "counter.c"
+#include "speed.c"
+
+/** Motor mode :
+ * 0 - pwm setup;
+ * 1 - speed control;
+ * 2 - position control. */
+int8_t motor_mode;
+
+/** Main loop counter. */
+uint8_t motor_loop_cpt;
+
+/** Statistics about speed control. */
+uint8_t motor_stat_speed, motor_stat_speed_cpt;
+
+/** Statistics about pwm values. */
+uint8_t motor_stat_pwm, motor_stat_pwm_cpt;
+
+/** Report of timer. */
+uint8_t motor_stat_timer, motor_stat_timer_cpt;
+
+/** Report of counters. */
+uint8_t motor_stat_counter, motor_stat_counter_cpt;
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t motor_timer_0, motor_timer_1, motor_timer_2;
+
+/* +AutoDec */
+/* Main loop. */
+static void
+main_loop (void);
+
+/** Handle incoming messages. */
+static void
+proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]);
+
+/* -AutoDec */
+
+/* Entry point. */
int
main (void)
{
DDRD = 0x60;
- motor_init ();
- motor_main ();
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ speed_init ();
+ rs232_init ();
+ proto_init (proto_callback, rs232_putc);
+ proto_send0 ('z');
sei ();
+ main_loop ();
return 0;
}
+
+/* Main loop. */
+static void
+main_loop (void)
+{
+ while (1)
+ {
+ motor_timer_0 = timer_read ();
+ while (!timer_pending ())
+ counter_poll ();
+ counter_update ();
+ motor_timer_2 = timer_read ();
+ /* Speed control. */
+ if (motor_mode >= 1)
+ {
+ speed_update ();
+ speed_compute_left_pwm ();
+ speed_compute_right_pwm ();
+ }
+ motor_timer_1 = timer_read ();
+ /* Pwm setup. */
+ pwm_update ();
+ /* Stats. */
+ if (motor_stat_speed && !--motor_stat_speed_cpt)
+ {
+ proto_send4 ('U',
+ speed_left_e_old >> 8, speed_left_e_old,
+ speed_left_int >> 8, speed_left_int);
+ proto_send4 ('V',
+ speed_right_e_old >> 8, speed_right_e_old,
+ speed_right_int >> 8, speed_right_int);
+ motor_stat_speed_cpt = motor_stat_speed;
+ }
+ if (motor_stat_pwm && !--motor_stat_pwm_cpt)
+ {
+ proto_send4 ('W',
+ pwm_left >> 8, pwm_left,
+ pwm_right >> 8, pwm_right);
+ motor_stat_pwm_cpt = motor_stat_pwm;
+ }
+ if (motor_stat_timer && !--motor_stat_timer_cpt)
+ {
+ proto_send3 ('T', motor_timer_2, motor_timer_1, motor_timer_0);
+ motor_stat_timer_cpt = motor_stat_timer;
+ }
+ if (motor_stat_counter && !--motor_stat_counter_cpt)
+ {
+ proto_send4 ('C',
+ counter_left >> 8, counter_left,
+ counter_right >> 8, counter_right);
+ motor_stat_counter_cpt = motor_stat_counter;
+ }
+ if ((motor_loop_cpt & 7) == 0)
+ {
+ if (rs232_poll ())
+ proto_accept (rs232_getc ());
+ }
+ motor_loop_cpt++;
+ }
+}
+
+/** Handle incoming messages. */
+static void
+proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
+{
+#define c(cmd, argc) (cmd << 8 | argc)
+ switch (c (cmd, argc))
+ {
+ case c ('z', 0):
+ reset ();
+ break;
+ case c ('w', 0):
+ speed_restart ();
+ motor_mode = 0;
+ pwm_left = 0;
+ pwm_right = 0;
+ break;
+ case c ('w', 4):
+ speed_restart ();
+ motor_mode = 0;
+ pwm_left = argv[0] << 8 | argv[1];
+ pwm_right = argv[2] << 8 | argv[3];
+ break;
+ case c ('s', 2):
+ motor_mode = 1;
+ speed_left_aim = argv[0];
+ speed_right_aim = argv[1];
+ break;
+ case c ('a', 1):
+ speed_acc_cpt = speed_acc = argv[0];
+ break;
+ case c ('p', 2):
+ speed_kp = argv[0] << 8 | argv[1];
+ break;
+ case c ('i', 2):
+ speed_ki = argv[0] << 8 | argv[1];
+ break;
+ case c ('m', 1):
+ motor_stat_speed_cpt = motor_stat_speed = argv[0];
+ motor_stat_pwm_cpt = motor_stat_pwm = argv[0];
+ break;
+ case c ('c', 1):
+ motor_stat_counter_cpt = motor_stat_counter = argv[0];
+ break;
+ case c ('t', 1):
+ motor_stat_timer_cpt = motor_stat_timer = argv[0];
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ proto_send (cmd, argc, argv);
+#undef c
+}
+
diff --git a/n/asserv/src/motor.c b/n/asserv/src/motor.c
deleted file mode 100644
index f54d989..0000000
--- a/n/asserv/src/motor.c
+++ /dev/null
@@ -1,402 +0,0 @@
-/* motor.c */
-/* APBTasserv - asservissement Robot 2005 {{{
- *
- * Copyright (C) 2004 Nicolas Schodet
- *
- * 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.
- *
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
- * }}} */
-#include "motor.h"
-#include "pwm.h"
-#include "timer.h"
-#include "counter.h"
-#include <n/avr/rs232/rs232.h>
-#include <n/avr/proto/proto.h>
-
-/* +AutoDec */
-/* -AutoDec */
-
-/* Variables globales. */
-uint8_t motor_asservi; /* Asservissement activé ? */
-uint8_t motor_pos_asserv; /* Si vrai, la carte d'asservissement est en
- mode d'asservissement en position. C'est à
- dire, l'ordinateur connecté à la carte
- d'asservissement gère lui même la vitesse
- afin d'asservir la position. En pratique,
- - pas d'acquitement lors d'un !v
- - arrét automatique si aucune commande !v
- reçue aprés n cycles. Voir motor_ttl. */
-uint8_t motor_ttl; /* Time to live : arrète le robot si arrive à
- 0 en mode asservissement position. */
-uint8_t motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */
-uint8_t motor_a; /* Acceleration. */
-uint8_t motor_a_cpt; /* Compteur d'acceleration. */
-int16_t motor_int_max; /* Terme intégral maximum. */
-uint8_t motor_pid_int; /* Compteur d'interruptions timer entre deux
- calculs de PID. */
-
-/* Statistiques, etc... */
-uint8_t motor_stat_delay; /* Delais entre les stats. */
-uint8_t motor_stat_delay_cpt; /* Compteur de stats. */
-uint8_t motor_cpt_delay; /* Delais entre rapport codeurs. */
-uint8_t motor_cpt_delay_cpt; /* Compteur entre les rapports. */
-
-/* Entrées. */
-uint8_t motor_gpi_delay; /* Delais entre deux envois. */
-uint8_t motor_gpi_delay_cpt; /* Compteur. */
-
-/* Moteurs. */
-int8_t motor_g_vdes, motor_g_vacc; /* Vitesse désirée, actuelle. */
-int8_t motor_d_vdes, motor_d_vacc;
-int16_t motor_g_cpt, motor_d_cpt; /* Compteurs. */
-int16_t motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le
- calcul de la dérivée. */
-int16_t motor_g_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les
- stats. */
-int16_t motor_g_int, motor_d_int; /* Valeur integrale. */
-int16_t motor_g_der, motor_d_der; /* Valeur dérivée. */
-
-/* On fait les cradingues... */
-#include "pwm.c"
-#include "timer.c"
-#include "counter.c"
-
-/* Initialise le module moteur. */
-void
-motor_init (void)
-{
- motor_asservi = 0;
- motor_pos_asserv = 0;
- motor_ttl = 10;
- motor_kp = 10; motor_ki = 5; motor_kd = 0;
- motor_a = 8;
- motor_a_cpt = 8;
- motor_int_max = 1000;
- motor_pid_int = 0;
- motor_stat_delay = 0; motor_stat_delay_cpt = 0;
- motor_cpt_delay = 0; motor_cpt_delay_cpt = 0;
- motor_gpi_delay = 0; motor_gpi_delay_cpt = 0;
- motor_g_vdes = 0; motor_g_vacc = 0;
- motor_d_vdes = 0; motor_d_vacc = 0;
- motor_g_cpt = 0; motor_d_cpt = 0;
- motor_g_e_old = 0; motor_d_e_old = 0;
- motor_g_pwm_old = 0; motor_d_pwm_old = 0;
- motor_g_int = 0; motor_d_int = 0;
- motor_g_der = 0; motor_d_der = 0;
- pwm_init ();
- timer_init ();
- counter_init ();
- rs232_init ();
- proto_init (motor_serial_callback, rs232_putc);
- proto_send0 ('z');
-}
-
-/* Limite un entier entre -MAX et MAX. */
-inline int16_t
-boundary (int16_t n, int16_t max)
-{
- if (n > max)
- return max;
- if (n < -max)
- return -max;
- return n;
-}
-
-/* Met à jour la vitesse du moteur gauche. */
-void
-motor_update_left_speed (void)
-{
- if (motor_g_vacc != motor_g_vdes)
- {
- if (motor_g_vacc < motor_g_vdes)
- {
- /* Accélère. */
- motor_g_vacc++;
- }
- else
- {
- /* Freine. */
- motor_g_vacc--;
- }
- }
-}
-
-/* Met à jour la vitesse du moteur droit. */
-void
-motor_update_right_speed (void)
-{
- if (motor_d_vacc != motor_d_vdes)
- {
- if (motor_d_vacc < motor_d_vdes)
- {
- /* Accélère. */
- motor_d_vacc++;
- }
- else
- {
- /* Freine. */
- motor_d_vacc--;
- }
- }
-}
-
-/* Calcule le PID associé au moteur gauche. */
-void
-motor_compute_left_pid (void)
-{
- int16_t e;
- int16_t diff;
- int16_t pwm;
- int16_t temp;
- /* Récupère les compteurs et calcule l'erreur. */
- diff = counter_l_frw;
- counter_l_frw = 0;
- diff -= counter_l_rev;
- counter_l_rev = 0;
- motor_g_cpt += diff;
- e = motor_g_vacc - diff; /* 10b = 8b + 9b */
- /* Calcul de l'integrale. */
- motor_g_int = motor_g_int + e; /* 12b = 11b + 10b */
- motor_g_int = boundary (motor_g_int, motor_int_max); /* 11b */
- /* Calcul de la dérivée. */
- //motor_g_der = safe_sub_long (e, motor_g_e_old);
- /* Calcul du PID. P... */
- pwm = e * motor_kp; /* 15b = 10b * 5b */
- /* I... */
- temp = motor_g_int * motor_ki; /* 15b = 11b * 4b */
- pwm = pwm + temp; /* 16b = 15b + 15b */
- /* D... */
- //temp = safe_mul_long (motor_g_der, motor_kd);
- //pwm = safe_add_long (pwm, temp);
- /* Conserve l'ancienne erreur pour le terme dérivé. */
- motor_g_e_old = e;
- /* Conserve l'ancienne pwm. */
- motor_g_pwm_old = pwm;
-}
-
-/* Calcule le PID associé au moteur droit. */
-void
-motor_compute_right_pid (void)
-{
- int16_t e;
- int16_t diff;
- int16_t pwm;
- int16_t temp;
- /* Récupère les compteurs et calcule l'erreur. */
- diff = counter_r_frw;
- counter_r_frw = 0;
- diff -= counter_r_rev;
- counter_r_rev = 0;
- motor_d_cpt += diff;
- e = motor_d_vacc - diff;
- /* Calcul de l'integrale. */
- motor_d_int = motor_d_int + e;
- motor_d_int = boundary (motor_d_int, motor_int_max);
- /* Calcul de la dérivée. */
- //motor_d_der = safe_sub_long (e, motor_d_e_old);
- /* Calcul du PID. P... */
- pwm = e * motor_kp;
- /* I... */
- temp = motor_d_int * motor_ki;
- pwm = pwm + temp;
- /* D... */
- //temp = safe_mul_long (motor_d_der, motor_kd);
- //pwm = safe_add_long (pwm, temp);
- /* Conserve l'ancienne erreur pour le terme dérivé. */
- motor_d_e_old = e;
- /* Conserve l'ancienne pwm. */
- motor_d_pwm_old = pwm;
-}
-
-/* Boucle principale. */
-void
-motor_main (void)
-{
- while (1)
- {
- /* Ne fait le traitement que s'il y a eu une interruption. */
- while (!timer_pending ())
- counter_update ();
- /* Calcul du PID. */
- if (1)
- {
- motor_compute_left_pid ();
- motor_compute_right_pid ();
- /* Applique les nouvelles valeurs au même moment. */
- if (motor_asservi)
- {
- pwm_set_left (motor_g_pwm_old);
- pwm_set_right (motor_d_pwm_old);
- }
- else
- {
- pwm_set_left (0);
- pwm_set_right (0);
- }
- /* Information de PWM. */
- if (motor_stat_delay)
- {
- if (!--motor_stat_delay_cpt)
- {
- proto_send4 ('l', motor_g_vacc, motor_g_e_old,
- motor_g_pwm_old >> 8, motor_g_pwm_old & 0xff);
- proto_send4 ('r', motor_d_vacc, motor_d_e_old,
- motor_d_pwm_old >> 8, motor_d_pwm_old & 0xff);
- motor_stat_delay_cpt = motor_stat_delay;
- }
- }
- /* Rapport des codeurs. */
- if (motor_cpt_delay)
- {
- if (!--motor_cpt_delay_cpt)
- {
- proto_send4 ('C', motor_g_cpt >> 8, motor_g_cpt & 0xff,
- motor_d_cpt >> 8, motor_d_cpt & 0xff);
- motor_cpt_delay_cpt = motor_cpt_delay;
- }
- }
- /* Accélère. */
- if (motor_a)
- {
- if (!--motor_a_cpt)
- {
- motor_update_left_speed ();
- motor_update_right_speed ();
- motor_a_cpt = motor_a;
- }
- }
- else
- {
- motor_g_vacc = motor_g_vdes;
- motor_d_vacc = motor_d_vdes;
- }
- }
- /* Le reste. */
- if ((motor_pid_int & 7) == 0)
- {
- /* Gestion du ttl. */
- if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) &&
- --motor_ttl == 0)
- {
- motor_g_vdes = 0;
- motor_d_vdes = 0;
- //serial_send_motor_ttl ();
- }
- if (rs232_poll ())
- proto_accept (rs232_getc ());
- /* GPI. */
- if (motor_gpi_delay)
- {
- if (!--motor_gpi_delay_cpt)
- {
- //serial_send_gpi (input_d ());
- motor_gpi_delay_cpt = motor_gpi_delay;
- }
- }
- }
- motor_pid_int++;
- }
-}
-
-/* Traite une entrée série. */
-void
-motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
-{
-#define c(cmd, argc) (cmd << 8 | argc)
- switch (c (cmd, argc))
- {
- case c ('z', 0):
- reset ();
- break;
- case c ('v', 2):
- motor_g_vdes = argv[0];
- motor_d_vdes = argv[1];
- if (motor_pos_asserv)
- {
- motor_ttl = 10;
- return;
- }
- break;
- case c ('V', 1):
- motor_pos_asserv = argv[0];
- break;
- case c ('s', 0):
- motor_g_vdes = 0;
- motor_d_vdes = 0;
- break;
- case c ('a', 1):
- motor_a = argv[0];
- motor_a_cpt = motor_a;
- break;
- case c ('p', 1):
- motor_kp = argv[0];
- break;
- case c ('i', 1):
- motor_ki = argv[0];
- break;
- case c ('d', 1):
- motor_kd = argv[0];
- break;
- case c ('m', 1):
- motor_stat_delay = argv[0];
- motor_stat_delay_cpt = motor_stat_delay;
- break;
- case c ('c', 1):
- motor_cpt_delay = argv[0];
- motor_cpt_delay_cpt = motor_cpt_delay;
- break;
- case c ('g', 1):
- motor_asservi = argv[0];
- motor_toggle_asservi ();
- break;
- case c ('h', 1):
- motor_gpi_delay = argv[0];
- motor_gpi_delay_cpt = motor_gpi_delay;
- break;
- case c ('k', 1):
- //temp = argv[0];
- //output_b (temp);
- break;
- case c ('D', 1):
- //temp = argv[0];
- //if (temp == 0x42)
- //enable_interrupts (INT_EXT);
- break;
- default:
- proto_send0 ('e');
- return;
- }
- proto_send (cmd, argc, argv);
-#undef c
-}
-
-/* Démarre l'asservissement. */
-void
-motor_toggle_asservi (void)
-{
- if (motor_asservi)
- {
- counter_reset ();
- motor_g_vacc = 0;
- motor_d_vdes = 0;
- motor_g_vdes = 0;
- motor_d_vdes = 0;
- motor_g_int = 0;
- motor_d_int = 0;
- }
-}
diff --git a/n/asserv/src/motor.h b/n/asserv/src/motor.h
deleted file mode 100644
index 5412712..0000000
--- a/n/asserv/src/motor.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#ifndef motor_h
-#define motor_h
-/* motor.h */
-/* {{{
- *
- * Copyright (C) 2004 Nicolas Schodet
- *
- * 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.
- *
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
- * }}} */
-
-#include <n/avr/proto/proto.h>
-#include <avr/signal.h>
-#include <inttypes.h>
-
-/* +AutoDec */
-
-/* Initialise le module moteur. */
-void
-motor_init (void);
-
-/* Limite un entier entre -MAX et MAX. */
-inline int16_t
-boundary (int16_t n, int16_t max);
-
-/* Met à jour la vitesse du moteur gauche. */
-void
-motor_update_left_speed (void);
-
-/* Met à jour la vitesse du moteur droit. */
-void
-motor_update_right_speed (void);
-
-/* Calcule le PID associé au moteur gauche. */
-void
-motor_compute_left_pid (void);
-
-/* Calcule le PID associé au moteur droit. */
-void
-motor_compute_right_pid (void);
-
-/* Boucle principale. */
-void
-motor_main (void);
-
-/* Traite une entrée série. */
-void
-motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]);
-
-short
-motor_parse (void);
-
-/* Démarre l'asservissement. */
-void
-motor_toggle_asservi (void);
-
-/* -AutoDec */
-
-#endif /* motor_h */
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 84791d4..7979842 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -1,8 +1,12 @@
/* pwm.c */
-/* {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,17 +21,29 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "pwm.h"
-#include <avr/io.h>
+
+/* PWM values. */
+int16_t pwm_left, pwm_right;
/* +AutoDec */
+
+/** Initialise PWM generator. */
+static inline void
+pwm_init (void);
+
+/** Preprocess a PWM value before sending it to hardware. */
+static inline uint8_t
+pwm_preproc (uint16_t v);
+
+/** Update the hardware PWM values. */
+static inline void
+pwm_update (void);
+
/* -AutoDec */
-inline void
+/** Initialise PWM generator. */
+static inline void
pwm_init (void)
{
/* No timer/counter interrupt. */
@@ -42,51 +58,50 @@ pwm_init (void)
DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2);
}
-static inline int16_t
-pwm_preproc (int16_t v)
+/** Preprocess a PWM value before sending it to hardware. */
+static inline uint8_t
+pwm_preproc (uint16_t v)
{
- v = (v >> 2) + ((v >> 1) & 1);
- /*if (v > 0)
- v += 0x0f;
- else if (v < 0)
- v -= 0x0f;*/
+ v += 0x10;
if (v > 255)
return 255;
- else if (v < -255)
- return -255;
else
return v;
}
-inline void
-pwm_set_left (int16_t v)
+/** Update the hardware PWM values. */
+static inline void
+pwm_update (void)
{
- v = pwm_preproc (v);
- if (v < 0)
+ /* Set left PWM. */
+ if (pwm_left == 0)
+ {
+ OCR1B = 0;
+ }
+ else if (pwm_left < 0)
{
PORTB &= ~_BV (2);
- OCR1B = -v;
+ OCR1B = pwm_preproc (-pwm_left);
}
else
{
PORTB |= _BV (2);
- OCR1B = v;
+ OCR1B = pwm_preproc (pwm_left);
}
-}
-
-inline void
-pwm_set_right (int16_t v)
-{
- v = pwm_preproc (v);
- if (v < 0)
+ /* Set right PWM. */
+ if (pwm_right == 0)
+ {
+ OCR1C = 0;
+ }
+ else if (pwm_right < 0)
{
PORTB |= _BV (3);
- OCR1C = -v;
+ OCR1C = pwm_preproc (-pwm_right);
}
else
{
PORTB &= ~_BV (3);
- OCR1C = v;
+ OCR1C = pwm_preproc (pwm_right);
}
}
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
new file mode 100644
index 0000000..cb8f2db
--- /dev/null
+++ b/n/asserv/src/speed.c
@@ -0,0 +1,159 @@
+/* speed.c - PI speed control. */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * 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.
+ *
+ * }}} */
+
+/** Actual speed. */
+int8_t speed_left, speed_right;
+/** Wanted speed. */
+int8_t speed_left_aim, speed_right_aim;
+/** Acceleration value. */
+uint8_t speed_acc = 8;
+/** Acceleration counter, speed gets updated when it reachs 0. */
+uint8_t speed_acc_cpt;
+/** Integral term. */
+int16_t speed_left_int, speed_right_int;
+/** Integral max value. */
+int16_t speed_int_max = 1024;
+/** Last error value. */
+int16_t speed_left_e_old, speed_right_e_old;
+/** P coeficients. 5.8 fixed point format. */
+uint16_t speed_kp = 2 * 255;
+/** I coeficients. 4.8 fixed point format. */
+uint16_t speed_ki = 1 * 255;
+
+/* +AutoDec */
+
+/** Initialise speed parameters. */
+static inline void
+speed_init (void);
+
+/** Update speeds according to the wanted speeds and the acceleration. */
+static inline void
+speed_update (void);
+
+/** Compute new pwm value for left motor. */
+static inline void
+speed_compute_left_pwm (void);
+
+/** Compute new pwm value for right motor. */
+static inline void
+speed_compute_right_pwm (void);
+
+/** Forget past event, usefull when the speed control is disabled for some
+ * time. */
+static inline void
+speed_restart (void);
+
+/* -AutoDec */
+
+/** Initialise speed parameters. */
+static inline void
+speed_init (void)
+{
+ speed_acc = 8;
+ speed_int_max = 1024;
+ speed_kp = 2 * 255;
+ speed_ki = 1 * 255;
+}
+
+/** Update speeds according to the wanted speeds and the acceleration. */
+static inline void
+speed_update (void)
+{
+ if (speed_acc)
+ {
+ speed_acc_cpt--;
+ if (speed_acc_cpt == 0)
+ {
+ speed_acc_cpt = speed_acc;
+ /* Update speeds. */
+ if (speed_left > speed_left_aim)
+ speed_left--;
+ else if (speed_left < speed_left_aim)
+ speed_left++;
+ if (speed_right > speed_right_aim)
+ speed_right--;
+ else if (speed_right < speed_right_aim)
+ speed_right++;
+ }
+ }
+ else
+ {
+ speed_left = speed_left_aim;
+ speed_right = speed_right_aim;
+ }
+}
+
+/** Compute new pwm value for left motor. */
+static inline void
+speed_compute_left_pwm (void)
+{
+ int16_t e;
+ int16_t pwm;
+ e = speed_left - counter_left_diff; /* 10b = 8b + 9b */
+ /* Integral update. */
+ speed_left_int += e; /* 12b = 11b + 10b */
+ if (speed_left_int > speed_int_max) /* 11b */
+ speed_left_int = speed_int_max;
+ else if (speed_left_int < -speed_left_int)
+ speed_left_int = -speed_int_max;
+ /* Compute PI. */ /* 16b = 15b + 15b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki); /* 15b = 11b * 4.8b */
+ /* Save result. */
+ speed_left_e_old = e;
+ pwm_left = pwm;
+}
+
+/** Compute new pwm value for right motor. */
+static inline void
+speed_compute_right_pwm (void)
+{
+ int16_t e;
+ int16_t pwm;
+ e = speed_right - counter_right_diff;
+ /* Integral update. */
+ speed_right_int += e;
+ if (speed_right_int > speed_int_max)
+ speed_right_int = speed_int_max;
+ else if (speed_right_int < -speed_right_int)
+ speed_right_int = -speed_int_max;
+ /* Compute PI. */
+ pwm = dsp_mul_i16f88 (e, speed_kp)
+ + dsp_mul_i16f88 (speed_right_int, speed_ki);
+ /* Save result. */
+ speed_right_e_old = e;
+ pwm_right = pwm;
+}
+
+/** Forget past event, usefull when the speed control is disabled for some
+ * time. */
+static inline void
+speed_restart (void)
+{
+ speed_left_int = 0;
+ speed_right_int = 0;
+ speed_left = 0;
+ speed_right = 0;
+}
diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c
new file mode 100644
index 0000000..37f8d2d
--- /dev/null
+++ b/n/asserv/src/test_dsp.c
@@ -0,0 +1,89 @@
+/* test_dsp.c */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * 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 "dsp.h"
+
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
+#include <avr/io.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+void
+proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
+{
+ int16_t a;
+ uint16_t b;
+ int16_t r;
+ int16_t i, j;
+ uint16_t w = 0xa66a;
+ switch (c | argc << 8)
+ {
+ case 'm' | 0 << 8:
+ for (i = 16; i > 0; i--)
+ {
+ a = w >> i;
+ for (j = 16; j >= 0; j--)
+ {
+ b = w >> j;
+ proto_send4 ('m',
+ (a >> 8) & 0xff, a & 0xff,
+ (b >> 8) & 0xff, b & 0xff);
+ r = dsp_mul_i16f88 (a, b);
+ proto_send2 ('r',
+ (r >> 8) & 0xff, r & 0xff);
+ r = dsp_mul_i16f88 (-a, b);
+ proto_send2 ('R',
+ (r >> 8) & 0xff, r & 0xff);
+ }
+ }
+ break;
+ case 'z' | 0 << 8:
+ reset ();
+ default:
+ proto_send0 ('e');
+ return;
+ }
+}
+
+int
+main (void)
+{
+ rs232_init ();
+ proto_init (proto_callback, rs232_putc);
+ rs232_putc ('!');
+ rs232_putc ('z');
+ rs232_putc ('d');
+ rs232_putc ('s');
+ rs232_putc ('p');
+ rs232_putc ('\r');
+ while (1)
+ {
+ uint8_t c = rs232_getc ();
+ proto_accept (c);
+ }
+ return 0;
+}
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
index ee1be2a..96e2d81 100644
--- a/n/asserv/src/test_pwm.c
+++ b/n/asserv/src/test_pwm.c
@@ -1,8 +1,12 @@
/* test_pwm.c */
-/* {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,12 +21,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "pwm.h"
-
#include <n/avr/rs232/rs232.h>
#include <n/avr/proto/proto.h>
#include <avr/io.h>
@@ -30,21 +29,24 @@
/* +AutoDec */
/* -AutoDec */
+#include "pwm.c"
+
void
proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
{
switch (c | argc << 8)
{
case 'l' | 2 << 8:
- pwm_set_left (argv[0] << 8 | argv[1]);
+ pwm_left = argv[0] << 8 | argv[1];
break;
case 'r' | 2 << 8:
- pwm_set_right (argv[0] << 8 | argv[1]);
+ pwm_right = argv[0] << 8 | argv[1];
break;
default:
proto_send0 ('e');
return;
}
+ pwm_update ();
proto_send (c, argc, argv);
}
diff --git a/n/asserv/src/timer.c b/n/asserv/src/timer.c
index b5d0b4d..1e18aca 100644
--- a/n/asserv/src/timer.c
+++ b/n/asserv/src/timer.c
@@ -1,8 +1,12 @@
/* timer.c */
-/* {{{
+/* asserv - Position & speed motor control on a ATmega128. {{{
*
* Copyright (C) 2004 Nicolas Schodet
*
+ * Robot APB Team/Efrei 2005.
+ * 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
@@ -17,19 +21,26 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
* }}} */
-#include "timer.h"
-#include <avr/io.h>
-#include <n/avr/utils/utils.h>
/* +AutoDec */
+
+/** Initialise the timer. */
+static inline void
+timer_init (void);
+
+/** Test if a overflow occured and reset the flag. */
+static inline int
+timer_pending (void);
+
+/** Read timer value. Used for performance analysis. */
+static inline int8_t
+timer_read (void);
+
/* -AutoDec */
/** Initialise the timer. */
-inline void
+static inline void
timer_init (void)
{
/* 1024 prescaler. */
@@ -39,8 +50,8 @@ timer_init (void)
* Tov = 1 / Fov = 4.44 ms */
}
-/* Test if a overflow occured and reset the flag. */
-inline int
+/** Test if a overflow occured and reset the flag. */
+static inline int
timer_pending (void)
{
if (TIFR & _BV (TOV0))
@@ -53,3 +64,10 @@ timer_pending (void)
return 0;
}
+/** Read timer value. Used for performance analysis. */
+static inline int8_t
+timer_read (void)
+{
+ return TCNT0;
+}
+
diff --git a/n/asserv/src/timer.h b/n/asserv/src/timer.h
deleted file mode 100644
index 8f6ae1e..0000000
--- a/n/asserv/src/timer.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#ifndef timer_h
-#define timer_h
-/* timer.h */
-/* {{{
- *
- * Copyright (C) 2004 Nicolas Schodet
- *
- * 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.
- *
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
- * }}} */
-
-/* +AutoDec */
-
-/** Initialise the timer. */
-inline void
-timer_init (void);
-
-/* Test if a overflow occured and reset the flag. */
-inline int
-timer_pending (void);
-
-/* -AutoDec */
-
-#endif /* timer_h */