From cf8fbdbfb994576b6f3278602c25e84795deac11 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 Apr 2012 20:18:16 +0200 Subject: digital: use 16bit speed --- .../modules/motor/speed_control/speed_control.c | 50 +++++++++++----------- .../modules/motor/speed_control/speed_control.h | 20 ++++----- .../modules/motor/speed_control/speed_control.txt | 2 +- 3 files changed, 37 insertions(+), 35 deletions(-) (limited to 'digital/avr') diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c index 2caf0937..c13b4614 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.c +++ b/digital/avr/modules/motor/speed_control/speed_control.c @@ -33,44 +33,45 @@ static void speed_control_update_by_speed (speed_control_t *speed_control) { /* Update current speed (be careful of overflow!). */ - if (speed_control->cons > speed_control->cur) + if (speed_control->cons_f > speed_control->cur_f) { - if ((uint16_t) (speed_control->cons - speed_control->cur) - < (uint16_t) speed_control->acc) - speed_control->cur = speed_control->cons; + if ((uint32_t) (speed_control->cons_f - speed_control->cur_f) + < (uint32_t) speed_control->acc_f) + speed_control->cur_f = speed_control->cons_f; else - speed_control->cur += speed_control->acc; + speed_control->cur_f += speed_control->acc_f; } else { - if ((uint16_t) (speed_control->cur - speed_control->cons) - < (uint16_t) speed_control->acc) - speed_control->cur = speed_control->cons; + if ((uint32_t) (speed_control->cur_f - speed_control->cons_f) + < (uint32_t) speed_control->acc_f) + speed_control->cur_f = speed_control->cons_f; else - speed_control->cur -= speed_control->acc; + speed_control->cur_f -= speed_control->acc_f; } } /** Compute maximum allowed speed according to: distance left, maximum speed, * current speed and acceleration. */ -static int16_t -speed_control_compute_max_speed (int32_t d, int16_t cur, int16_t acc, - int8_t max) +static int32_t +speed_control_compute_max_speed_f (int32_t d, int32_t cur_f, int16_t acc_f, + int16_t max) { int16_t s; + int32_t s_f; /* Compute maximum speed in order to be able to brake in time. * The "+ 0xff" is to ceil result. * s = sqrt (2 * a * d) */ - s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8); + s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc_f + 0xff) >> 8); /* Apply consign. */ s = UTILS_MIN (max, 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; + s_f = (int32_t) s << 8; + UTILS_BOUND (s_f, cur_f - acc_f, cur_f + acc_f); + return s_f; } /** Update current speed according to a position consign. */ @@ -79,8 +80,9 @@ speed_control_update_by_position (speed_control_t *speed_control, pos_control_t *pos_control) { int32_t diff = speed_control->pos_cons - pos_control->cons; - speed_control->cur = speed_control_compute_max_speed - (diff, speed_control->cur, speed_control->acc, speed_control->max); + speed_control->cur_f = speed_control_compute_max_speed_f + (diff, speed_control->cur_f, speed_control->acc_f, + speed_control->max); } void @@ -103,17 +105,17 @@ speed_control_update (speed_control_t *speed_control, else speed_control_update_by_speed (speed_control); /* Update shaft position. */ - speed_control->pos_control->cons += speed_control->cur >> 8; + speed_control->pos_control->cons += speed_control->cur_f >> 8; } else - speed_control->cur = 0; + speed_control->cur_f = 0; } void -speed_control_set_speed (speed_control_t *speed_control, int8_t speed) +speed_control_set_speed (speed_control_t *speed_control, int16_t speed) { speed_control->use_pos = 0; - speed_control->cons = speed << 8; + speed_control->cons_f = speed << 8; } void @@ -143,8 +145,8 @@ speed_control_hard_stop (speed_control_t *speed_control) { /* No future movement. */ speed_control->use_pos = 0; - speed_control->cur = 0; - speed_control->cons = 0; + speed_control->cur_f = 0; + speed_control->cons_f = 0; /* Really stop right here, position control on the current point. */ speed_control->pos_control->cons = speed_control->pos_control->cur; } diff --git a/digital/avr/modules/motor/speed_control/speed_control.h b/digital/avr/modules/motor/speed_control/speed_control.h index 14e1ef15..714f3d54 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.h +++ b/digital/avr/modules/motor/speed_control/speed_control.h @@ -30,20 +30,20 @@ /** Speed control state. */ struct speed_control_t { - /** Current speed, f8.8. */ - int16_t cur; + /** Current speed, f16.8. */ + int32_t cur_f; /** Whether to use the consign position (1) or not (0). */ uint8_t use_pos; - /** Consign speed, f8.8. */ - int16_t cons; + /** Consign speed, f16.8. */ + int32_t cons_f; /** Consign position. */ uint32_t pos_cons; - /** Maximum speed for position consign, u7. */ - int8_t max; - /** Slow speed, deprecated, u7. */ - int8_t slow; + /** Maximum speed for position consign, u15. */ + int16_t max; + /** Slow speed, deprecated, u15. */ + int16_t slow; /** Acceleration, f8.8. */ - int16_t acc; + int16_t acc_f; /** Associated position control, to simplify function prototypes. */ pos_control_t *pos_control; }; @@ -61,7 +61,7 @@ speed_control_update (speed_control_t *speed_control, /** Set speed consign. Accelerate to the given speed. */ void -speed_control_set_speed (speed_control_t *speed_control, int8_t speed); +speed_control_set_speed (speed_control_t *speed_control, int16_t speed); /** Set position consign offset. Move to a position measured from the current * controlled position (which may be different from the actual position), diff --git a/digital/avr/modules/motor/speed_control/speed_control.txt b/digital/avr/modules/motor/speed_control/speed_control.txt index 94b4d0b4..2cdebb46 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.txt +++ b/digital/avr/modules/motor/speed_control/speed_control.txt @@ -13,7 +13,7 @@ too high. Usage ===== -The `max` and `acc` parameters should be set to maximum speed and maximum +The `max` and `acc_f` parameters should be set to maximum speed and maximum acceleration. Other fields should better be changed using the helper functions. -- cgit v1.2.3