19016791193

Committed 02 Nov 2025 06:40PM UTC coverage: 57.167% (-2.6%) from 59.744%

Build # 19016791193

Build Type

Pull #406

github

Committed by

laurensvalk

Commit Message

bricks/virtualhub: Replace with embedded simulation.

Instead of using the newly introduced simhub alongside the virtualhub, we'll just replace the old one entirely now that it has reached feature parity. We can keep calling it the virtualhub.

Pull Request Pull Request #406: New virtual hub for more effective debugging

Run Details

41 of 48 new or added lines in 7 files covered. (85.42%)

414 existing lines in 53 files now uncovered.

4479 of 7835 relevant lines covered (57.17%)

17178392.75 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

25.0

/lib/pbio/src/control_settings.c

// SPDX-License-Identifier: MIT
// Copyright (c) 2018-2023 The Pybricks Authors

// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2020-2023 LEGO System A/S

#include <stdlib.h>

#include <pbio/config.h>
#include <pbio/control_settings.h>
#include <pbio/int_math.h>
#include <pbio/observer.h>

/**
 * Converts milliseconds to time ticks used by controller.
 *
 * @param [in] ms             Time in milliseconds.
 * @return                    Time converted to control ticks.
 */
uint32_t pbio_control_time_ms_to_ticks(uint32_t ms) {
    if (ms > UINT32_MAX / PBIO_TRAJECTORY_TICKS_PER_MS) {
        return UINT32_MAX;
    }
    return ms * PBIO_TRAJECTORY_TICKS_PER_MS;
}

/**
 * Converts time ticks used by controller to milliseconds.
 *
 * @param [in] ticks          Control timer ticks.
 * @return                    Time converted to milliseconds.
 */
uint32_t pbio_control_time_ticks_to_ms(uint32_t ticks) {
    return ticks / PBIO_TRAJECTORY_TICKS_PER_MS;
}

/**
 * Converts position-like control units to application-specific units.
 *
 * This should only be used if input/ouput are within known bounds.
 *
 * @param [in] s              Control settings containing the scale.
 * @param [in] input          Signal in control units.
 * @return                    Signal in application units.
 */
int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32_t input) {
    return input / s->ctl_steps_per_app_step;
}

/**
 * Converts position-like control units to application-specific units (integer).
 *
 * This can be used with large inputs but there is more overhead.
 *
 * @param [in] s              Control settings containing the scale.
 * @param [in] input          Signal in control units.
 * @return                    Signal in application units.
 */
int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t *s, const pbio_angle_t *input) {
    return pbio_angle_to_low_res(input, s->ctl_steps_per_app_step);
}

/**
 * Converts position-like control units to application-specific units (float).
 *
 * @param [in] s              Control settings containing the scale.
 * @param [in] input          Signal in control units.
 * @return                    Signal in application units.
 */
float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t *s, const pbio_angle_t *input) {
    return pbio_angle_to_low_res_float(input, s->ctl_steps_per_app_step);
}

/**
 * Converts application-specific units to position-like control units.
 *
 * @param [in] s              Control settings containing the scale.
 * @param [in] input          Signal in application units.
 * @return                    Signal in control units.
 */
int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32_t input) {
    if (input > INT32_MAX / s->ctl_steps_per_app_step) {
        return INT32_MAX;
    }
    if (input < -INT32_MAX / s->ctl_steps_per_app_step) {
        return -INT32_MAX;
    }
    return input * s->ctl_steps_per_app_step;
}

/**
 * Converts application-specific units to position-like control units.
 *
 * This can be used with large inputs but there is more overhead.
 *
 * @param [in]  s              Control settings containing the scale.
 * @param [in]  input          Signal in application units.
 * @param [out] output         Signal in control units.
 */
void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t *s, int32_t input, pbio_angle_t *output) {
    pbio_angle_from_low_res(output, input, s->ctl_steps_per_app_step);
}

/**
 * Converts actuation-like control units to application-specific units.
 *
 * @param [in] input          Actuation in control units (uNm).
 * @return                    Actuation in application units (mNm).
 */
