From df7d0d94ecc69fa9aadd2ab90db89b5ef1a74030 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 29 Apr 2010 23:03:56 +0200 Subject: digital/mimot/src/dirty: add copy/pasted firmware for mimot, refs #123 This is a quick and dirty firmware copied from asserv. This will be improved once #124 is closed. --- digital/mimot/src/dirty/main.c | 453 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 453 insertions(+) create mode 100644 digital/mimot/src/dirty/main.c (limited to 'digital/mimot/src/dirty/main.c') diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c new file mode 100644 index 00000000..9b674461 --- /dev/null +++ b/digital/mimot/src/dirty/main.c @@ -0,0 +1,453 @@ +/* main.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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 "modules/math/fixed/fixed.h" +#include "io.h" + +#include "state.h" + +#include "counter.h" +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "aux.h" + +#include "twi_proto.h" +#include "eeprom.h" + +#ifndef HOST +# include "timer.h" +#else +# include "simu.host.h" +#endif + +/** Report command completion. */ +uint8_t main_sequence_ack = 4, main_sequence_ack_cpt; + +/** Report of counters. */ +uint8_t main_stat_counter, main_stat_counter_cpt; + +/** Report of auxiliary position. */ +uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt; + +/** Statistics about speed control. */ +uint8_t main_stat_speed, main_stat_speed_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[6]; + +static void +main_loop (void); + +/** Entry point. */ +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + /* Pull-ups. */ + PORTB = 0xe0; + PORTC = 0xfc; + PORTD = 0x80; + pwm_init (); + timer_init (); + counter_init (); + uart0_init (); + twi_proto_init (); + speed_init (); + aux_init (); + eeprom_read_params (); + proto_send0 ('z'); + sei (); + while (1) + main_loop (); + return 0; +} + +/** Main loop. */ +static void +main_loop (void) +{ + main_timer[5] = timer_read (); + timer_wait (); + /* Counter update. */ + counter_update (); + main_timer[0] = timer_read (); + /* Position control. */ + pos_update (); + main_timer[1] = timer_read (); + /* Pwm setup. */ + pwm_update (); + main_timer[2] = timer_read (); + /* Compute absolute position. */ + aux_pos_update (); + /* Compute trajectory. */ + aux_traj_update (); + /* Speed control. */ + speed_update (); + main_timer[3] = timer_read (); + /* Stats. */ + if (main_sequence_ack + && (state_aux[0].sequence_ack != state_aux[0].sequence_finish + || state_aux[1].sequence_ack != state_aux[1].sequence_finish) + && !--main_sequence_ack_cpt) + { + proto_send2b ('A', state_aux[0].sequence_finish, + state_aux[1].sequence_finish); + main_sequence_ack_cpt = main_sequence_ack; + } + if (main_stat_counter && !--main_stat_counter_cpt) + { + proto_send2w ('C', counter_aux[0], counter_aux[1]); + main_stat_counter_cpt = main_stat_counter; + } + if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) + { + proto_send2w ('Y', aux[0].pos, aux[1].pos); + main_stat_aux_pos_cpt = main_stat_aux_pos; + } + if (main_stat_speed && !--main_stat_speed_cpt) + { + proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8); + main_stat_speed_cpt = main_stat_speed; + } + if (main_stat_pos && !--main_stat_pos_cpt) + { + proto_send4w ('P', pos_aux[0].e_old, pos_aux[0].i, + pos_aux[1].e_old, pos_aux[1].i); + main_stat_pos_cpt = main_stat_pos; + } + if (main_stat_pwm && !--main_stat_pwm_cpt) + { + proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur); + main_stat_pwm_cpt = main_stat_pwm; + } + if (main_stat_timer && !--main_stat_timer_cpt) + { + proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3], + main_timer[4], main_timer[4], main_timer[5]); + main_stat_timer_cpt = main_stat_timer; + } + if (main_print_pin && !--main_print_pin_cpt) + { + proto_send1b ('I', PINC & 0xfc); + main_print_pin_cpt = main_print_pin; + } + /* Misc. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); + twi_proto_update (); + main_timer[4] = timer_read (); +} + +/** Handle incoming messages. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ + /* Many commands use the first argument as a selector. */ + struct aux_t *auxp = 0; + struct pwm_t *pwm = 0; + struct pos_t *pos = 0; + struct speed_t *speed = 0; + struct state_t *state = 0; + if (args[0] < AC_ASSERV_AUX_NB) + { + auxp = &aux[args[0]]; + pwm = &pwm_aux[args[0]]; + pos = &pos_aux[args[0]]; + speed = &speed_aux[args[0]]; + state = &state_aux[args[0]]; + } + /* Decode command. */ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset. */ + utils_reset (); + break; + /* Commands. */ + case c ('w', 0): + /* Set zero auxiliary pwm. */ + pos_reset (&pos_aux[0]); + pos_reset (&pos_aux[1]); + state_aux[0].mode = MODE_PWM; + state_aux[1].mode = MODE_PWM; + pwm_set (&pwm_aux[0], 0); + pwm_set (&pwm_aux[1], 0); + break; + case c ('w', 3): + /* Set auxiliary pwm. + * - b: aux index. + * - w: pwm. */ + if (!auxp) { proto_send0 ('?'); return; } + pos_reset (pos); + state->mode = MODE_PWM; + pwm_set (pwm, v8_to_v16 (args[1], args[2])); + break; + case c ('c', 3): + /* Add to auxiliary position consign. + * - b: aux index. + * - w: consign offset. */ + if (!auxp) { proto_send0 ('?'); return; } + state->mode = MODE_POS; + state->variant = 0; + pos->cons += v8_to_v16 (args[1], args[2]); + break; + case c ('s', 1): + /* Set auxiliary zero speed. + * - b: aux index. */ + if (!auxp) { proto_send0 ('?'); return; } + state->mode = MODE_SPEED; + state->variant = 0; + speed->use_pos = 0; + speed->cons = 0; + break; + case c ('s', 2): + /* Set auxiliary speed. + * - b: aux index. + * - b: speed. */ + if (!auxp) { proto_send0 ('?'); return; } + state->mode = MODE_SPEED; + state->variant = 0; + speed->use_pos = 0; + speed->cons = args[1] << 8; + break; + case c ('s', 6): + /* Set auxiliary speed controlled position consign. + * - b: aux index. + * - d: consign offset. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[5] == state->sequence) + break; + speed->use_pos = 1; + speed->pos_cons = pos->cons; + speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]); + state_start (state, MODE_SPEED, args[5]); + break; + case c ('y', 4): + /* Auxiliary go to position. + * - b: aux index. + * - w: pos, i16. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[3] == state->sequence) + break; + aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]); + break; + case c ('y', 3): + /* Auxiliary find zero. + * - b: aux index. + * - b: speed. + * - b: sequence number. */ + if (!auxp) { proto_send0 ('?'); return; } + if (args[2] == state->sequence) + break; + aux_traj_find_zero_start (auxp, args[1], args[2]); + break; + case c ('a', 2): + /* Set all acknoledge. + * - b: first auxiliary ack sequence number. + * - b: second auxiliary ack sequence number. */ + state_acknowledge (&state_aux[0], args[1]); + state_acknowledge (&state_aux[1], args[2]); + break; + /* Stats. + * - b: interval between stats. */ + case c ('A', 1): + /* Command completion report. */ + main_sequence_ack_cpt = main_sequence_ack = args[0]; + break; + case c ('C', 1): + /* Counter stats. */ + main_stat_counter_cpt = main_stat_counter = args[0]; + break; + case c ('Y', 1): + /* Auxiliary position stats. */ + main_stat_aux_pos_cpt = main_stat_aux_pos = args[0]; + break; + case c ('S', 1): + /* Motor speed control stats. */ + main_stat_speed_cpt = main_stat_speed = args[0]; + break; + case c ('P', 1): + /* Auxiliary 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') + { + /* Many commands use the first argument as a selector. */ + switch (args[1]) + { + case 0: + case 1: + pos = &pos_aux[args[1]]; + speed = &speed_aux[args[1]]; + break; + default: + pos = 0; + speed = 0; + break; + } + switch (c (args[0], size)) + { + case c ('a', 4): + /* Set acceleration. + * - b: index. + * - w: acceleration. */ + if (!speed) { proto_send0 ('?'); return; } + speed->acc = v8_to_v16 (args[2], args[3]); + break; + case c ('s', 4): + /* Set maximum and slow speed. + * - b: index. + * - b: max speed. + * - b: slow speed. */ + if (!speed) { proto_send0 ('?'); return; } + speed->max = args[2]; + speed->slow = args[3]; + break; + case c ('p', 4): + /* Set proportional coefficient. + * - b: index. + * - w: P coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kp = v8_to_v16 (args[2], args[3]); + break; + case c ('i', 4): + /* Set integral coefficient. + * - b: index. + * - w: I coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->ki = v8_to_v16 (args[2], args[3]); + break; + case c ('d', 4): + /* Set differential coefficient. + * - b: index. + * - w: D coefficient. */ + if (!pos) { proto_send0 ('?'); return; } + pos->kd = v8_to_v16 (args[2], args[3]); + break; + case c ('b', 7): + /* Set blocking detection parameters. + * - b: index. + * - w: error limit. + * - w: speed limit. + * - b: counter limit. */ + if (!pos) { proto_send0 ('?'); return; } + pos->blocked_error_limit = v8_to_v16 (args[2], args[3]); + pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]); + pos->blocked_counter_limit = args[6]; + break; + case c ('E', 3): + pos_e_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('I', 3): + pos_i_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('D', 3): + pos_d_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('w', 2): + /* Set PWM direction. + * - b: bits: 0000[aux1][aux0][right][left]. */ + pwm_set_reverse (args[1]); + 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 ('a', speed_aux[0].acc, speed_aux[1].acc); + proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow); + proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow); + proto_send3w ('b', pos_aux[0].blocked_error_limit, + pos_aux[0].blocked_speed_limit, + pos_aux[0].blocked_counter_limit); + proto_send3w ('b', pos_aux[1].blocked_error_limit, + pos_aux[1].blocked_speed_limit, + pos_aux[1].blocked_counter_limit); + proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp); + proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki); + proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd); + proto_send1w ('E', pos_e_sat); + proto_send1w ('I', pos_i_sat); + proto_send1w ('D', pos_d_sat); + proto_send1b ('w', pwm_reverse); + break; + default: + proto_send0 ('?'); + return; + } + } + else + { + proto_send0 ('?'); + return; + } + break; + } + proto_send (cmd, size, args); +#undef c +} + -- cgit v1.2.3