summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
authorschodet2006-03-26 14:16:20 +0000
committerschodet2006-03-26 14:16:20 +0000
commit98f731d45cb51040bb6ee364920cb0de34939e0a (patch)
treece769578f3fac0c4925de5d5d99625ce00272fb7 /n
parenta0966ea8b0ed34d2a95a33c311e60a9a331290b8 (diff)
Ajout du déplacement avec consigne donnée en position, controlé en vitesse.
Ajout des stats sur la vitesse. Ajout d'explications sur le PID. Correction de la formule de tension dans la simulation à cause du mode PWM fast.
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/asserv/Makefile2
-rw-r--r--n/asserv/src/asserv/main.c30
-rw-r--r--n/asserv/src/asserv/pos.c18
-rw-r--r--n/asserv/src/asserv/simu.host.c4
-rw-r--r--n/asserv/src/asserv/speed.c35
5 files changed, 80 insertions, 9 deletions
diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile
index 3dbda13..7ed24d9 100644
--- a/n/asserv/src/asserv/Makefile
+++ b/n/asserv/src/asserv/Makefile
@@ -3,7 +3,7 @@ PROGS = asserv
HOST_PROGS = test_motor_model
asserv_SOURCES = main.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
+MODULES = proto uart utils math/fixed
test_motor_model_MODULES =
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 866d784..11ef09a 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -27,6 +27,7 @@
#include "modules/proto/proto.h"
#include "modules/utils/utils.h"
#include "modules/utils/byte.h"
+#include "modules/math/fixed/fixed.h"
#include "io.h"
#include "misc.h"
@@ -55,6 +56,9 @@ int8_t main_mode;
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
+/** Statistics about speed control. */
+uint8_t main_stat_speed, main_stat_speed_cpt;
+
/** Statistics about shaft position control. */
uint8_t main_stat_pos, main_stat_pos_cpt;
@@ -127,6 +131,11 @@ main_loop (void)
proto_send2w ('C', counter_left, counter_right);
main_stat_counter_cpt = main_stat_counter;
}
+ if (main_stat_speed && !--main_stat_speed_cpt)
+ {
+ 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)
{
proto_send4w ('P', pos_theta_e_old, pos_theta_int,
@@ -208,12 +217,33 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_cons = args[0] << 8;
speed_alpha_cons = args[1] << 8;
break;
+ case c ('s', 6):
+ /* Set speed controlled position consign.
+ * - w: theta consign offset.
+ * - w: alpha consign offset.
+ * - b: theta max speed.
+ * - b: alpha max speed. */
+ /* WARNING! This is not safe regarding proto! If the sender miss the
+ * acknoledgement it will send this commande twice! */
+ main_mode = 2;
+ speed_pos = 1;
+ speed_theta_pos_cons = pos_theta_cons;
+ speed_theta_pos_cons += v8_to_v16 (args[0], args[1]);
+ speed_alpha_pos_cons = pos_alpha_cons;
+ speed_alpha_pos_cons += v8_to_v16 (args[2], args[3]);
+ speed_theta_cons = args[4] << 8;
+ speed_alpha_cons = args[5] << 8;
+ break;
/* Stats.
* - b: interval between stats. */
case c ('C', 1):
/* Counter stats. */
main_stat_counter_cpt = main_stat_counter = args[0];
break;
+ case c ('S', 1):
+ /* Motor speed control stats. */
+ main_stat_speed_cpt = main_stat_speed = args[0];
+ break;
case c ('P', 1):
/* Motor position control stats. */
main_stat_pos_cpt = main_stat_pos = args[0];
diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c
index b6bee63..d4204c7 100644
--- a/n/asserv/src/asserv/pos.c
+++ b/n/asserv/src/asserv/pos.c
@@ -55,7 +55,21 @@ int32_t pos_theta_e_old, pos_alpha_e_old;
/* +AutoDec */
/* -AutoDec */
-/** Compute a PID. */
+/** Compute a PID.
+ * How to compute maximum numbers size:
+ * Result is 24 bits (16 bits kept after shift).
+ * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff
+ * max is 12 bits.
+ * If int_sat == 1023, i max is 11 bits.
+ * In the final addition, let's give 23 bits to the p part, and 22 bits to the
+ * i and d part (23b + 22b + 22b => 23b + 23b => 24b).
+ * Therefore, kp can be 23 - 11 = 12 bits (f4.8).
+ * ki can be 22 - 11 = 11 bits (f3.8).
+ * kd can be 22 - 12 = 10 bits (f2.8).
+ * How to increase this number:
+ * - lower the shift.
+ * - 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)
@@ -72,7 +86,7 @@ pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old,
pid = e * kp + *i * ki + diff * kd;
/* Save result. */
*e_old = e;
- return pid >> 12;
+ return pid >> 8;
}
/** Update PWM according to consign. */
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index e7cc4ee..1035af1 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -80,8 +80,8 @@ simu_step (void)
/* Convert pwm value into voltage. */
assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX);
assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX);
- simu_left_model.u = (double) pwm_left / PWM_MAX;
- simu_right_model.u = (double) pwm_right / PWM_MAX;
+ simu_left_model.u = (double) (pwm_left + 1) / (PWM_MAX + 1);
+ simu_right_model.u = (double) (pwm_right + 1) / (PWM_MAX + 1);
/* Make one step. */
old_left_th = simu_left_model.th;
old_right_th = simu_right_model.th;
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index d354307..324e231 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -31,7 +31,7 @@
/** Current speed, f8.8. */
int16_t speed_theta_cur, speed_alpha_cur;
-/** Consign speed, f8.8. */
+/** Consign speed or maximum speed for position consign, f8.8. */
int16_t speed_theta_cons, speed_alpha_cons;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
@@ -63,11 +63,38 @@ speed_update_by_speed (void)
speed_alpha_cur -= speed_alpha_acc;
}
+/** Compute maximum allowed speed according to: distance left, maximum speed
+ * consign, current speed and acceleration. */
+static int16_t
+speed_compute_max_speed (int32_t d, int16_t cons, int16_t cur, int16_t acc)
+{
+ int16_t s;
+ /* Compute maximum speed in order to be able to brake in time.
+ * s = sqrt (2 * a * d) */
+ s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d));
+ /* Apply consign. */
+ s = UTILS_MIN (cons >> 8, s);
+ /* Apply sign. */
+ if (d < 0)
+ s = -s;
+ /* Convert to f8.8 and check acceleration. */
+ s = s << 8;
+ UTILS_BOUND (s, cur - acc, cur + acc);
+ return s;
+}
+
/** Update shaft position consign according to a position consign. */
static void
speed_update_by_position (void)
{
- // TODO
+ speed_theta_cur =
+ speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons,
+ speed_theta_cons, speed_theta_cur,
+ speed_theta_acc);
+ speed_alpha_cur =
+ speed_compute_max_speed (speed_alpha_pos_cons - pos_alpha_cons,
+ speed_alpha_cons, speed_alpha_cur,
+ speed_alpha_acc);
}
/** Update shaft position consign according to consign. */
@@ -80,7 +107,7 @@ speed_update (void)
else
speed_update_by_speed ();
/* Update shaft position. */
- pos_theta_cur += speed_theta_cur >> 8;
- pos_alpha_cur += speed_alpha_cur >> 8;
+ pos_theta_cons += speed_theta_cur >> 8;
+ pos_alpha_cons += speed_alpha_cur >> 8;
}