summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2008-03-14 00:26:41 +0100
committerNicolas Schodet2008-03-14 00:26:41 +0100
commitfc9956a4b1858d490a195eae4574bfa4d18b066f (patch)
treeadeddb77a5f2fabdc22e4fc81c880b80e1b0090a /digital
parentd11d86aac76679041f836125215016ccd4c10f15 (diff)
* digital/asserv/src/asserv:
- moved speed control state to structures.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c24
-rw-r--r--digital/asserv/src/asserv/main.c44
-rw-r--r--digital/asserv/src/asserv/speed.c81
-rw-r--r--digital/asserv/src/asserv/speed.h26
-rw-r--r--digital/asserv/src/asserv/traj.c22
5 files changed, 98 insertions, 99 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index f435925e..dbbdbdec 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -42,15 +42,15 @@ eeprom_read_params (void)
uint16_t *p16;
if (eeprom_read_byte (p8++) != EEPROM_KEY)
return;
- speed_theta_max = eeprom_read_byte (p8++);
- speed_alpha_max = eeprom_read_byte (p8++);
- speed_theta_slow = eeprom_read_byte (p8++);
- speed_alpha_slow = eeprom_read_byte (p8++);
+ speed_theta.max = eeprom_read_byte (p8++);
+ speed_alpha.max = eeprom_read_byte (p8++);
+ speed_theta.slow = eeprom_read_byte (p8++);
+ speed_alpha.slow = eeprom_read_byte (p8++);
pwm_set_reverse (eeprom_read_byte (p8++));
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
- speed_theta_acc = eeprom_read_word (p16++);
- speed_alpha_acc = eeprom_read_word (p16++);
+ speed_theta.acc = eeprom_read_word (p16++);
+ speed_alpha.acc = eeprom_read_word (p16++);
pos_theta.kp = eeprom_read_word (p16++);
pos_alpha.kp = eeprom_read_word (p16++);
pos_theta.ki = eeprom_read_word (p16++);
@@ -69,15 +69,15 @@ eeprom_write_params (void)
uint8_t *p8 = (uint8_t *) EEPROM_START;
uint16_t *p16;
eeprom_write_byte (p8++, EEPROM_KEY);
- eeprom_write_byte (p8++, speed_theta_max);
- eeprom_write_byte (p8++, speed_alpha_max);
- eeprom_write_byte (p8++, speed_theta_slow);
- eeprom_write_byte (p8++, speed_alpha_slow);
+ eeprom_write_byte (p8++, speed_theta.max);
+ eeprom_write_byte (p8++, speed_alpha.max);
+ eeprom_write_byte (p8++, speed_theta.slow);
+ eeprom_write_byte (p8++, speed_alpha.slow);
eeprom_write_byte (p8++, pwm_reverse);
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
- eeprom_write_word (p16++, speed_theta_acc);
- eeprom_write_word (p16++, speed_alpha_acc);
+ eeprom_write_word (p16++, speed_theta.acc);
+ eeprom_write_word (p16++, speed_alpha.acc);
eeprom_write_word (p16++, pos_theta.kp);
eeprom_write_word (p16++, pos_alpha.kp);
eeprom_write_word (p16++, pos_theta.ki);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index d2340b10..6d761315 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -158,7 +158,7 @@ main_loop (void)
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8);
+ proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
@@ -251,18 +251,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('s', 0):
/* Stop (set zero speed). */
state_main.mode = MODE_SPEED;
- speed_pos = 0;
- speed_theta_cons = 0;
- speed_alpha_cons = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
break;
case c ('s', 2):
/* Set speed.
* - b: theta speed.
* - b: alpha speed. */
state_main.mode = MODE_SPEED;
- speed_pos = 0;
- speed_theta_cons = args[0] << 8;
- speed_alpha_cons = args[1] << 8;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = args[0] << 8;
+ speed_alpha.cons = args[1] << 8;
break;
case c ('s', 9):
/* Set speed controlled position consign.
@@ -272,11 +272,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (args[8] == state_main.sequence)
break;
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]);
+ speed_theta.use_pos = speed_alpha.use_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]);
state_start (&state_main, args[8]);
break;
case c ('f', 1):
@@ -285,7 +285,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (args[0] == state_main.sequence)
break;
state_main.mode = MODE_TRAJ;
- speed_pos = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
traj_mode = 10;
state_start (&state_main, args[0]);
break;
@@ -375,8 +375,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set acceleration.
* - w: theta.
* - w: alpha. */
- speed_theta_acc = v8_to_v16 (args[1], args[2]);
- speed_alpha_acc = v8_to_v16 (args[3], args[4]);
+ speed_theta.acc = v8_to_v16 (args[1], args[2]);
+ speed_alpha.acc = v8_to_v16 (args[3], args[4]);
break;
case c ('s', 5):
/* Set maximum and slow speed.
@@ -384,10 +384,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: alpha max.
* - b: theta slow.
* - b: alpha slow. */
- speed_theta_max = args[1];
- speed_alpha_max = args[2];
- speed_theta_slow = args[3];
- speed_alpha_slow = args[4];
+ speed_theta.max = args[1];
+ speed_alpha.max = args[2];
+ speed_theta.slow = args[3];
+ speed_alpha.slow = args[4];
break;
case c ('p', 3):
pos_theta.kp = pos_alpha.kp = v8_to_v16 (args[1], args[2]);
@@ -436,9 +436,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
proto_send1w ('f', postrack_footing);
- proto_send2w ('a', speed_theta_acc, speed_alpha_acc);
- proto_send4b ('s', speed_theta_max, speed_alpha_max,
- speed_theta_slow, speed_alpha_slow);
+ proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
+ proto_send4b ('s', speed_theta.max, speed_alpha.max,
+ speed_theta.slow, speed_alpha.slow);
proto_send2w ('p', pos_theta.kp, pos_alpha.kp);
proto_send2w ('i', pos_theta.ki, pos_alpha.ki);
proto_send2w ('d', pos_theta.kd, pos_alpha.kd);
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index c2106909..b50a6b52 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -37,39 +37,20 @@
* shaft position.
*/
-/** Current speed, f8.8. */
-int16_t speed_theta_cur, speed_alpha_cur;
-/** Consign speed, f8.8. */
-int16_t speed_theta_cons, speed_alpha_cons;
-/** Maximum speed for position consign, u8. */
-int8_t speed_theta_max, speed_alpha_max;
-/** Slow speed for position consign, u8. */
-int8_t speed_theta_slow, speed_alpha_slow;
-/** Consign position. */
-uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
-/** 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;
+/** Theta/alpha speed control states. */
+struct speed_t speed_theta, speed_alpha;
/** Update shaft position consign according to a speed consign. */
static void
-speed_update_by_speed (void)
+speed_update_by_speed (struct speed_t *speed)
{
/* Update current speed. */
- if (UTILS_ABS (speed_theta_cons - speed_theta_cur) < speed_theta_acc)
- speed_theta_cur = speed_theta_cons;
- else if (speed_theta_cons > speed_theta_cur)
- speed_theta_cur += speed_theta_acc;
- else
- speed_theta_cur -= speed_theta_acc;
- if (UTILS_ABS (speed_alpha_cons - speed_alpha_cur) < speed_alpha_acc)
- speed_alpha_cur = speed_alpha_cons;
- else if (speed_alpha_cons > speed_alpha_cur)
- speed_alpha_cur += speed_alpha_acc;
+ if (UTILS_ABS (speed->cons - speed->cur) < speed->acc)
+ speed->cur = speed->cons;
+ else if (speed->cons > speed->cur)
+ speed->cur += speed->acc;
else
- speed_alpha_cur -= speed_alpha_acc;
+ speed->cur -= speed->acc;
}
/** Compute maximum allowed speed according to: distance left, maximum speed,
@@ -94,22 +75,26 @@ 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 (void)
+speed_update_by_position (struct speed_t *speed, struct pos_t *pos)
{
- int32_t theta_d = speed_theta_pos_cons - pos_theta.cons;
- int32_t alpha_d = speed_alpha_pos_cons - pos_alpha.cons;
- if (theta_d >= -speed_theta_max && theta_d <= speed_theta_max)
- speed_theta_cur = theta_d << 8;
+ int32_t diff = speed->pos_cons - pos->cons;
+ if (diff >= -speed->max && diff <= speed->max)
+ speed->cur = diff << 8;
else
- speed_theta_cur = speed_compute_max_speed (theta_d, speed_theta_cur,
- speed_theta_acc, speed_theta_max);
- if (alpha_d >= -speed_alpha_max && alpha_d <= speed_alpha_max)
- speed_alpha_cur = alpha_d << 8;
+ 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)
+{
+ if (speed->use_pos)
+ speed_update_by_position (speed, pos);
else
- 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)
- state_finish (&state_main);
+ speed_update_by_speed (speed);
+ /* Update shaft position. */
+ pos->cons += speed->cur >> 8;
}
/** Update shaft position consign according to consign. */
@@ -117,12 +102,14 @@ void
speed_update (void)
{
/* Update speed. */
- if (speed_pos)
- speed_update_by_position ();
- else
- speed_update_by_speed ();
- /* Update shaft position. */
- pos_theta.cons += speed_theta_cur >> 8;
- pos_alpha.cons += speed_alpha_cur >> 8;
+ speed_update_by (&speed_theta, &pos_theta);
+ speed_update_by (&speed_alpha, &pos_alpha);
+ /* Check for completion. */
+ if ((speed_theta.use_pos || speed_alpha.use_pos)
+ && (!speed_theta.use_pos || speed_theta.cur == 0)
+ && (!speed_alpha.use_pos || speed_alpha.cur == 0))
+ {
+ state_finish (&state_main);
+ }
}
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
index 82899871..f62f267f 100644
--- a/digital/asserv/src/asserv/speed.h
+++ b/digital/asserv/src/asserv/speed.h
@@ -25,14 +25,26 @@
*
* }}} */
-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;
+/** Speed control state. */
+struct speed_t
+{
+ /** Current speed, f8.8. */
+ int16_t cur;
+ /** Consign speed, f8.8. */
+ int16_t cons;
+ /** Maximum speed for position consign, u8. */
+ int8_t max;
+ /** Slow speed for position consign, u8. */
+ int8_t slow;
+ /** Acceleration, f8.8. */
+ int16_t acc;
+ /** Consign position. */
+ uint32_t pos_cons;
+ /** Whether to use the consign position (1) or not (0). */
+ uint8_t use_pos;
+};
-extern int16_t speed_theta_acc, speed_alpha_acc;
+extern struct speed_t speed_theta, speed_alpha;
void
speed_update (void);
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index fc41d3a5..226430a2 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -46,29 +46,29 @@ static void
traj_ftw (void)
{
int16_t speed;
- speed = speed_theta_slow;
+ speed = speed_theta.slow;
speed *= 256;
if (PINC & _BV (0) && PINC & _BV (1))
{
- speed_theta_cons = -speed;
- speed_alpha_cons = 0;
+ speed_theta.cons = -speed;
+ speed_alpha.cons = 0;
}
else if (PINC & _BV (0))
{
- speed_theta_cons = -speed / 2;
- speed_alpha_cons = speed / 2;
+ speed_theta.cons = -speed / 2;
+ speed_alpha.cons = speed / 2;
}
else if (PINC & _BV (1))
{
- speed_theta_cons = -speed / 2;
- speed_alpha_cons = -speed / 2;
+ speed_theta.cons = -speed / 2;
+ speed_alpha.cons = -speed / 2;
}
else
{
- speed_theta_cons = 0;
- speed_alpha_cons = 0;
- speed_theta_cur = 0;
- speed_alpha_cur = 0;
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
+ speed_theta.cur = 0;
+ speed_alpha.cur = 0;
state_finish (&state_main);
traj_mode = 11;
}