int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input) {
    // All applications currently use this scale, but it could be generalized
    // to a application specific conversion constant.
    return input / 1000;
}

/**
 * Converts application-specific units to actuation-like control units.
 *
 * @param [in] input          Actuation in application units (mNm).
 * @return                    Actuation in control units (uNm).
 */
int32_t pbio_control_settings_actuation_app_to_ctl(int32_t input) {
    // All applications currently use this scale, but it could be generalized
    // to a application specific conversion constant.
    return input * 1000;
}

/**
 * Multiplies an angle (mdeg), speed (mdeg/s) or angle integral (mdeg s)
 * by a control gain and scales to uNm.
 *
 * @param [in] value         Input value (mdeg, mdeg/s, or mdeg s).
 * @param [in] gain          Gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
 * @return                   Torque in uNm.
 */
int32_t pbio_control_settings_mul_by_gain(int32_t value, int32_t gain) {
    return pbio_int_math_mult_then_div(gain, value, 1000);
}

/**
 * Divides a torque (uNm) by a control gain to get an angle (mdeg),
 * speed (mdeg/s) or angle integral (mdeg s), and accounts for scaling.
 *
 * Only positive gains are allowed. If the gain is zero or less, this returns
 * zero.
 *
 * @param [in] value         Input value (uNm).
 * @param [in] gain          Positive gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
 * @return                   Result in mdeg, mdeg/s, or mdeg s.
 */
int32_t pbio_control_settings_div_by_gain(int32_t value, int32_t gain) {
    if (gain < 1) {
        return 0;
    }
    return pbio_int_math_mult_then_div(value, 1000, gain);
}

/**
 * Multiplies a value by the loop time in seconds.
 *
 * @param [in] input          Input value.
 * @return                    Input scaled by loop time in seconds.
 */
int32_t pbio_control_settings_mul_by_loop_time(int32_t input) {
    return input / (1000 / PBIO_CONFIG_CONTROL_LOOP_TIME_MS);
}

/**
 * Gets the control limits for movement and actuation, in application units.
 *
 * @param [in]  s             Control settings structure from which to read.
 * @param [out] speed         Speed limit in application units.
 * @param [out] acceleration  Absolute rate of change of the speed during on-ramp of the maneuver.
 * @param [out] deceleration  Absolute rate of change of the speed during off-ramp of the maneuver.
 */
void pbio_control_settings_get_trajectory_limits(const pbio_control_settings_t *s, int32_t *speed, int32_t *acceleration, int32_t *deceleration) {
    *speed = pbio_control_settings_ctl_to_app(s, s->speed_max);
    *acceleration = pbio_control_settings_ctl_to_app(s, s->acceleration);
    *deceleration = pbio_control_settings_ctl_to_app(s, s->deceleration);
}

/**
 * Sets the control limits for movement in application units.
 *
 * @param [in] s              Control settings structure from which to read.
 * @param [in] speed          Speed limit in application units.
 * @param [in] acceleration   Absolute rate of change of the speed during on-ramp of the maneuver.
 * @param [in] deceleration   Absolute rate of change of the speed during off-ramp of the maneuver.
 * @return                    ::PBIO_SUCCESS on success
 *                            ::PBIO_ERROR_INVALID_ARG if any argument is negative.
 */
pbio_error_t pbio_control_settings_set_trajectory_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t deceleration) {
    // Validate that all inputs are within allowed bounds.
    pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);
    if (err != PBIO_SUCCESS) {
        return err;
    }
    err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, acceleration);
    if (err != PBIO_SUCCESS) {
        return err;
    }
    err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, deceleration);
    if (err != PBIO_SUCCESS) {
        return err;
    }
    s->speed_max = pbio_control_settings_app_to_ctl(s, speed);
    s->acceleration = pbio_control_settings_app_to_ctl(s, acceleration);
    s->deceleration = pbio_control_settings_app_to_ctl(s, deceleration);
    return PBIO_SUCCESS;
}

/**
 * Gets the control limits for actuation, in application units.
 *
 * @param [in]  s             Control settings structure from which to read.
 * @return      actuation     Upper limit on actuation.
 */
