summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
authorschodet2004-10-13 00:12:18 +0000
committerschodet2004-10-13 00:12:18 +0000
commit7868feb67d7c2d1414cbcd84b6f02a7305a65117 (patch)
tree35826ef1077ff133862b295f93242f89e4b7dc68 /n/asserv
parentfdd6216054017f4aded7501d4a6e725710438a35 (diff)
Version toute nouvelle qu'elle est bien et bien organisée.
Gestion du mode pwm simple. Gestion de la virgule fixe dans les coefs. Préparation pour les améliorations futures. Séparation dans des fichiers différents. Netoyage.
Diffstat (limited to 'n/asserv')
-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 */