From d47046315caff4ce01b3ea39126b476e56ab3703 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 20 Apr 2008 21:29:29 +0200 Subject: * digital/asserv/src/asserv: - added absolute auxiliary motor position command. --- digital/asserv/src/asserv/Makefile | 2 +- digital/asserv/src/asserv/aux.c | 54 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/aux.h | 43 ++++++++++++++++++++++++++++ digital/asserv/src/asserv/main.c | 14 +++++++-- digital/asserv/src/asserv/twi_proto.c | 11 +++---- 5 files changed, 114 insertions(+), 10 deletions(-) create mode 100644 digital/asserv/src/asserv/aux.c create mode 100644 digital/asserv/src/asserv/aux.h diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile index f3987bd7..4a61d08b 100644 --- a/digital/asserv/src/asserv/Makefile +++ b/digital/asserv/src/asserv/Makefile @@ -3,7 +3,7 @@ PROGS = asserv AVR_PROGS = test_counter HOST_PROGS = test_motor_model asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pwm_mp.avr.c \ - pwm_ocr.avr.c pos.c speed.c postrack.c traj.c \ + pwm_ocr.avr.c pos.c speed.c postrack.c traj.c aux.c \ twi_proto.c eeprom.avr.c state.c \ simu.host.c motor_model.host.c models.host.c test_counter_SOURCES = test_counter.c timer.avr.c diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c new file mode 100644 index 00000000..880496b4 --- /dev/null +++ b/digital/asserv/src/asserv/aux.c @@ -0,0 +1,54 @@ +/* aux.c - Auxiliary motors commands. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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 "aux.h" + +#include "state.h" + +#include "counter.h" +#include "pos.h" +#include "speed.h" + +/** Motor state. */ +struct aux_t aux0; + +/** Update positions. */ +void +aux_pos_update (void) +{ + /* Easy... */ + aux0.pos += counter_aux0_diff; +} + +/** Goto position. */ +void +aux_traj_goto_start (uint16_t pos, uint8_t seq) +{ + speed_aux0.use_pos = 1; + speed_aux0.pos_cons = pos_aux0.cur; + speed_aux0.pos_cons += (int16_t) (pos - aux0.pos); + state_start (&state_aux0, MODE_SPEED, seq); +} + diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h new file mode 100644 index 00000000..175933cc --- /dev/null +++ b/digital/asserv/src/asserv/aux.h @@ -0,0 +1,43 @@ +#ifndef aux_h +#define aux_h +/* aux.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/** Auxiliary motor informations. */ +struct aux_t +{ + /** Absolute position. */ + int16_t pos; +}; + +extern struct aux_t aux0; + +void +aux_pos_update (void); + +void +aux_traj_goto_start (uint16_t pos, uint8_t seq); + +#endif /* aux_h */ diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index a8d40b08..8be5373f 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -40,6 +40,7 @@ #include "speed.h" #include "postrack.h" #include "traj.h" +#include "aux.h" #include "twi_proto.h" #include "eeprom.h" @@ -132,6 +133,7 @@ main_loop (void) main_timer[2] = timer_read (); /* Compute absolute position. */ postrack_update (); + aux_pos_update (); /* Compute trajectory. */ if (state_main.mode >= MODE_TRAJ) traj_update (); @@ -160,7 +162,7 @@ main_loop (void) } if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) { - proto_send1w ('Z', pos_aux0.cur); + proto_send1w ('Y', aux0.pos); main_stat_aux_pos_cpt = main_stat_aux_pos; } if (main_stat_speed && !--main_stat_speed_cpt) @@ -383,6 +385,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v32 (0, args[8], args[9], 0), args[10]); break; + case c ('y', 3): + /* Auxiliary go to position. + * - w: pos, i16. + * - b: sequence number. */ + if (args[2] == state_aux0.sequence) + break; + aux_traj_goto_start (v8_to_v16 (args[0], args[1]), args[2]); + break; case c ('a', 2): /* Set both acknoledge. * - b: main ack sequence number. @@ -408,7 +418,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Position stats. */ main_stat_postrack_cpt = main_stat_postrack = args[0]; break; - case c ('Z', 1): + case c ('Y', 1): /* Auxiliary position stats. */ main_stat_aux_pos_cpt = main_stat_aux_pos = args[0]; break; diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index c7564766..ffc55061 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -38,6 +38,7 @@ #include "speed.h" #include "postrack.h" #include "traj.h" +#include "aux.h" struct twi_proto_t { @@ -83,8 +84,8 @@ twi_proto_update (void) status[7] = v32_to_v8 (postrack_y, 1); status[8] = v32_to_v8 (postrack_a, 2); status[9] = v32_to_v8 (postrack_a, 1); - status[10] = v16_to_v8 (pos_aux0.cons, 1); - status[11] = v16_to_v8 (pos_aux0.cons, 0); + status[10] = v16_to_v8 (aux0.pos, 1); + status[11] = v16_to_v8 (aux0.pos, 0); twi_sl_update (status, sizeof (status)); } @@ -166,12 +167,8 @@ twi_proto_callback (u8 *buf, u8 size) /* Move the arm. * - w: new position. * - b: speed. */ - speed_aux0.use_pos = 1; - speed_aux0.pos_cons = pos_aux0.cons; - speed_aux0.pos_cons += v8_to_v32 (0, 0, buf[2], buf[3]); speed_aux0.max = buf[4]; - speed_aux0.slow = buf[4]; - state_start (&state_aux0, MODE_SPEED, 0); + aux_traj_goto_start (v8_to_v16 (buf[2], buf[3]), 0); break; case c ('p', x): /* Set parameters. */ -- cgit v1.2.3