int32_t pbio_control_settings_get_actuation_limit(const pbio_control_settings_t *s) {
    return pbio_control_settings_actuation_ctl_to_app(s->actuation_max);
}

/**
 * Sets the control limit for actuation, in application units.
 *
 * @param [in] s              Control settings structure from which to read.
 * @param [in] limit          Upper limit on actuation.
 * @return                    ::PBIO_SUCCESS on success
 *                            ::PBIO_ERROR_INVALID_ARG if any argument is negative.
 */
pbio_error_t pbio_control_settings_set_actuation_limit(pbio_control_settings_t *s, int32_t limit) {
    if (limit < 1 || limit > pbio_control_settings_actuation_ctl_to_app(pbio_observer_get_max_torque())) {
        return PBIO_ERROR_INVALID_ARG;
    }
    s->actuation_max = pbio_control_settings_actuation_app_to_ctl(limit);
    return PBIO_SUCCESS;
}

/**
 * Gets the PID control parameters.
 *
 * Kp, Ki, and Kd are returned given in control units. Everything else in application units.
 *
 * @param [in]  s                    Control settings structure from which to read.
 * @param [out] pid_kp               Position error feedback constant.
 * @param [out] pid_ki               Accumulated error feedback constant.
 * @param [out] pid_kd               Speed error feedback constant.
 * @param [out] integral_deadzone    Zone (angle) around the target within which the integrator should not accumulate errors.
 * @param [out] integral_change_max  Absolute bound on the rate at which the integrator accumulates errors, in application units.
 */
void pbio_control_settings_get_pid(const pbio_control_settings_t *s, int32_t *pid_kp, int32_t *pid_ki, int32_t *pid_kd, int32_t *integral_deadzone, int32_t *integral_change_max) {
    *pid_kp = s->pid_kp;
    *pid_ki = s->pid_ki;
    *pid_kd = s->pid_kd;
    *integral_deadzone = pbio_control_settings_ctl_to_app(s, s->integral_deadzone);
    *integral_change_max = pbio_control_settings_ctl_to_app(s, s->integral_change_max);
}

/**
 * Verifies that a position-like limit (e.g. angle tolerance) has a valid value.
 *
 * @param [in] s                    Control settings structure.
 * @param [in] value                Value to be tested.
 * @return                          ::PBIO_SUCCESS or ::PBIO_ERROR_INVALID_ARG.
 */
static pbio_error_t pbio_control_settings_validate_position_setting(pbio_control_settings_t *s, int32_t value) {
    if (value < 0 || value > pbio_control_settings_ctl_to_app(s, INT32_MAX)) {
        return PBIO_ERROR_INVALID_ARG;
    }
    return PBIO_SUCCESS;
}

/**
 * Sets the PID control parameters.
 *
 * Kp, Ki, and Kd should be given in control units. Everything else in application units.
 *
 * @param [in] s                    Control settings structure to write to.
 * @param [in] pid_kp               Position error feedback constant.
 * @param [in] pid_ki               Accumulated error feedback constant.
 * @param [in] pid_kd               Speed error feedback constant.
 * @param [in] integral_deadzone    Zone (angle) around the target within which the integrator should not accumulate errors.
 * @param [in] integral_change_max  Absolute bound on the rate at which the integrator accumulates errors, in application units.
 * @return                           ::PBIO_SUCCESS on success
 *                                   ::PBIO_ERROR_INVALID_ARG if any argument is negative.
 */
pbio_error_t pbio_control_settings_set_pid(pbio_control_settings_t *s, int32_t pid_kp, int32_t pid_ki, int32_t pid_kd, int32_t integral_deadzone, int32_t integral_change_max) {
    if (pid_kp < 0 || pid_ki < 0 || pid_kd < 0) {
        return PBIO_ERROR_INVALID_ARG;
    }
    // integral_change_max has physical units of speed, so must satisfy bound.
    pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, integral_change_max);
    if (err != PBIO_SUCCESS) {
        return err;
    }
    err = pbio_control_settings_validate_position_setting(s, integral_deadzone);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    s->pid_kp = pid_kp;
    s->pid_ki = pid_ki;
    s->pid_kd = pid_kd;
    s->integral_deadzone = pbio_control_settings_app_to_ctl(s, integral_deadzone);
    s->integral_change_max = pbio_control_settings_app_to_ctl(s, integral_change_max);
    return PBIO_SUCCESS;
}

