summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-03-14 00:26:08 +0100
committerNicolas Schodet2008-03-14 00:26:08 +0100
commitd11d86aac76679041f836125215016ccd4c10f15 (patch)
treeb0d264df533a5210859d45872bccf28ec369ecf5 /digital/asserv
parenta0c34f97eec1773195e2b9cf400696d2f91c9ebf (diff)
* digital/asserv/src/asserv:
- moved position control parameters to a structure.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c24
-rw-r--r--digital/asserv/src/asserv/main.c49
-rw-r--r--digital/asserv/src/asserv/pos.c60
-rw-r--r--digital/asserv/src/asserv/pos.h26
-rw-r--r--digital/asserv/src/asserv/speed.c8
5 files changed, 81 insertions, 86 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 6b54a02b..f435925e 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -51,12 +51,12 @@ eeprom_read_params (void)
postrack_set_footing (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++);
- pos_alpha_ki = eeprom_read_word (p16++);
- pos_theta_kd = eeprom_read_word (p16++);
- pos_alpha_kd = 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++);
+ pos_alpha.ki = eeprom_read_word (p16++);
+ pos_theta.kd = eeprom_read_word (p16++);
+ pos_alpha.kd = eeprom_read_word (p16++);
pos_e_sat = eeprom_read_word (p16++);
pos_int_sat = eeprom_read_word (p16++);
pos_blocked = eeprom_read_word (p16++);
@@ -78,12 +78,12 @@ eeprom_write_params (void)
eeprom_write_word (p16++, postrack_footing);
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);
- eeprom_write_word (p16++, pos_alpha_ki);
- eeprom_write_word (p16++, pos_theta_kd);
- eeprom_write_word (p16++, pos_alpha_kd);
+ eeprom_write_word (p16++, pos_theta.kp);
+ eeprom_write_word (p16++, pos_alpha.kp);
+ eeprom_write_word (p16++, pos_theta.ki);
+ eeprom_write_word (p16++, pos_alpha.ki);
+ eeprom_write_word (p16++, pos_theta.kd);
+ eeprom_write_word (p16++, pos_alpha.kd);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_int_sat);
eeprom_write_word (p16++, pos_blocked);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index b4872ac5..d2340b10 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -163,8 +163,8 @@ main_loop (void)
}
if (main_stat_pos && !--main_stat_pos_cpt)
{
- proto_send4w ('P', pos_theta_e_old, pos_theta_int,
- pos_alpha_e_old, pos_alpha_int);
+ proto_send4w ('P', pos_theta.e_old, pos_theta.i,
+ pos_alpha.e_old, pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
#ifdef HOST
@@ -214,7 +214,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Commands. */
case c ('w', 0):
/* Set zero pwm. */
- pos_reset ();
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
state_main.mode = MODE_PWM;
pwm_left = 0;
pwm_right = 0;
@@ -223,7 +224,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set pwm.
* - w: left pwm.
* - w: right pwm. */
- pos_reset ();
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
state_main.mode = MODE_PWM;
pwm_left = v8_to_v16 (args[0], args[1]);
UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
@@ -233,7 +235,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('w', 2):
/* Set auxiliary pwm.
* - w: pwm. */
- pos_reset (); // TODO
+ //pos_reset (); // TODO
state_aux0.mode = MODE_PWM;
pwm_aux0 = v8_to_v16 (args[0], args[1]);
UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX);
@@ -243,8 +245,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: theta consign offset.
* - w: alpha consign offset. */
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]);
+ 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). */
@@ -271,9 +273,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
state_main.mode = MODE_SPEED;
speed_pos = 1;
- speed_theta_pos_cons = pos_theta_cons;
+ 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 = 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;
@@ -291,7 +293,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set acknoledge.
* - b: ack sequence number. */
if (state_main.blocked)
- pos_reset ();
+ {
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ }
state_acknowledge (&state_main, args[0]);
break;
/* Stats.
@@ -385,25 +390,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_alpha_slow = args[4];
break;
case c ('p', 3):
- pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]);
+ pos_theta.kp = pos_alpha.kp = v8_to_v16 (args[1], args[2]);
break;
case c ('p', 5):
- pos_theta_kp = v8_to_v16 (args[1], args[2]);
- pos_alpha_kp = v8_to_v16 (args[3], args[4]);
+ pos_theta.kp = v8_to_v16 (args[1], args[2]);
+ pos_alpha.kp = v8_to_v16 (args[3], args[4]);
break;
case c ('i', 3):
- pos_theta_ki = pos_alpha_ki = v8_to_v16 (args[1], args[2]);
+ pos_theta.ki = pos_alpha.ki = v8_to_v16 (args[1], args[2]);
break;
case c ('i', 5):
- pos_theta_ki = v8_to_v16 (args[1], args[2]);
- pos_alpha_ki = v8_to_v16 (args[3], args[4]);
+ pos_theta.ki = v8_to_v16 (args[1], args[2]);
+ pos_alpha.ki = v8_to_v16 (args[3], args[4]);
break;
case c ('d', 3):
- pos_theta_kd = pos_alpha_kd = v8_to_v16 (args[1], args[2]);
+ pos_theta.kd = pos_alpha.kd = v8_to_v16 (args[1], args[2]);
break;
case c ('d', 5):
- pos_theta_kd = v8_to_v16 (args[1], args[2]);
- pos_alpha_kd = v8_to_v16 (args[3], args[4]);
+ pos_theta.kd = v8_to_v16 (args[1], args[2]);
+ pos_alpha.kd = v8_to_v16 (args[3], args[4]);
break;
case c ('E', 3):
pos_e_sat = v8_to_v16 (args[1], args[2]);
@@ -434,9 +439,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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);
+ 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);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_int_sat);
proto_send1w ('b', pos_blocked);
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index 0a63ec4f..e241b0fb 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -40,30 +40,17 @@
* 16 bits are enough as long as there is no long blocking (see 2005 cup!).
*/
-/** Current theta/alpha. */
-uint32_t pos_theta_cur, pos_alpha_cur;
-/** Consign theta/alpha. */
-uint32_t pos_theta_cons, pos_alpha_cons;
+/** Theta/alpha control states. */
+struct pos_t pos_theta, pos_alpha;
/** Error saturation. */
int32_t pos_e_sat = 1023;
/** Integral saturation. */
int32_t pos_int_sat = 1023;
-/** P coefficients. */
-uint16_t pos_theta_kp, pos_alpha_kp;
-/** I coefficients. */
-uint16_t pos_theta_ki, pos_alpha_ki;
-/** D coefficients. */
-uint16_t pos_theta_kd, pos_alpha_kd;
/** Blocked value. If error is greater than this value, stop the robot and
* report blocked state. */
int32_t pos_blocked = 15000L;
-/** Current integral values. */
-int32_t pos_theta_int, pos_alpha_int;
-/** Last error values. */
-int32_t pos_theta_e_old, pos_alpha_e_old;
-
/** Compute a PID.
* How to compute maximum numbers size:
* Result is 24 bits (16 bits kept after shift).
@@ -80,21 +67,20 @@ int32_t pos_theta_e_old, pos_alpha_e_old;
* - bound the value returned.
* - lower e & int saturation. */
static inline int16_t
-pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old,
- uint16_t kp, uint16_t ki, uint16_t kd)
+pos_compute_pid (int32_t e, struct pos_t *pos)
{
int32_t diff, pid;
/* Saturate error. */
UTILS_BOUND (e, -pos_e_sat, pos_e_sat);
/* Integral update. */
- *i += e;
- UTILS_BOUND (*i, -pos_int_sat, pos_int_sat);
+ pos->i += e;
+ UTILS_BOUND (pos->i, -pos_int_sat, pos_int_sat);
/* Differential value. */
- diff = e - *e_old;
+ diff = e - pos->e_old;
/* Compute PID. */
- pid = e * kp + *i * ki + diff * kd;
+ pid = e * pos->kp + pos->i * pos->ki + diff * pos->kd;
/* Save result. */
- *e_old = e;
+ pos->e_old = e;
return pid >> 8;
}
@@ -105,11 +91,11 @@ pos_update (void)
int16_t pid_theta, pid_alpha;
int32_t diff_theta, diff_alpha;
/* Update current shaft positions. */
- pos_theta_cur += counter_left_diff + counter_right_diff;
- pos_alpha_cur += counter_right_diff - counter_left_diff;
+ pos_theta.cur += counter_left_diff + counter_right_diff;
+ pos_alpha.cur += counter_right_diff - counter_left_diff;
/* Compute PID. */
- diff_theta = pos_theta_cons - pos_theta_cur;
- diff_alpha = pos_alpha_cons - pos_alpha_cur;
+ diff_theta = pos_theta.cons - pos_theta.cur;
+ diff_alpha = pos_alpha.cons - pos_alpha.cur;
if (state_main.blocked
|| diff_theta < -pos_blocked || pos_blocked < diff_theta
|| diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
@@ -121,12 +107,8 @@ pos_update (void)
}
else
{
- pid_theta = pos_compute_pid (diff_theta, &pos_theta_int,
- &pos_theta_e_old, pos_theta_kp,
- pos_theta_ki, pos_theta_kd);
- pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha_int,
- &pos_alpha_e_old, pos_alpha_kp,
- pos_alpha_ki, pos_alpha_kd);
+ pid_theta = pos_compute_pid (diff_theta, &pos_theta);
+ pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha);
/* Update PWM. */
pwm_left = pid_theta - pid_alpha;
UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
@@ -135,14 +117,14 @@ pos_update (void)
}
}
-/** Reset position control internal state. To be called when the position
- * control is deactivated. */
+/** Reset position control state. To be called when the position control is
+ * deactivated. */
void
-pos_reset (void)
+pos_reset (struct pos_t *pos)
{
- 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->i = 0;
+ pos->cur = 0;
+ pos->cons = 0;
+ pos->e_old = 0;
}
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
index ec6d93c9..1859d178 100644
--- a/digital/asserv/src/asserv/pos.h
+++ b/digital/asserv/src/asserv/pos.h
@@ -25,23 +25,31 @@
*
* }}} */
-extern uint32_t pos_theta_cur, pos_alpha_cur;
-extern uint32_t pos_theta_cons, pos_alpha_cons;
+/** Position control state. */
+struct pos_t
+{
+ /** Current position. */
+ uint32_t cur;
+ /** Consign position. */
+ uint32_t cons;
+ /** PID coefficients (f8.8, maximum depends on saturation values). */
+ uint16_t kp, ki, kd;
+ /** Current integral value. */
+ int32_t i;
+ /** Last error value. */
+ int32_t e_old;
+};
+
+extern struct pos_t pos_theta, pos_alpha;
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);
+pos_reset (struct pos_t *pos);
#endif /* pos_h */
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index 887fab11..c2106909 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -96,8 +96,8 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
static void
speed_update_by_position (void)
{
- int32_t theta_d = speed_theta_pos_cons - pos_theta_cons;
- int32_t alpha_d = speed_alpha_pos_cons - pos_alpha_cons;
+ 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;
else
@@ -122,7 +122,7 @@ speed_update (void)
else
speed_update_by_speed ();
/* Update shaft position. */
- pos_theta_cons += speed_theta_cur >> 8;
- pos_alpha_cons += speed_alpha_cur >> 8;
+ pos_theta.cons += speed_theta_cur >> 8;
+ pos_alpha.cons += speed_alpha_cur >> 8;
}