summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2008-03-09 17:40:28 +0100
committerNicolas Schodet2008-03-09 17:40:28 +0100
commita60c072e17cd502a06d9cea5de928b899d5cac16 (patch)
treea5c0c387fb9549f774fa4bbc3f2dd92fa77c0265
parent9943963706b24fec06d09999e706cca975039e92 (diff)
* digital/asserv/src/asserv:
- cleaned up, separated compilation. - prepared auxialliary motor control.
-rw-r--r--digital/asserv/src/asserv/Makefile3
-rw-r--r--digital/asserv/src/asserv/counter_ext.avr.c2
-rw-r--r--digital/asserv/src/asserv/counter_tcc.avr.c1
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c21
-rw-r--r--digital/asserv/src/asserv/eeprom.h40
-rw-r--r--digital/asserv/src/asserv/main.c80
-rw-r--r--digital/asserv/src/asserv/pos.c28
-rw-r--r--digital/asserv/src/asserv/pos.h47
-rw-r--r--digital/asserv/src/asserv/postrack.c33
-rw-r--r--digital/asserv/src/asserv/postrack.h41
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c32
-rw-r--r--digital/asserv/src/asserv/pwm.h43
-rw-r--r--digital/asserv/src/asserv/simu.host.c30
-rw-r--r--digital/asserv/src/asserv/simu.host.h51
-rw-r--r--digital/asserv/src/asserv/speed.c17
-rw-r--r--digital/asserv/src/asserv/speed.h40
-rw-r--r--digital/asserv/src/asserv/state.h110
-rw-r--r--digital/asserv/src/asserv/timer.avr.c29
-rw-r--r--digital/asserv/src/asserv/timer.h41
-rw-r--r--digital/asserv/src/asserv/traj.c19
-rw-r--r--digital/asserv/src/asserv/traj.h33
-rw-r--r--digital/asserv/src/asserv/twi_proto.c1
22 files changed, 547 insertions, 195 deletions
diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile
index 3ddd904c..9e3c10ac 100644
--- a/digital/asserv/src/asserv/Makefile
+++ b/digital/asserv/src/asserv/Makefile
@@ -1,7 +1,8 @@
BASE = ../../../avr
PROGS = asserv
HOST_PROGS = test_motor_model
-asserv_SOURCES = main.c twi_proto.c counter_ext.avr.c \
+asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pos.c speed.c \
+ postrack.c traj.c twi_proto.c eeprom.avr.c \
simu.host.c motor_model.host.c models.host.c
test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c
MODULES = proto uart utils math/fixed twi
diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c
index ff962496..89209a96 100644
--- a/digital/asserv/src/asserv/counter_ext.avr.c
+++ b/digital/asserv/src/asserv/counter_ext.avr.c
@@ -24,6 +24,8 @@
* }}} */
#include "common.h"
#include "counter.h"
+
+#include "modules/utils/utils.h"
#include "io.h"
/**
diff --git a/digital/asserv/src/asserv/counter_tcc.avr.c b/digital/asserv/src/asserv/counter_tcc.avr.c
index 9b64d916..030ebcc7 100644
--- a/digital/asserv/src/asserv/counter_tcc.avr.c
+++ b/digital/asserv/src/asserv/counter_tcc.avr.c
@@ -24,6 +24,7 @@
* }}} */
#include "common.h"
#include "counter.h"
+
#include "modules/utils/utils.h"
#include "io.h"
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 26e5c4b6..53d0af6a 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -1,4 +1,4 @@
-/* eeprom.c */
+/* eeprom.avr.c - Save parameters to internal EEPROM. */
/* asserv - Position & speed motor control on AVR. {{{
*
* Copyright (C) 2005 Nicolas Schodet
@@ -22,17 +22,20 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "eeprom.h"
+
#include <avr/eeprom.h>
-/* Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x45
-#define EEPROM_START 256
+#include "pwm.h"
+#include "pos.h"
+#include "speed.h"
+#include "postrack.h"
-/* +AutoDec */
-/* -AutoDec */
+#define EEPROM_START 0
/* Read parameters from eeprom. */
-static void
+void
eeprom_read_params (void)
{
uint8_t *p8 = (uint8_t *) EEPROM_START;
@@ -60,7 +63,7 @@ eeprom_read_params (void)
}
/* Write parameters to eeprom. */
-static void
+void
eeprom_write_params (void)
{
uint8_t *p8 = (uint8_t *) EEPROM_START;
@@ -87,7 +90,7 @@ eeprom_write_params (void)
}
/* Clear eeprom parameters. */
-static void
+void
eeprom_clear_params (void)
{
uint8_t *p = (uint8_t *) EEPROM_START;
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
new file mode 100644
index 00000000..929d0da6
--- /dev/null
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -0,0 +1,40 @@
+#ifndef eeprom_h
+#define eeprom_h
+/* eeprom.h - Save parameters to internal EEPROM. */
+/* 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.
+ *
+ * }}} */
+
+/* Change the eeprom key each time you change eeprom format. */
+#define EEPROM_KEY 0x45
+
+void
+eeprom_read_params (void);
+
+void
+eeprom_write_params (void);
+
+void
+eeprom_clear_params (void);
+
+#endif /* eeprom_h */
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 02d98701..385ac0e5 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -23,6 +23,7 @@
*
* }}} */
#include "common.h"
+
#include "modules/uart/uart.h"
#include "modules/proto/proto.h"
#include "modules/utils/utils.h"
@@ -31,36 +32,25 @@
#include "io.h"
#include "misc.h"
+#include "state.h"
#include "counter.h"
-#include "twi_proto.h"
+#include "pwm.h"
+#include "pos.h"
+#include "speed.h"
+#include "postrack.h"
+#include "traj.h"
-/** Motor command sequence, do not use values above 127, do not use zero. */
-uint8_t main_sequence, main_sequence_ack, main_sequence_finish;
+#include "twi_proto.h"
+#include "eeprom.h"
-/* This is implementation include. */
#ifndef HOST
-# include "timer.avr.c"
-# include "pwm.avr.c"
+# include "timer.h"
#else
# include "simu.host.h"
#endif
-#include "pos.c"
-#include "speed.c"
-#include "postrack.c"
-#include "traj.c"
-#ifndef HOST
-# include "eeprom.avr.c"
-#endif
-/** Motor control mode:
- * 0: pwm setup.
- * 1: shaft position control.
- * 2: speed control.
- * 3: trajectory control. */
-int8_t main_mode;
-
-/** Report trajectory end. */
+/** Report command completion. */
uint8_t main_sequence_ack_cpt = 2;
/** Report of counters. */
@@ -93,14 +83,9 @@ uint8_t main_simu, main_simu_cpt;
* analisys. */
uint8_t main_timer[6];
-/* +AutoDec */
-
-/** Main loop. */
static void
main_loop (void);
-/* -AutoDec */
-
/** Entry point. */
int
main (int argc, char **argv)
@@ -136,8 +121,8 @@ main_loop (void)
/* Counter update. */
counter_update ();
main_timer[0] = timer_read ();
- /* Postion control. */
- if (main_mode >= 1)
+ /* Position control. */
+ if (state_main.mode >= MODE_POS)
pos_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
@@ -146,16 +131,17 @@ main_loop (void)
/* Compute absolute position. */
postrack_update ();
/* Compute trajectory. */
- if (main_mode >= 3)
+ if (state_main.mode >= MODE_TRAJ)
traj_update ();
/* Speed control. */
- if (main_mode >= 2)
+ if (state_main.mode >= MODE_SPEED)
speed_update ();
main_timer[3] = timer_read ();
/* Stats. */
- if (main_sequence_ack != main_sequence_finish && !--main_sequence_ack_cpt)
+ if (state_main.sequence_ack != state_main.sequence_finish
+ && !--main_sequence_ack_cpt)
{
- proto_send1b ('A', main_sequence_finish);
+ proto_send1b ('A', state_main.sequence_finish);
main_sequence_ack_cpt = 4;
}
if (main_stat_counter && !--main_stat_counter_cpt)
@@ -227,7 +213,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('w', 0):
/* Set zero pwm. */
pos_reset ();
- main_mode = 0;
+ state_main.mode = MODE_PWM;
pwm_left = 0;
pwm_right = 0;
break;
@@ -236,7 +222,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: left pwm.
* - w: right pwm. */
pos_reset ();
- main_mode = 0;
+ state_main.mode = MODE_PWM;
pwm_left = v8_to_v16 (args[0], args[1]);
UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
pwm_right = v8_to_v16 (args[2], args[3]);
@@ -246,13 +232,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Add to position consign.
* - w: theta consign offset.
* - w: alpha consign offset. */
- main_mode = 1;
+ state_main.mode = MODE_POS;
pos_theta_cons += v8_to_v16 (args[0], args[1]);
pos_alpha_cons += v8_to_v16 (args[2], args[3]);
break;
case c ('s', 0):
/* Stop (set zero speed). */
- main_mode = 2;
+ state_main.mode = MODE_SPEED;
speed_pos = 0;
speed_theta_cons = 0;
speed_alpha_cons = 0;
@@ -261,7 +247,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set speed.
* - b: theta speed.
* - b: alpha speed. */
- main_mode = 2;
+ state_main.mode = MODE_SPEED;
speed_pos = 0;
speed_theta_cons = args[0] << 8;
speed_alpha_cons = args[1] << 8;
@@ -271,32 +257,32 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: theta consign offset.
* - d: alpha consign offset.
* - b: sequence number. */
- if (args[8] == main_sequence)
+ if (args[8] == state_main.sequence)
break;
- main_mode = 2;
+ state_main.mode = MODE_SPEED;
speed_pos = 1;
speed_theta_pos_cons = pos_theta_cons;
speed_theta_pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
speed_alpha_pos_cons = pos_alpha_cons;
speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
- main_sequence = args[8];
+ state_start (&state_main, args[8]);
break;
case c ('f', 1):
/* Go to the wall.
* - b: sequence number. */
- if (args[0] == main_sequence)
+ if (args[0] == state_main.sequence)
break;
- main_mode = 3;
+ state_main.mode = MODE_TRAJ;
speed_pos = 0;
traj_mode = 10;
- main_sequence = args[0];
+ state_start (&state_main, args[0]);
break;
case c ('a', 1):
/* Set acknoledge.
* - b: ack sequence number. */
- main_sequence_ack = args[0];
- if (pos_blocked_state)
+ if (state_main.blocked)
pos_reset ();
+ state_acknowledge (&state_main, args[0]);
break;
/* Stats.
* - b: interval between stats. */
@@ -418,9 +404,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* 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);
+ pwm_reverse (args[1], args[2]);
break;
case c ('E', 2):
/* Write to eeprom.
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index eecd71f3..bfb6811e 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -22,6 +22,15 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "pos.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "counter.h"
+#include "pwm.h"
+#include "state.h"
/**
* This file is responsible for position motor control. The consign is a
@@ -54,11 +63,6 @@ int32_t pos_blocked = 15000L;
int32_t pos_theta_int, pos_alpha_int;
/** Last error values. */
int32_t pos_theta_e_old, pos_alpha_e_old;
-/** One if blocked. */
-uint8_t pos_blocked_state;
-
-/* +AutoDec */
-/* -AutoDec */
/** Compute a PID.
* How to compute maximum numbers size:
@@ -95,7 +99,7 @@ pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old,
}
/** Update PWM according to consign. */
-static void
+void
pos_update (void)
{
int16_t pid_theta, pid_alpha;
@@ -106,15 +110,14 @@ pos_update (void)
/* Compute PID. */
diff_theta = pos_theta_cons - pos_theta_cur;
diff_alpha = pos_alpha_cons - pos_alpha_cur;
- if (pos_blocked_state
+ if (state_main.blocked
|| diff_theta < -pos_blocked || pos_blocked < diff_theta
|| diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
{
/* Blocked. */
pwm_left = 0;
pwm_right = 0;
- pos_blocked_state = 1;
- main_sequence_finish = main_sequence | 0x80;
+ state_blocked (&state_main);
}
else
{
@@ -132,13 +135,14 @@ pos_update (void)
}
}
-/** Reset position control internal state. */
-static void
+/** Reset position control internal state. To be called when the position
+ * control is deactivated. */
+void
pos_reset (void)
{
pos_theta_int = pos_alpha_int = 0;
pos_theta_cur = pos_alpha_cur = 0;
pos_theta_cons = pos_alpha_cons = 0;
pos_theta_e_old = pos_alpha_e_old = 0;
- pos_blocked_state = 0;
}
+
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
new file mode 100644
index 00000000..ec6d93c9
--- /dev/null
+++ b/digital/asserv/src/asserv/pos.h
@@ -0,0 +1,47 @@
+#ifndef pos_h
+#define pos_h
+/* pos.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.
+ *
+ * }}} */
+
+extern uint32_t pos_theta_cur, pos_alpha_cur;
+extern uint32_t pos_theta_cons, pos_alpha_cons;
+
+extern int32_t pos_e_sat;
+extern int32_t pos_int_sat;
+extern uint16_t pos_theta_kp, pos_alpha_kp;
+extern uint16_t pos_theta_ki, pos_alpha_ki;
+extern uint16_t pos_theta_kd, pos_alpha_kd;
+extern int32_t pos_blocked;
+
+extern int32_t pos_theta_int, pos_alpha_int;
+extern int32_t pos_theta_e_old, pos_alpha_e_old;
+
+void
+pos_update (void);
+
+void
+pos_reset (void);
+
+#endif /* pos_h */
diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c
index 6da77592..14de6c09 100644
--- a/digital/asserv/src/asserv/postrack.c
+++ b/digital/asserv/src/asserv/postrack.c
@@ -1,4 +1,4 @@
-/* postrack.c */
+/* postrack.c - Compute current position from counters. */
/* asserv - Position & speed motor control on AVR. {{{
*
* Copyright (C) 2006 Nicolas Schodet
@@ -22,6 +22,12 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "postrack.h"
+
+#include "modules/math/fixed/fixed.h"
+
+#include "counter.h"
/** Current position, f24.8. */
int32_t postrack_x, postrack_y;
@@ -39,26 +45,10 @@ uint16_t postrack_footing;
* a angle in f8.24 format.
* - this factor is in f8.24 format, therefore, 1 is writen (1L << 24).
* - there is a two factor because of the sum done in the motor control. */
-uint32_t postrack_footing_factor;
-
-/* +AutoDec */
+static uint32_t postrack_footing_factor;
/** Initialise the position tracker. */
-static inline void
-postrack_init (void);
-
-/** Update the current position. */
-static inline void
-postrack_update (void);
-
-/** Change the footing value. */
-static inline void
-postrack_set_footing (uint16_t footing);
-
-/* -AutoDec */
-
-/** Initialise the position tracker. */
-static inline void
+void
postrack_init (void)
{
/* Prevent division by 0 by providing a default large value. */
@@ -68,7 +58,7 @@ postrack_init (void)
#define M_PI 3.14159265358979323846 /* pi */
/** Update the current position. */
-static inline void
+void
postrack_update (void)
{
int32_t d, dd, da, na, dsc;
@@ -107,10 +97,11 @@ postrack_update (void)
#define M_1_PI 0.31830988618379067154 /* 1/pi */
/** Change the footing value. */
-static inline void
+void
postrack_set_footing (uint16_t footing)
{
postrack_footing = footing;
postrack_footing_factor =
(uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing;
}
+
diff --git a/digital/asserv/src/asserv/postrack.h b/digital/asserv/src/asserv/postrack.h
new file mode 100644
index 00000000..26fc4ee3
--- /dev/null
+++ b/digital/asserv/src/asserv/postrack.h
@@ -0,0 +1,41 @@
+#ifndef postrack_h
+#define postrack_h
+/* postrack.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.
+ *
+ * }}} */
+
+extern int32_t postrack_x, postrack_y;
+extern int32_t postrack_a;
+extern uint16_t postrack_footing;
+
+void
+postrack_init (void);
+
+void
+postrack_update (void);
+
+void
+postrack_set_footing (uint16_t footing);
+
+#endif /* postrack_h */
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index b80483e0..58e18e9d 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -22,6 +22,11 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "io.h"
/** Define the PWM output used for left motor. */
#define PWM_LEFT_OCR OCR1B
@@ -32,9 +37,6 @@
/** Define the direction output for right motor. */
#define PWM_RIGHT_DIR 5
-/** Define the absolute maximum PWM value. */
-#define PWM_MAX 0x3ff
-
/** PWM values, this is an error if absolute value is greater than the
* maximum. */
int16_t pwm_left, pwm_right;
@@ -42,20 +44,8 @@ int16_t pwm_left, pwm_right;
* on port B. */
uint8_t pwm_dir = _BV (PWM_LEFT_DIR);
-/* +AutoDec */
-
/** Initialise PWM generator. */
-static inline void
-pwm_init (void);
-
-/** Update the hardware PWM values. */
-static inline void
-pwm_update (void);
-
-/* -AutoDec */
-
-/** Initialise PWM generator. */
-static inline void
+void
pwm_init (void)
{
/* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic.
@@ -71,7 +61,7 @@ pwm_init (void)
}
/** Update the hardware PWM values. */
-static inline void
+void
pwm_update (void)
{
uint16_t left, right;
@@ -123,3 +113,11 @@ pwm_update (void)
PWM_RIGHT_OCR = right;
}
+void
+pwm_reverse (uint8_t left, uint8_t right)
+{
+ pwm_dir = 0;
+ if (left) pwm_dir |= _BV (PWM_LEFT_DIR);
+ if (right) pwm_dir |= _BV (PWM_RIGHT_DIR);
+}
+
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
new file mode 100644
index 00000000..2480f7eb
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm.h
@@ -0,0 +1,43 @@
+#ifndef pwm_h
+#define pwm_h
+/* pwm.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.
+ *
+ * }}} */
+
+/** Define the absolute maximum PWM value. */
+#define PWM_MAX 0x3ff
+
+extern int16_t pwm_left, pwm_right;
+extern uint8_t pwm_dir;
+
+void
+pwm_init (void);
+
+void
+pwm_update (void);
+
+void
+pwm_reverse (uint8_t left, uint8_t right);
+
+#endif /* pwm_h */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 2041d67a..6e0a1cf8 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -21,16 +21,20 @@
* Web: http://perso.efrei.fr/~schodet/
* }}} */
#include "common.h"
+#include "simu.host.h"
+
#include "modules/host/host.h"
#include "modules/utils/utils.h"
-#include "simu.host.h"
-#include "motor_model.host.h"
-#include "models.host.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
+#include "pwm.h"
+
+#include "motor_model.host.h"
+#include "models.host.h"
+
/** Simulate some AVR regs. */
uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
@@ -177,3 +181,23 @@ pwm_update (void)
{
}
+void
+eeprom_read_params (void)
+{
+}
+
+void
+eeprom_write_params (void)
+{
+}
+
+void
+eeprom_clear_params (void)
+{
+}
+
+void
+pwm_reverse (uint8_t left, uint8_t right)
+{
+}
+
diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h
index 45778bd6..8d92c44b 100644
--- a/digital/asserv/src/asserv/simu.host.h
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -23,68 +23,17 @@
* Web: http://perso.efrei.fr/~schodet/
* }}} */
-/** Simulate some AVR regs. */
extern uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
-/** Overall counter values. */
-extern uint16_t counter_left, counter_right;
-/** Counter differences since last update.
- * Maximum of 9 significant bits, sign included. */
-extern int16_t counter_left_diff, counter_right_diff;
-
-/** Define the direction output for left motor. */
-#define PWM_LEFT_DIR 4
-/** Define the direction output for right motor. */
-#define PWM_RIGHT_DIR 5
-
-/** Define the absolute maximum PWM value. */
-#define PWM_MAX 0x3ff
-
-/** PWM values, this is an error if absolute value is greater than the
- * maximum. */
-extern int16_t pwm_left, pwm_right;
-/** PWM reverse direction, only set pwm dir bits or you will get weird results
- * on port B. */
-extern uint8_t pwm_dir;
-
-/** Computed simulated position. */
extern double simu_pos_x, simu_pos_y, simu_pos_a;
-#define EEPROM_KEY 0xa5
-#define eeprom_read_params() do { } while (0)
-#define eeprom_write_params() do { } while (0)
-#define eeprom_clear_params() do { } while (0)
-
-/* +AutoDec */
-
-/** Initialise the timer. */
void
timer_init (void);
-/** Wait for timer overflow. */
void
timer_wait (void);
-/** Read timer value. Used for performance analysis. */
uint8_t
timer_read (void);
-/** Initialize the counters. */
-void
-counter_init (void);
-
-/** Update overall counter values and compute diffs. */
-void
-counter_update (void);
-
-/** Initialise PWM generator. */
-void
-pwm_init (void);
-
-/** Update the hardware PWM values. */
-void
-pwm_update (void);
-
-/* -AutoDec */
-
#endif /* simu_host_h */
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index 7e8ed558..be673273 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -22,6 +22,14 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "speed.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "pos.h"
+#include "state.h"
/**
* This file is responsible for speed control. It changes the current shafts
@@ -39,15 +47,12 @@ int8_t speed_theta_max, speed_alpha_max;
int8_t speed_theta_slow, speed_alpha_slow;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
-/** Weither to use the consign position (1) or not (0). */
+/** Whether to use the consign position (1) or not (0). */
uint8_t speed_pos;
/** Acceleration, uf8.8. */
int16_t speed_theta_acc, speed_alpha_acc;
-/* +AutoDec */
-/* -AutoDec */
-
/** Update shaft position consign according to a speed consign. */
static void
speed_update_by_speed (void)
@@ -104,11 +109,11 @@ speed_update_by_position (void)
speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur,
speed_alpha_acc, speed_alpha_max);
if (speed_theta_cur == 0 && speed_alpha_cur == 0)
- main_sequence_finish = main_sequence;
+ state_finish (&state_main);
}
/** Update shaft position consign according to consign. */
-static void
+void
speed_update (void)
{
/* Update speed. */
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
new file mode 100644
index 00000000..82899871
--- /dev/null
+++ b/digital/asserv/src/asserv/speed.h
@@ -0,0 +1,40 @@
+#ifndef speed_h
+#define speed_h
+/* speed.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.
+ *
+ * }}} */
+
+extern int16_t speed_theta_cur, speed_alpha_cur;
+extern int16_t speed_theta_cons, speed_alpha_cons;
+extern int8_t speed_theta_max, speed_alpha_max;
+extern int8_t speed_theta_slow, speed_alpha_slow;
+extern uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
+extern uint8_t speed_pos;
+
+extern int16_t speed_theta_acc, speed_alpha_acc;
+
+void
+speed_update (void);
+
+#endif /* speed_h */
diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h
new file mode 100644
index 00000000..20000943
--- /dev/null
+++ b/digital/asserv/src/asserv/state.h
@@ -0,0 +1,110 @@
+#ifndef state_h
+#define state_h
+/* state.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.
+ *
+ * }}} */
+
+/** There is two mechanism to acknowledge long lived command completion.
+ *
+ * The first one is dedicated to unreliable and full duplex channels like the
+ * serial port. It is based on sequence numbers.
+ *
+ * The second one is simpler, but can only be used with reliable and half
+ * duplex channels like the TWI bus. It just use a flag to remember command
+ * completion witch is reseted when a new command arrives. */
+
+/** Control mode. */
+enum state_mode_t
+{
+ /** Simple PWM setup mode. */
+ MODE_PWM,
+ /** Position control mode. */
+ MODE_POS,
+ /** Speed control mode. */
+ MODE_SPEED,
+ /** Trajectory control mode. */
+ MODE_TRAJ,
+};
+
+/** Current motor state. */
+struct state_t
+{
+ /** Current motor control mode. */
+ uint8_t mode;
+ /** Sequence number of the currently processed command, should be between
+ * 1 and 127. When a command is received on the serial port it is ignored
+ * if its sequence number is equal to the current sequence number. In
+ * this case a duplicated message is inferred. */
+ uint8_t sequence;
+ /** Sequence number of the most recently finished command. When a command
+ * is finished, the current sequence number is copied to this variable. */
+ uint8_t sequence_finish;
+ /** Sequence number of the most recently acknowledged command. Until this
+ * is not equal to the last finished command sequence number, a message is
+ * generated on the serial line. */
+ uint8_t sequence_ack;
+ /** Simpler flag based mechanism, indicates if the last received command
+ * is finished. */
+ uint8_t finished;
+ /** Wether the motor is blocked. */
+ uint8_t blocked;
+};
+
+/** Main motors state. */
+struct state_t state_main;
+
+/** Start a new command execution. */
+static inline void
+state_start (struct state_t *motor, uint8_t sequence)
+{
+ motor->sequence = sequence;
+ motor->finished = 0;
+}
+
+/** Signal the current command completion. */
+static inline void
+state_finish (struct state_t *motor)
+{
+ motor->sequence_finish = motor->sequence;
+ motor->finished = 1;
+}
+
+/** Signal the current command completion. */
+static inline void
+state_blocked (struct state_t *motor)
+{
+ motor->sequence_finish = motor->sequence | 0x80;
+ motor->blocked = 1;
+}
+
+/** Acknowledge a command completion. */
+static inline void
+state_acknowledge (struct state_t *motor, uint8_t sequence)
+{
+ motor->sequence_ack = sequence;
+ if (sequence == motor->sequence_finish)
+ motor->blocked = 0;
+}
+
+#endif /* state_h */
diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c
index dcad74d0..d0defdee 100644
--- a/digital/asserv/src/asserv/timer.avr.c
+++ b/digital/asserv/src/asserv/timer.avr.c
@@ -22,25 +22,13 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
-/* +AutoDec */
+#include "modules/utils/utils.h"
+#include "io.h"
/** Initialise the timer. */
-static inline void
-timer_init (void);
-
-/** Wait for timer overflow. */
-static inline void
-timer_wait (void);
-
-/** Read timer value. Used for performance analysis. */
-static inline uint8_t
-timer_read (void);
-
-/* -AutoDec */
-
-/** Initialise the timer. */
-static inline void
+void
timer_init (void)
{
TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
@@ -52,7 +40,7 @@ timer_init (void)
}
/** Wait for timer overflow. */
-static inline void
+void
timer_wait (void)
{
while (!(TIFR & _BV (TOV0)))
@@ -61,10 +49,3 @@ timer_wait (void)
TIFR = _BV (TOV0);
}
-/** Read timer value. Used for performance analysis. */
-static inline uint8_t
-timer_read (void)
-{
- return TCNT0;
-}
-
diff --git a/digital/asserv/src/asserv/timer.h b/digital/asserv/src/asserv/timer.h
new file mode 100644
index 00000000..754a2d46
--- /dev/null
+++ b/digital/asserv/src/asserv/timer.h
@@ -0,0 +1,41 @@
+#ifndef timer_h
+#define timer_h
+/* timer.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.
+ *
+ * }}} */
+
+/** Read timer value. Used for performance analysis. */
+extern inline uint8_t
+timer_read (void)
+{
+ return TCNT0;
+}
+
+void
+timer_init (void);
+
+void
+timer_wait (void);
+
+#endif /* timer_h */
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 926dcc5c..440c7be4 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -22,6 +22,19 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+#include "traj.h"
+
+#include "io.h"
+
+#include "speed.h"
+#include "state.h"
+
+#ifdef HOST
+# include "simu.host.h"
+#endif
+
+/* This file provides high level trajectories commands. */
/** Traj mode:
* 10, 11: go to the wall.
@@ -29,7 +42,7 @@
uint8_t traj_mode;
/** Go to the wall mode. */
-void
+static void
traj_ftw (void)
{
int16_t speed;
@@ -56,13 +69,13 @@ traj_ftw (void)
speed_alpha_cons = 0;
speed_theta_cur = 0;
speed_alpha_cur = 0;
- main_sequence_finish = main_sequence;
+ state_finish (&state_main);
traj_mode = 11;
}
}
/* Compute new speed according the defined trajectory. */
-static void
+void
traj_update (void)
{
switch (traj_mode)
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
new file mode 100644
index 00000000..fc127d60
--- /dev/null
+++ b/digital/asserv/src/asserv/traj.h
@@ -0,0 +1,33 @@
+#ifndef traj_h
+#define traj_h
+/* traj.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.
+ *
+ * }}} */
+
+extern uint8_t traj_mode;
+
+void
+traj_update (void);
+
+#endif /* traj_h */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index bea56790..04ef34ae 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -24,6 +24,7 @@
* }}} */
#include "common.h"
#include "twi_proto.h"
+
#include "modules/utils/utils.h"
#include "modules/utils/byte.h"
#include "modules/twi/twi.h"