/**
 * Gets the tolerances associated with reaching a position target.
 * @param [in]  s           Control settings structure from which to read.
 * @param [out] speed       Speed tolerance in application units.
 * @param [out] position    Position tolerance in application units.
 */
void pbio_control_settings_get_target_tolerances(const pbio_control_settings_t *s, int32_t *speed, int32_t *position) {
    *position = pbio_control_settings_ctl_to_app(s, s->position_tolerance);
    *speed = pbio_control_settings_ctl_to_app(s, s->speed_tolerance);
}

/**
 * Sets the tolerances associated with reaching a position target, in application units.
 *
 * @param [in] s            Control settings structure to write to.
 * @param [in] speed        Speed tolerance in application units.
 * @param [in] position     Position tolerance in application units.
 * @return                  ::PBIO_SUCCESS on success
 *                          ::PBIO_ERROR_INVALID_ARG if any argument is negative.
 */
pbio_error_t pbio_control_settings_set_target_tolerances(pbio_control_settings_t *s, int32_t speed, int32_t position) {
    if (position < 0) {
        return PBIO_ERROR_INVALID_ARG;
    }
    pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);
    if (err != PBIO_SUCCESS) {
        return err;
    }
    err = pbio_control_settings_validate_position_setting(s, position);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    s->position_tolerance = pbio_control_settings_app_to_ctl(s, position);
    s->speed_tolerance = pbio_control_settings_app_to_ctl(s, speed);
    return PBIO_SUCCESS;
}

/**
 * Gets the tolerances associated with the controller being stalled, in application units.
 *
 * @param [in]  s           Control settings structure from which to read.
 * @param [out] speed       If this speed can't be reached with maximum actuation, it is stalled.
 * @param [out] time        Minimum consecutive stall time (ticks) before stall flag getter returns true.
 */
void pbio_control_settings_get_stall_tolerances(const pbio_control_settings_t *s, int32_t *speed, uint32_t *time) {
    *speed = pbio_control_settings_ctl_to_app(s, s->stall_speed_limit);
    *time = pbio_control_time_ticks_to_ms(s->stall_time);
}

/**
 * Sets the tolerances associated with the controller being stalled, in application units.
 *
 * @param [in] s            Control settings structure to write to.
 * @param [in] speed        If this speed can't be reached with maximum actuation, it is stalled.
 * @param [in] time         Minimum consecutive stall time (ticks) before stall flag getter returns true.
 * @return                  ::PBIO_SUCCESS on success
 *                          ::PBIO_ERROR_INVALID_ARG if any argument is negative.
 */
