summaryrefslogtreecommitdiff
path: root/n/line-follower/src/main.c
diff options
context:
space:
mode:
authorprot2004-12-09 23:57:42 +0000
committerprot2004-12-09 23:57:42 +0000
commit853fb5b5d899eb55f1f7d4044bff0a8d5d84b694 (patch)
treebe3ed35adefb3637e15963f70d12ce6402d35ccb /n/line-follower/src/main.c
parent603d1062ef6decd1a3065a12efc8f12480edb71b (diff)
Importing files from n/asserv
Diffstat (limited to 'n/line-follower/src/main.c')
-rw-r--r--n/line-follower/src/main.c232
1 files changed, 232 insertions, 0 deletions
diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c
new file mode 100644
index 0000000..72d4b39
--- /dev/null
+++ b/n/line-follower/src/main.c
@@ -0,0 +1,232 @@
+/* main.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/interrupt.h>
+#include <avr/io.h>
+
+#include "pwm.c"
+#include "timer.c"
+#include "counter.c"
+#include "speed.c"
+#include "postrack.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;
+
+/** Report position. */
+uint8_t motor_stat_postrack, motor_stat_postrack_cpt;
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3;
+
+/* +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;
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ speed_init ();
+ postrack_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_3 = timer_read ();
+ postrack_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_send4 ('T', motor_timer_3, 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_stat_postrack && !--motor_stat_postrack_cpt)
+ {
+ proto_send4 ('X',
+ postrack_x >> 24, postrack_x >> 16,
+ postrack_x >> 8, postrack_x);
+ proto_send4 ('Y',
+ postrack_y >> 24, postrack_y >> 16,
+ postrack_y >> 8, postrack_y);
+ proto_send4 ('A',
+ postrack_a >> 24, postrack_a >> 16,
+ postrack_a >> 8, postrack_a);
+ motor_stat_postrack_cpt = motor_stat_postrack;
+ }
+ /* Misc. */
+ 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 ('f', 2):
+ postrack_set_footing (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;
+ case c ('T', 1):
+ motor_stat_postrack_cpt = motor_stat_postrack = argv[0];
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ proto_send (cmd, argc, argv);
+#undef c
+}
+