From 853fb5b5d899eb55f1f7d4044bff0a8d5d84b694 Mon Sep 17 00:00:00 2001 From: prot Date: Thu, 9 Dec 2004 23:57:42 +0000 Subject: Importing files from n/asserv --- n/line-follower/src/main.c | 232 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 232 insertions(+) create mode 100644 n/line-follower/src/main.c (limited to 'n/line-follower/src/main.c') 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 +#include +#include +#include +#include + +#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 +} + -- cgit v1.2.3