summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/main.c')
-rw-r--r--n/asserv/src/asserv/main.c260
1 files changed, 260 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
new file mode 100644
index 0000000..2f9c77b
--- /dev/null
+++ b/n/asserv/src/asserv/main.c
@@ -0,0 +1,260 @@
+/* main.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "modules/uart/uart.h"
+#include "modules/proto/proto.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/byte.h"
+#include "io.h"
+
+/* This is implementation include. */
+#include "timer.c"
+#include "counter.c"
+#include "pwm.c"
+#include "pos.c"
+#include "eeprom.c"
+
+/** Motor control mode:
+ * 0: pwm setup.
+ * 1: shaft position control.
+ * 2: speed control.
+ * 3: trajectory control. */
+int8_t main_mode;
+
+/** Report of counters. */
+uint8_t main_stat_counter, main_stat_counter_cpt;
+
+/** Statistics about shaft position control. */
+uint8_t main_stat_pos, main_stat_pos_cpt;
+
+/** Statistics about pwm values. */
+uint8_t main_stat_pwm, main_stat_pwm_cpt;
+
+/** Report of timer. */
+uint8_t main_stat_timer, main_stat_timer_cpt;
+
+/** Print input port. */
+uint8_t main_print_pin, main_print_pin_cpt;
+
+/** Record timer value at different stage of computing. Used for performance
+ * analisys. */
+uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4;
+
+/* +AutoDec */
+
+/** Main loop. */
+static void
+main_loop (void);
+
+/* -AutoDec */
+
+/** Entry point. */
+int
+main (void)
+{
+ DDRD = 0x60;
+ /* Pull-ups. */
+ PORTA = _BV (7) | _BV (1) | _BV (0);
+ eeprom_read_params ();
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ uart0_init ();
+ proto_send0 ('z');
+ sei ();
+ while (1)
+ main_loop ();
+ return 0;
+}
+
+/** Main loop. */
+static void
+main_loop (void)
+{
+ timer_wait ();
+ /* Counter update. */
+ counter_update ();
+ /* Postion control. */
+ if (main_mode >= 1)
+ pos_update ();
+ /* Pwm setup. */
+ pwm_update ();
+ /* Stats. */
+ if (main_stat_counter && !--main_stat_counter_cpt)
+ {
+ proto_send2w ('C', counter_left, counter_right);
+ main_stat_counter_cpt = main_stat_counter;
+ }
+ if (main_stat_pos && !--main_stat_pos_cpt)
+ {
+ proto_send4w ('P', pos_theta_e_old, pos_theta_int,
+ pos_alpha_e_old, pos_alpha_int);
+ main_stat_pos_cpt = main_stat_pos;
+ }
+ if (main_stat_pwm && !--main_stat_pwm_cpt)
+ {
+ proto_send2w ('W', pwm_left, pwm_right);
+ main_stat_pwm_cpt = main_stat_pwm;
+ }
+ if (main_stat_timer && !--main_stat_timer_cpt)
+ {
+ proto_send5b ('T', main_timer_4, main_timer_3, main_timer_2,
+ main_timer_1, main_timer_0);
+ main_stat_timer_cpt = main_stat_timer;
+ }
+ if (main_print_pin && !--main_print_pin_cpt)
+ {
+ proto_send1b ('I', PINA);
+ main_print_pin_cpt = main_print_pin;
+ }
+ /* Misc. */
+ while (uart0_poll ())
+ proto_accept (uart0_getc ());
+}
+
+/** Handle incoming messages. */
+void
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
+{
+#define c(cmd, size) (cmd << 8 | size)
+ switch (c (cmd, size))
+ {
+ case c ('z', 0):
+ utils_reset ();
+ break;
+ /* Commands. */
+ case c ('w', 0):
+ /* Set zero pwm. */
+ pos_reset ();
+ main_mode = 0;
+ pwm_left = 0;
+ pwm_right = 0;
+ break;
+ case c ('w', 4):
+ /* Set pwm.
+ * - w: left pwm.
+ * - w: right pwm. */
+ pos_reset ();
+ main_mode = 0;
+ pwm_left = v8_to_v16 (args[0], args[1]);
+ pwm_right = v8_to_v16 (args[2], args[3]);
+ break;
+ /* Stats.
+ * - b: interval between stats. */
+ case c ('C', 1):
+ /* Counter stats. */
+ main_stat_counter_cpt = main_stat_counter = args[0];
+ break;
+ case c ('P', 1):
+ /* Motor position control stats. */
+ main_stat_pos_cpt = main_stat_pos = args[0];
+ break;
+ case c ('W', 1):
+ /* Pwm stats. */
+ main_stat_pwm_cpt = main_stat_pwm = args[0];
+ break;
+ case c ('T', 1):
+ /* Timing stats. */
+ main_stat_timer_cpt = main_stat_timer = args[0];
+ break;
+ case c ('I', 1):
+ /* Input port stats. */
+ main_print_pin_cpt = main_print_pin = args[0];
+ break;
+ default:
+ /* Params. */
+ if (cmd == 'p')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('p', 3):
+ pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('p', 5):
+ pos_theta_kp = v8_to_v16 (args[1], args[2]);
+ pos_alpha_kp = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('i', 3):
+ pos_theta_ki = pos_alpha_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 5):
+ pos_theta_ki = v8_to_v16 (args[1], args[2]);
+ pos_alpha_ki = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('d', 3):
+ pos_theta_kd = pos_alpha_kd = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('d', 5):
+ pos_theta_kd = v8_to_v16 (args[1], args[2]);
+ pos_alpha_kd = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('E', 3):
+ pos_e_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('I', 3):
+ pos_int_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('w', 3):
+ /* Set PWM direction.
+ * - b: inverse left direction.
+ * - b: inverse right direction. */
+ pwm_dir = 0;
+ if (args[1]) pwm_dir |= _BV (PWM_LEFT_DIR);
+ if (args[2]) pwm_dir |= _BV (PWM_RIGHT_DIR);
+ break;
+ case c ('E', 2):
+ /* Write to eeprom.
+ * - b: 00: clear config, 01: write config. */
+ if (args[1])
+ eeprom_write_params ();
+ else
+ eeprom_clear_params ();
+ break;
+ case c ('P', 1):
+ /* Print current settings. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send2w ('p', pos_theta_kp, pos_alpha_kp);
+ proto_send2w ('i', pos_theta_ki, pos_alpha_ki);
+ proto_send2w ('d', pos_theta_kd, pos_alpha_kd);
+ proto_send1w ('E', pos_e_sat);
+ proto_send1w ('I', pos_int_sat);
+ proto_send1b ('w', pwm_dir);
+ break;
+ default:
+ proto_send0 ('?');
+ return;
+ }
+ }
+ else
+ {
+ proto_send0 ('?');
+ return;
+ }
+ break;
+ }
+ proto_send (cmd, size, args);
+#undef c
+}
+