summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-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;
}