summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-05-03 13:33:08 +0200
committerNicolas Schodet2008-05-03 13:33:08 +0200
commit339aef5ad2cbc67e7bd6e8b968fe9e373c4654e8 (patch)
tree30295c371c09de136a2380fbd7685723adfbc530 /digital/asserv
parent578dc890532b15754e1760db750e480bcc99dc52 (diff)
* digital/asserv/src/asserv:
- added better blocking detection.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c8
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c10
-rw-r--r--digital/asserv/src/asserv/pos.c32
-rw-r--r--digital/asserv/src/asserv/pos.h7
5 files changed, 46 insertions, 13 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 0c91fbc0..0cb04aae 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -96,7 +96,9 @@ eeprom_read_params (void)
pos_e_sat = eeprom_read_word (p16++);
pos_i_sat = eeprom_read_word (p16++);
pos_d_sat = eeprom_read_word (p16++);
- pos_blocked = eeprom_read_word (p16++);
+ pos_blocked_error_limit = eeprom_read_word (p16++);
+ pos_blocked_speed_limit = eeprom_read_word (p16++);
+ pos_blocked_counter_limit = eeprom_read_word (p16++);
traj_eps = eeprom_read_word (p16++);
traj_aeps = eeprom_read_word (p16++);
traj_set_angle_limit (eeprom_read_word (p16++));
@@ -134,7 +136,9 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_i_sat);
eeprom_write_word (p16++, pos_d_sat);
- eeprom_write_word (p16++, pos_blocked);
+ eeprom_write_word (p16++, pos_blocked_error_limit);
+ eeprom_write_word (p16++, pos_blocked_speed_limit);
+ eeprom_write_word (p16++, pos_blocked_counter_limit);
eeprom_write_word (p16++, traj_eps);
eeprom_write_word (p16++, traj_aeps);
eeprom_write_word (p16++, traj_angle_limit);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index c9c0a158..30d59780 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x4b
+#define EEPROM_KEY 0x4c
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 2ea3d95d..228426d6 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -577,8 +577,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('D', 3):
pos_d_sat = v8_to_v16 (args[1], args[2]);
break;
- case c ('b', 3):
- pos_blocked = v8_to_v16 (args[1], args[2]);
+ case c ('b', 7):
+ pos_blocked_error_limit = v8_to_v16 (args[1], args[2]);
+ pos_blocked_speed_limit = v8_to_v16 (args[3], args[4]);
+ pos_blocked_counter_limit = v8_to_v16 (args[5], args[6]);
break;
case c ('e', 5):
traj_eps = v8_to_v16 (args[1], args[2]);
@@ -619,7 +621,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
- proto_send1w ('b', pos_blocked);
+ proto_send3w ('b', pos_blocked_error_limit,
+ pos_blocked_speed_limit,
+ pos_blocked_counter_limit);
proto_send2w ('e', traj_eps, traj_aeps);
proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', pwm_reverse);
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index 9109e165..af3d0315 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -52,9 +52,12 @@ int32_t pos_e_sat = 1023;
int32_t pos_i_sat = 1023;
/** Differential saturation. */
int32_t pos_d_sat = 1023;
-/** Blocked value. If error is greater than this value, stop the robot and
- * report blocked state. */
-int32_t pos_blocked = 15000L;
+/** Blocking detection: error limit. */
+int32_t pos_blocked_error_limit = 2048;
+/** Blocking detection: speed limit. */
+int32_t pos_blocked_speed_limit = 0x10;
+/** Blocking detection: counter limit. */
+int32_t pos_blocked_counter_limit = 20;
/** Compute a PID.
* How to compute maximum numbers size:
@@ -108,8 +111,23 @@ pos_update (void)
/* Compute PID. */
diff_theta = pos_theta.cons - pos_theta.cur;
diff_alpha = pos_alpha.cons - pos_alpha.cur;
- if (diff_theta < -pos_blocked || pos_blocked < diff_theta
- || diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
+ /* Compute actual speed and test for blocking. */
+ int32_t cur_speed_theta = pos_theta.cur - pos_theta.cur_old;
+ pos_theta.cur_old = pos_theta.cur;
+ int32_t cur_speed_alpha = pos_alpha.cur - pos_alpha.cur_old;
+ pos_alpha.cur_old = pos_alpha.cur;
+ if ((UTILS_ABS (diff_theta) > pos_blocked_error_limit
+ && UTILS_ABS (cur_speed_theta) < pos_blocked_speed_limit))
+ pos_theta.blocked_counter++;
+ else
+ pos_theta.blocked_counter = 0;
+ if ((UTILS_ABS (diff_alpha) > pos_blocked_error_limit
+ && UTILS_ABS (cur_speed_alpha) < pos_blocked_speed_limit))
+ pos_alpha.blocked_counter++;
+ else
+ pos_alpha.blocked_counter = 0;
+ if (pos_theta.blocked_counter > pos_blocked_counter_limit
+ || pos_alpha.blocked_counter > pos_blocked_counter_limit)
{
/* Blocked. */
pos_reset (&pos_theta);
@@ -135,7 +153,7 @@ pos_update (void)
pos_aux0.cur += counter_aux0_diff;
/* Compute PID. */
diff = pos_aux0.cons - pos_aux0.cur;
- if (diff < -pos_blocked || pos_blocked < diff)
+ if (UTILS_ABS (diff) > 5000)
{
/* Blocked. */
pos_reset (&pos_aux0);
@@ -160,5 +178,7 @@ pos_reset (struct pos_t *pos)
pos->cur = 0;
pos->cons = 0;
pos->e_old = 0;
+ pos->cur_old = 0;
+ pos->blocked_counter = 0;
}
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
index 4f29965c..cf02cda3 100644
--- a/digital/asserv/src/asserv/pos.h
+++ b/digital/asserv/src/asserv/pos.h
@@ -38,13 +38,18 @@ struct pos_t
int32_t i;
/** Last error value. */
int32_t e_old;
+ /** Old current position. */
+ uint32_t cur_old;
+ /** Count the number of blocked detection. */
+ uint8_t blocked_counter;
};
extern struct pos_t pos_theta, pos_alpha;
extern struct pos_t pos_aux0;
extern int32_t pos_e_sat, pos_i_sat, pos_d_sat;
-extern int32_t pos_blocked;
+extern int32_t pos_blocked_error_limit, pos_blocked_speed_limit,
+ pos_blocked_counter_limit;
void
pos_update (void);