pbio_error_t pbio_control_settings_set_stall_tolerances(pbio_control_settings_t *s, int32_t speed, uint32_t time) {
    pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    s->stall_speed_limit = pbio_control_settings_app_to_ctl(s, speed);
    s->stall_time = pbio_control_time_ms_to_ticks(time);
    return PBIO_SUCCESS;
}

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2018-2023 The Pybricks Authors
3
4	// SPDX-License-Identifier: BSD-3-Clause
5	// Copyright (c) 2020-2023 LEGO System A/S
6
7	#include <stdlib.h>
8
9	#include <pbio/config.h>
10	#include <pbio/control_settings.h>
11	#include <pbio/int_math.h>
12	#include <pbio/observer.h>
13
14	/**
15	* Converts milliseconds to time ticks used by controller.
16	*
17	* @param [in] ms Time in milliseconds.
18	* @return Time converted to control ticks.
19	*/
20	uint32_t pbio_control_time_ms_to_ticks(uint32_t ms) {	8,472✔
21	if (ms > UINT32_MAX / PBIO_TRAJECTORY_TICKS_PER_MS) {	8,472✔
UNCOV 22	return UINT32_MAX;	×
23	}
24	return ms * PBIO_TRAJECTORY_TICKS_PER_MS;	8,472✔
25	}
26
27	/**
28	* Converts time ticks used by controller to milliseconds.
29	*
30	* @param [in] ticks Control timer ticks.
31	* @return Time converted to milliseconds.
32	*/
33	uint32_t pbio_control_time_ticks_to_ms(uint32_t ticks) {	1,019✔
34	return ticks / PBIO_TRAJECTORY_TICKS_PER_MS;	1,019✔
35	}
36
37	/**
38	* Converts position-like control units to application-specific units.
39	*
40	* This should only be used if input/ouput are within known bounds.
41	*
42	* @param [in] s Control settings containing the scale.
43	* @param [in] input Signal in control units.
44	* @return Signal in application units.
45	*/
46	int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32_t input) {	132✔
47	return input / s->ctl_steps_per_app_step;	132✔
48	}
49
50	/**
51	* Converts position-like control units to application-specific units (integer).
52	*
53	* This can be used with large inputs but there is more overhead.
54	*
55	* @param [in] s Control settings containing the scale.
56	* @param [in] input Signal in control units.
57	* @return Signal in application units.
58	*/
59	int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t s, const pbio_angle_t input) {	73✔
60	return pbio_angle_to_low_res(input, s->ctl_steps_per_app_step);	73✔
61	}
62
63	/**
64	* Converts position-like control units to application-specific units (float).
65	*
66	* @param [in] s Control settings containing the scale.
67	* @param [in] input Signal in control units.
68	* @return Signal in application units.
69	*/
70	float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t s, const pbio_angle_t input) {	2✔
71	return pbio_angle_to_low_res_float(input, s->ctl_steps_per_app_step);	2✔
72	}
73
74	/**
75	* Converts application-specific units to position-like control units.
76	*
77	* @param [in] s Control settings containing the scale.
78	* @param [in] input Signal in application units.
79	* @return Signal in control units.
80	*/
81	int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32_t input) {	132✔
82	if (input > INT32_MAX / s->ctl_steps_per_app_step) {	132✔
UNCOV 83	return INT32_MAX;	×
84	}
85	if (input < -INT32_MAX / s->ctl_steps_per_app_step) {	132✔
UNCOV 86	return -INT32_MAX;	×
87	}
88	return input * s->ctl_steps_per_app_step;	132✔
89	}
90
91	/**
92	* Converts application-specific units to position-like control units.
93	*
94	* This can be used with large inputs but there is more overhead.
95	*
96	* @param [in] s Control settings containing the scale.
97	* @param [in] input Signal in application units.
98	* @param [out] output Signal in control units.
99	*/
100	void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t s, int32_t input, pbio_angle_t output) {	109✔
101	pbio_angle_from_low_res(output, input, s->ctl_steps_per_app_step);	109✔
102	}	109✔
103
104	/**
105	* Converts actuation-like control units to application-specific units.
106	*
107	* @param [in] input Actuation in control units (uNm).
108	* @return Actuation in application units (mNm).
109	*/
110	int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input) {	5✔
111	// All applications currently use this scale, but it could be generalized
112	// to a application specific conversion constant.
113	return input / 1000;	5✔
114	}
115
116	/**
117	* Converts application-specific units to actuation-like control units.
118	*
119	* @param [in] input Actuation in application units (mNm).
120	* @return Actuation in control units (uNm).
121	*/
UNCOV 122	int32_t pbio_control_settings_actuation_app_to_ctl(int32_t input) {	×
123	// All applications currently use this scale, but it could be generalized
124	// to a application specific conversion constant.
UNCOV 125	return input * 1000;	×
126	}
127
128	/**
129	* Multiplies an angle (mdeg), speed (mdeg/s) or angle integral (mdeg s)
130	* by a control gain and scales to uNm.
131	*
132	* @param [in] value Input value (mdeg, mdeg/s, or mdeg s).
133	* @param [in] gain Gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
134	* @return Torque in uNm.
135	*/
136	int32_t pbio_control_settings_mul_by_gain(int32_t value, int32_t gain) {	190,676✔
137	return pbio_int_math_mult_then_div(gain, value, 1000);	190,676✔
138	}
139
140	/**
141	* Divides a torque (uNm) by a control gain to get an angle (mdeg),
142	* speed (mdeg/s) or angle integral (mdeg s), and accounts for scaling.
143	*
144	* Only positive gains are allowed. If the gain is zero or less, this returns
145	* zero.
146	*
147	* @param [in] value Input value (uNm).
148	* @param [in] gain Positive gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
149	* @return Result in mdeg, mdeg/s, or mdeg s.
150	*/
151	int32_t pbio_control_settings_div_by_gain(int32_t value, int32_t gain) {	116,343✔
152	if (gain < 1) {	116,343✔
153	return 0;	55,535✔
154	}
155	return pbio_int_math_mult_then_div(value, 1000, gain);	60,808✔
156	}
157
158	/**
159	* Multiplies a value by the loop time in seconds.
160	*
161	* @param [in] input Input value.
162	* @return Input scaled by loop time in seconds.
163	*/
164	int32_t pbio_control_settings_mul_by_loop_time(int32_t input) {	123,983✔
165	return input / (1000 / PBIO_CONFIG_CONTROL_LOOP_TIME_MS);	123,983✔
166	}
167
168	/**
169	* Gets the control limits for movement and actuation, in application units.
170	*
171	* @param [in] s Control settings structure from which to read.
172	* @param [out] speed Speed limit in application units.
173	* @param [out] acceleration Absolute rate of change of the speed during on-ramp of the maneuver.
174	* @param [out] deceleration Absolute rate of change of the speed during off-ramp of the maneuver.
175	*/
UNCOV 176	void pbio_control_settings_get_trajectory_limits(const pbio_control_settings_t s, int32_t speed, int32_t acceleration, int32_t deceleration) {	×
UNCOV 177	*speed = pbio_control_settings_ctl_to_app(s, s->speed_max);	×
UNCOV 178	*acceleration = pbio_control_settings_ctl_to_app(s, s->acceleration);	×
UNCOV 179	*deceleration = pbio_control_settings_ctl_to_app(s, s->deceleration);	×
UNCOV 180	}	×
181
182	/**
183	* Sets the control limits for movement in application units.
184	*
185	* @param [in] s Control settings structure from which to read.
186	* @param [in] speed Speed limit in application units.
187	* @param [in] acceleration Absolute rate of change of the speed during on-ramp of the maneuver.
188	* @param [in] deceleration Absolute rate of change of the speed during off-ramp of the maneuver.
189	* @return ::PBIO_SUCCESS on success
190	* ::PBIO_ERROR_INVALID_ARG if any argument is negative.
191	*/
UNCOV 192	pbio_error_t pbio_control_settings_set_trajectory_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t deceleration) {	×
193	// Validate that all inputs are within allowed bounds.
UNCOV 194	pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);	×
UNCOV 195	if (err != PBIO_SUCCESS) {	×
UNCOV 196	return err;	×
197	}
UNCOV 198	err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, acceleration);	×
UNCOV 199	if (err != PBIO_SUCCESS) {	×
UNCOV 200	return err;	×
201	}
UNCOV 202	err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, deceleration);	×
UNCOV 203	if (err != PBIO_SUCCESS) {	×
UNCOV 204	return err;	×
205	}
UNCOV 206	s->speed_max = pbio_control_settings_app_to_ctl(s, speed);	×
UNCOV 207	s->acceleration = pbio_control_settings_app_to_ctl(s, acceleration);	×
UNCOV 208	s->deceleration = pbio_control_settings_app_to_ctl(s, deceleration);	×
UNCOV 209	return PBIO_SUCCESS;	×
210	}
211
212	/**
213	* Gets the control limits for actuation, in application units.
214	*
215	* @param [in] s Control settings structure from which to read.
216	* @return actuation Upper limit on actuation.
217	*/
UNCOV 218	int32_t pbio_control_settings_get_actuation_limit(const pbio_control_settings_t *s) {	×
UNCOV 219	return pbio_control_settings_actuation_ctl_to_app(s->actuation_max);	×
220	}
221
222	/**
223	* Sets the control limit for actuation, in application units.
224	*
225	* @param [in] s Control settings structure from which to read.
226	* @param [in] limit Upper limit on actuation.
227	* @return ::PBIO_SUCCESS on success
228	* ::PBIO_ERROR_INVALID_ARG if any argument is negative.
229	*/
UNCOV 230	pbio_error_t pbio_control_settings_set_actuation_limit(pbio_control_settings_t *s, int32_t limit) {	×
UNCOV 231	if (limit < 1 \|\| limit > pbio_control_settings_actuation_ctl_to_app(pbio_observer_get_max_torque())) {	×
232	return PBIO_ERROR_INVALID_ARG;	×
233	}
UNCOV 234	s->actuation_max = pbio_control_settings_actuation_app_to_ctl(limit);	×
UNCOV 235	return PBIO_SUCCESS;	×
236	}
237
238	/**
239	* Gets the PID control parameters.
240	*
241	* Kp, Ki, and Kd are returned given in control units. Everything else in application units.
242	*
243	* @param [in] s Control settings structure from which to read.
244	* @param [out] pid_kp Position error feedback constant.
245	* @param [out] pid_ki Accumulated error feedback constant.
246	* @param [out] pid_kd Speed error feedback constant.
247	* @param [out] integral_deadzone Zone (angle) around the target within which the integrator should not accumulate errors.
248	* @param [out] integral_change_max Absolute bound on the rate at which the integrator accumulates errors, in application units.
249	*/
250	void pbio_control_settings_get_pid(const pbio_control_settings_t s, int32_t pid_kp, int32_t pid_ki, int32_t pid_kd, int32_t integral_deadzone, int32_t integral_change_max) {	×
251	*pid_kp = s->pid_kp;	×
252	*pid_ki = s->pid_ki;	×
253	*pid_kd = s->pid_kd;	×
254	*integral_deadzone = pbio_control_settings_ctl_to_app(s, s->integral_deadzone);	×
255	*integral_change_max = pbio_control_settings_ctl_to_app(s, s->integral_change_max);	×
256	}	×
257
258	/**
259	* Verifies that a position-like limit (e.g. angle tolerance) has a valid value.
260	*
261	* @param [in] s Control settings structure.
262	* @param [in] value Value to be tested.
263	* @return ::PBIO_SUCCESS or ::PBIO_ERROR_INVALID_ARG.
264	*/
265	static pbio_error_t pbio_control_settings_validate_position_setting(pbio_control_settings_t *s, int32_t value) {	×
266	if (value < 0 \|\| value > pbio_control_settings_ctl_to_app(s, INT32_MAX)) {	×
267	return PBIO_ERROR_INVALID_ARG;	×
268	}
UNCOV 269	return PBIO_SUCCESS;	×
270	}
271
272	/**
273	* Sets the PID control parameters.
274	*
275	* Kp, Ki, and Kd should be given in control units. Everything else in application units.
276	*
277	* @param [in] s Control settings structure to write to.
278	* @param [in] pid_kp Position error feedback constant.
279	* @param [in] pid_ki Accumulated error feedback constant.
280	* @param [in] pid_kd Speed error feedback constant.
281	* @param [in] integral_deadzone Zone (angle) around the target within which the integrator should not accumulate errors.
282	* @param [in] integral_change_max Absolute bound on the rate at which the integrator accumulates errors, in application units.
283	* @return ::PBIO_SUCCESS on success
284	* ::PBIO_ERROR_INVALID_ARG if any argument is negative.
285	*/
286	pbio_error_t pbio_control_settings_set_pid(pbio_control_settings_t *s, int32_t pid_kp, int32_t pid_ki, int32_t pid_kd, int32_t integral_deadzone, int32_t integral_change_max) {	×
287	if (pid_kp < 0 \|\| pid_ki < 0 \|\| pid_kd < 0) {	×
UNCOV 288	return PBIO_ERROR_INVALID_ARG;	×
289	}
290	// integral_change_max has physical units of speed, so must satisfy bound.
291	pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, integral_change_max);	×
292	if (err != PBIO_SUCCESS) {	×
UNCOV 293	return err;	×
294	}
295	err = pbio_control_settings_validate_position_setting(s, integral_deadzone);	×
296	if (err != PBIO_SUCCESS) {	×
UNCOV 297	return err;	×
298	}
299
300	s->pid_kp = pid_kp;	×
301	s->pid_ki = pid_ki;	×
302	s->pid_kd = pid_kd;	×
303	s->integral_deadzone = pbio_control_settings_app_to_ctl(s, integral_deadzone);	×
304	s->integral_change_max = pbio_control_settings_app_to_ctl(s, integral_change_max);	×
305	return PBIO_SUCCESS;	×
306	}
307
308	/**
309	* Gets the tolerances associated with reaching a position target.
310	* @param [in] s Control settings structure from which to read.
311	* @param [out] speed Speed tolerance in application units.
312	* @param [out] position Position tolerance in application units.
313	*/
314	void pbio_control_settings_get_target_tolerances(const pbio_control_settings_t s, int32_t speed, int32_t *position) {	×
315	*position = pbio_control_settings_ctl_to_app(s, s->position_tolerance);	×
316	*speed = pbio_control_settings_ctl_to_app(s, s->speed_tolerance);	×
317	}	×
318
319	/**
320	* Sets the tolerances associated with reaching a position target, in application units.
321	*
322	* @param [in] s Control settings structure to write to.
323	* @param [in] speed Speed tolerance in application units.
324	* @param [in] position Position tolerance in application units.
325	* @return ::PBIO_SUCCESS on success
326	* ::PBIO_ERROR_INVALID_ARG if any argument is negative.
327	*/
328	pbio_error_t pbio_control_settings_set_target_tolerances(pbio_control_settings_t *s, int32_t speed, int32_t position) {	×
329	if (position < 0) {	×
UNCOV 330	return PBIO_ERROR_INVALID_ARG;	×
331	}
332	pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);	×
333	if (err != PBIO_SUCCESS) {	×
UNCOV 334	return err;	×
335	}
336	err = pbio_control_settings_validate_position_setting(s, position);	×
337	if (err != PBIO_SUCCESS) {	×
UNCOV 338	return err;	×
339	}
340
341	s->position_tolerance = pbio_control_settings_app_to_ctl(s, position);	×
342	s->speed_tolerance = pbio_control_settings_app_to_ctl(s, speed);	×
343	return PBIO_SUCCESS;	×
344	}
345
346	/**
347	* Gets the tolerances associated with the controller being stalled, in application units.
348	*
349	* @param [in] s Control settings structure from which to read.
350	* @param [out] speed If this speed can't be reached with maximum actuation, it is stalled.
351	* @param [out] time Minimum consecutive stall time (ticks) before stall flag getter returns true.
352	*/
353	void pbio_control_settings_get_stall_tolerances(const pbio_control_settings_t s, int32_t speed, uint32_t *time) {	×
354	*speed = pbio_control_settings_ctl_to_app(s, s->stall_speed_limit);	×
355	*time = pbio_control_time_ticks_to_ms(s->stall_time);	×
356	}	×
357
358	/**
359	* Sets the tolerances associated with the controller being stalled, in application units.
360	*
361	* @param [in] s Control settings structure to write to.
362	* @param [in] speed If this speed can't be reached with maximum actuation, it is stalled.
363	* @param [in] time Minimum consecutive stall time (ticks) before stall flag getter returns true.
364	* @return ::PBIO_SUCCESS on success
365	* ::PBIO_ERROR_INVALID_ARG if any argument is negative.
366	*/
367	pbio_error_t pbio_control_settings_set_stall_tolerances(pbio_control_settings_t *s, int32_t speed, uint32_t time) {	×
368	pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);	×
369	if (err != PBIO_SUCCESS) {	×
UNCOV 370	return err;	×
371	}
372
373	s->stall_speed_limit = pbio_control_settings_app_to_ctl(s, speed);	×
374	s->stall_time = pbio_control_time_ms_to_ticks(time);	×
375	return PBIO_SUCCESS;	×
376	}

pybricks / pybricks-micropython / 19016791193

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous