summaryrefslogtreecommitdiff
path: root/digital/avr
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr')
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.c50
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.h20
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.txt2
3 files changed, 37 insertions, 35 deletions
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.