6675885095

Committed 28 Oct 2023 08:38AM UTC coverage: 56.053% (+10.0%) from 46.074%

Build # 6675885095

Build Type

push

github

Committed by

laurensvalk

Commit Message

pybricks.hubs.MoveHub: Use standard hub init.

The Move Hub cannot have true vector axes, but we can still use this API to initialize the hub using numeric indices.

This ensures we can use the custom orientation not just in tilt, but also in acceleration like we do on other hubs.

Run Details

3616 of 6451 relevant lines covered (56.05%)

20895680.75 hits per line

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

53.06

/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.
 *
 * 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 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);
}

/**
 * Checks if a time sample is equal to or newer than a given base time stamp.
 *
 * @param [in] sample         Sample time.
 * @param [in] base           Base time to compare to.
 * @return                    True if sample time is equal to or newer than base time, else false.
 */
bool pbio_control_settings_time_is_later(uint32_t sample, uint32_t base) {
    return sample - base < UINT32_MAX / 2;
}

/**
 * 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) {	1,833✔
21	if (ms > UINT32_MAX / PBIO_TRAJECTORY_TICKS_PER_MS) {	1,833✔
22	return UINT32_MAX;
23	}
24	return ms * PBIO_TRAJECTORY_TICKS_PER_MS;	1,833✔
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) {	5✔
34	return ticks / PBIO_TRAJECTORY_TICKS_PER_MS;	5✔
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) {	41✔
47	return input / s->ctl_steps_per_app_step;	41✔
48	}
49
50	/**
51	* Converts position-like control units to application-specific units.
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) {	28✔
60	return pbio_angle_to_low_res(input, s->ctl_steps_per_app_step);	28✔
61	}
62
63	/**
64	* Converts application-specific units to position-like control units.
65	*
66	* @param [in] s Control settings containing the scale.
67	* @param [in] input Signal in application units.
68	* @return Signal in control units.
69	*/
70	int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32_t input) {	56✔
71	if (input > INT32_MAX / s->ctl_steps_per_app_step) {	56✔
72	return INT32_MAX;
73	}
74	if (input < -INT32_MAX / s->ctl_steps_per_app_step) {	56✔
75	return -INT32_MAX;
76	}
77	return input * s->ctl_steps_per_app_step;	56✔
78	}
79
80	/**
81	* Converts application-specific units to position-like control units.
82	*
83	* This can be used with large inputs but there is more overhead.
84	*
85	* @param [in] s Control settings containing the scale.
86	* @param [in] input Signal in application units.
87	* @param [out] output Signal in control units.
88	*/
89	void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t s, int32_t input, pbio_angle_t output) {	39✔
90	pbio_angle_from_low_res(output, input, s->ctl_steps_per_app_step);	39✔
91	}	39✔
92
93	/**
94	* Converts actuation-like control units to application-specific units.
95	*
96	* @param [in] input Actuation in control units (uNm).
97	* @return Actuation in application units (mNm).
98	*/
99	int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input) {	3✔
100	// All applications currently use this scale, but it could be generalized
101	// to a application specific conversion constant.
102	return input / 1000;	3✔
103	}
104
105	/**
106	* Converts application-specific units to actuation-like control units.
107	*
108	* @param [in] input Actuation in application units (mNm).
109	* @return Actuation in control units (uNm).
110	*/
111	int32_t pbio_control_settings_actuation_app_to_ctl(int32_t input) {	1✔
112	// All applications currently use this scale, but it could be generalized
113	// to a application specific conversion constant.
114	return input * 1000;	1✔
115	}
116
117	/**
118	* Multiplies an angle (mdeg), speed (mdeg/s) or angle integral (mdeg s)
119	* by a control gain and scales to uNm.
120	*
121	* @param [in] value Input value (mdeg, mdeg/s, or mdeg s).
122	* @param [in] gain Gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
123	* @return Torque in uNm.
124	*/
125	int32_t pbio_control_settings_mul_by_gain(int32_t value, int32_t gain) {	65,764✔
126	return pbio_int_math_mult_then_div(gain, value, 1000);	65,764✔
127	}
128
129	/**
130	* Divides a torque (uNm) by a control gain to get an angle (mdeg),
131	* speed (mdeg/s) or angle integral (mdeg s), and accounts for scaling.
132	*
133	* Only positive gains are allowed. If the gain is zero or less, this returns
134	* zero.
135	*
136	* @param [in] value Input value (uNm).
137	* @param [in] gain Positive gain in uNm/deg, uNm/(deg/s), or uNm/(deg s).
138	* @return Result in mdeg, mdeg/s, or mdeg s.
139	*/
140	int32_t pbio_control_settings_div_by_gain(int32_t value, int32_t gain) {	42,240✔
141	if (gain < 1) {	42,240✔
142	return 0;	10,220✔
143	}
144	return pbio_int_math_mult_then_div(value, 1000, gain);	21,100✔
145	}
146
147	/**
148	* Multiplies a value by the loop time in seconds.
149	*
150	* @param [in] input Input value.
151	* @return Input scaled by loop time in seconds.
152	*/
153	int32_t pbio_control_settings_mul_by_loop_time(int32_t input) {	45,428✔
154	return input / (1000 / PBIO_CONFIG_CONTROL_LOOP_TIME_MS);	45,428✔
155	}
156
157	/**
158	* Checks if a time sample is equal to or newer than a given base time stamp.
159	*
160	* @param [in] sample Sample time.
161	* @param [in] base Base time to compare to.
162	* @return True if sample time is equal to or newer than base time, else false.
163	*/
164	bool pbio_control_settings_time_is_later(uint32_t sample, uint32_t base) {	16,443✔
165	return sample - base < UINT32_MAX / 2;	16,443✔
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	*/
176	void pbio_control_settings_get_trajectory_limits(const pbio_control_settings_t s, int32_t speed, int32_t acceleration, int32_t deceleration) {	2✔
177	*speed = pbio_control_settings_ctl_to_app(s, s->speed_max);	2✔
178	*acceleration = pbio_control_settings_ctl_to_app(s, s->acceleration);	2✔
179	*deceleration = pbio_control_settings_ctl_to_app(s, s->deceleration);	2✔
180	}	2✔
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	*/
192	pbio_error_t pbio_control_settings_set_trajectory_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t deceleration) {	2✔
193	// Validate that all inputs are within allowed bounds.
194	pbio_error_t err = pbio_trajectory_validate_speed_limit(s->ctl_steps_per_app_step, speed);	2✔
195	if (err != PBIO_SUCCESS) {	2✔
196	return err;
197	}
198	err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, acceleration);	1✔
199	if (err != PBIO_SUCCESS) {	1✔
200	return err;
201	}
202	err = pbio_trajectory_validate_acceleration_limit(s->ctl_steps_per_app_step, deceleration);	1✔
203	if (err != PBIO_SUCCESS) {	1✔
204	return err;
205	}
206	s->speed_max = pbio_control_settings_app_to_ctl(s, speed);	1✔
207	s->acceleration = pbio_control_settings_app_to_ctl(s, acceleration);	1✔
208	s->deceleration = pbio_control_settings_app_to_ctl(s, deceleration);	1✔
209	return PBIO_SUCCESS;	1✔
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	*/
218	int32_t pbio_control_settings_get_actuation_limit(const pbio_control_settings_t *s) {	2✔
219	return pbio_control_settings_actuation_ctl_to_app(s->actuation_max);	2✔
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	*/
230	pbio_error_t pbio_control_settings_set_actuation_limit(pbio_control_settings_t *s, int32_t limit) {	1✔
231	if (limit < 1 \|\| limit > pbio_control_settings_actuation_ctl_to_app(pbio_observer_get_max_torque())) {	1✔
232	return PBIO_ERROR_INVALID_ARG;	×
233	}
234	s->actuation_max = pbio_control_settings_actuation_app_to_ctl(limit);	1✔
235	return PBIO_SUCCESS;	1✔
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	}
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) {	×
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) {	×
293	return err;
294	}
295	err = pbio_control_settings_validate_position_setting(s, integral_deadzone);	×
296	if (err != PBIO_SUCCESS) {	×
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) {	×
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) {	×
334	return err;
335	}
336	err = pbio_control_settings_validate_position_setting(s, position);	×
337	if (err != PBIO_SUCCESS) {	×
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) {	×
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 / 6675885095

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