summaryrefslogtreecommitdiff
path: root/n/asserv/src/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/main.c')
-rw-r--r--n/asserv/src/main.c184
1 files changed, 176 insertions, 8 deletions
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
+}
+