summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2009-03-30 00:19:56 +0200
committerNicolas Schodet2009-03-30 00:19:56 +0200
commitc0aba9481e57896be0a0f7b480f47eb80432f60c (patch)
tree8bb2d87f64c33dc6c757ba7e35d1947795821a3c /digital
parenta1ee3e571e1235416eac8a7a4038cf195ad7d89a (diff)
* digital/asserv/src/asserv:
- use pointers for speed updates.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/main.c1
-rw-r--r--digital/asserv/src/asserv/speed.c66
-rw-r--r--digital/asserv/src/asserv/speed.h5
3 files changed, 51 insertions, 21 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 0a29d4ff..db3064da 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -108,6 +108,7 @@ main (int argc, char **argv)
uart0_init ();
twi_proto_init ();
postrack_init ();
+ speed_init ();
traj_init ();
eeprom_read_params ();
proto_send0 ('z');
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index 6f180a05..8625c6df 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -43,6 +43,15 @@ struct speed_t speed_theta, speed_alpha;
/** Auxiliaries speed control states. */
struct speed_t speed_aux0;
+/** Initialise speed control states. */
+void
+speed_init (void)
+{
+ speed_theta.pos = &pos_theta;
+ speed_alpha.pos = &pos_alpha;
+ speed_aux0.pos = &pos_aux0;
+}
+
/** Update shaft position consign according to a speed consign. */
static void
speed_update_by_speed (struct speed_t *speed)
@@ -79,51 +88,66 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
/** Update shaft position consign according to a position consign. */
static void
-speed_update_by_position (struct speed_t *speed, struct pos_t *pos)
+speed_update_by_position (struct speed_t *speed)
{
- int32_t diff = speed->pos_cons - pos->cons;
+ int32_t diff = speed->pos_cons - speed->pos->cons;
speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc,
speed->max);
}
/** Update shaft position consign according to its consign type. */
static void
-speed_update_by (struct speed_t *speed, struct pos_t *pos)
+speed_update_by (struct speed_t *speed)
{
if (speed->use_pos)
- speed_update_by_position (speed, pos);
+ speed_update_by_position (speed);
else
speed_update_by_speed (speed);
/* Update shaft position. */
- pos->cons += speed->cur >> 8;
+ speed->pos->cons += speed->cur >> 8;
}
-/** Update shaft position consign according to consign. */
-void
-speed_update (void)
+/** Update shaft position consign for two motors system. */
+static void
+speed_update_double (struct state_t *state, struct speed_t *speed0,
+ struct speed_t *speed1)
{
- if (state_main.mode >= MODE_SPEED)
+ if (state->mode >= MODE_SPEED)
{
/* Update speed. */
- speed_update_by (&speed_theta, &pos_theta);
- speed_update_by (&speed_alpha, &pos_alpha);
+ speed_update_by (speed0);
+ speed_update_by (speed1);
/* Check for completion. */
- if (state_main.mode == MODE_SPEED
- && (speed_theta.use_pos || speed_alpha.use_pos)
- && (!speed_theta.use_pos || speed_theta.cur == 0)
- && (!speed_alpha.use_pos || speed_alpha.cur == 0))
+ if (state->mode == MODE_SPEED
+ && (speed0->use_pos || speed1->use_pos)
+ && (!speed0->use_pos || speed0->cur == 0)
+ && (!speed1->use_pos || speed1->cur == 0))
{
- state_finish (&state_main);
+ state_finish (state);
}
}
- if (state_aux0.mode >= MODE_SPEED)
+}
+
+/** Update shaft position consign for one motor system. */
+static void
+speed_update_single (struct state_t *state, struct speed_t *speed)
+{
+ if (state->mode >= MODE_SPEED)
{
/* Update speed. */
- speed_update_by (&speed_aux0, &pos_aux0);
+ speed_update_by (speed);
/* Check for completion. */
- if (state_aux0.mode == MODE_SPEED
- && speed_aux0.use_pos && speed_aux0.cur == 0)
- state_finish (&state_aux0);
+ if (state->mode == MODE_SPEED
+ && speed->use_pos && speed->cur == 0)
+ state_finish (state);
}
}
+/** Update shaft position consign according to consign. */
+void
+speed_update (void)
+{
+ speed_update_double (&state_main, &speed_theta, &speed_alpha);
+ speed_update_single (&state_aux0, &speed_aux0);
+}
+
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
index d5df91a2..3ed02393 100644
--- a/digital/asserv/src/asserv/speed.h
+++ b/digital/asserv/src/asserv/speed.h
@@ -28,6 +28,8 @@
/** Speed control state. */
struct speed_t
{
+ /** Controlled position. */
+ struct pos_t *pos;
/** Current speed, f8.8. */
int16_t cur;
/** Consign speed, f8.8. */
@@ -48,6 +50,9 @@ extern struct speed_t speed_theta, speed_alpha;
extern struct speed_t speed_aux0;
void
+speed_init (void);
+
+void
speed_update (void);
#endif /* speed_h */