From 0cc8897a51d07baaf42db53f70c9c9aac5272b86 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 6 Nov 2005 18:01:08 +0000 Subject: Première version du programme asserv. --- n/asserv/src/asserv/main.c | 260 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 260 insertions(+) create mode 100644 n/asserv/src/asserv/main.c (limited to 'n/asserv/src/asserv/main.c') 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 +} + -- cgit v1.2.3