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

90.7

/lib/pbio/src/observer.c

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

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

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

#include <pbio/angle.h>
#include <pbio/dcmotor.h>
#include <pbio/int_math.h>
#include <pbio/observer.h>
#include <pbio/trajectory.h>

// Values generated by pbio/doc/control/model.py
#define MAX_NUM_SPEED (2500000)
#define MAX_NUM_ACCELERATION (25000000)
#define MAX_NUM_CURRENT (30000)
#define MAX_NUM_VOLTAGE (12000)
#define MAX_NUM_TORQUE (1000000)
#define PRESCALE_SPEED (858)
#define PRESCALE_ACCELERATION (85)
#define PRESCALE_CURRENT (71582)
#define PRESCALE_VOLTAGE (178956)
#define PRESCALE_TORQUE (2147)

/**
 * Resets the observer to a new angle. Speed and current are reset to zero.
 *
 * @param [in]  obs            The observer instance.
 * @param [in]  angle          Angle to which the observer should be reset.
 */
void pbio_observer_reset(pbio_observer_t *obs, const pbio_angle_t *angle) {

    // Reset angle to input and other states to zero.
    obs->angle = *angle;
    obs->speed = 0;
    obs->current = 0;

    // Reset stall state.
    obs->stalled = false;

    // Reset position differentiator.
    pbio_differentiator_reset(&obs->differentiator, angle);
}

/**
 * Gets the observer state, which is the estimated state of the real system.
 *
 * @param [in]  obs            The observer instance.
 * @param [out] speed_num      Speed in millidegrees/second as numeric derivative of angle.
 * @param [out] angle_est      Model estimate of angle in millidegrees.
 * @param [out] speed_est      Model estimate of speed in millidegrees/second.
 */
void pbio_observer_get_estimated_state(const pbio_observer_t *obs, int32_t *speed_num, pbio_angle_t *angle_est, int32_t *speed_est) {
    // Return angle in millidegrees.
    *angle_est = obs->angle;

    // Return speed in millidegrees per second.
    *speed_est = obs->speed;
    *speed_num = obs->speed_numeric;
}

static void update_stall_state(pbio_observer_t *obs, uint32_t time, pbio_dcmotor_actuation_t actuation, int32_t voltage, int32_t feedback_voltage) {

    // Anything other than voltage actuation is not included in the observer
    // model, so it should not cause any stall flags to be raised.
    if (actuation != PBIO_DCMOTOR_ACTUATION_VOLTAGE) {
        obs->stalled = false;
        return;
    }

    // Convert to forward motion to simplify checks.
    int32_t speed = obs->speed;
    if (voltage < 0) {
        speed *= -1;
        voltage *= -1;
        feedback_voltage *= -1;
    }

    // Check stall conditions.
    if (// Motor is going slow or even backward.
        speed < obs->settings.stall_speed_limit &&
        // Model is ahead of reality (and therefore pushing back negative),
        // indicating an unmodelled load.
        feedback_voltage < 0 &&
        // Feedback exceeds the (ratio of) the voltage it would be on getting
        // fully stuck (where applied voltage equals feedback).
        -feedback_voltage * 100 > voltage * obs->settings.feedback_voltage_stall_ratio &&
        // Feedback voltage is nonnegligible, i.e. larger than friction torque.
        voltage > obs->settings.feedback_voltage_negligible
        ) {
        // If this is the rising edge of the stall flag, reset start time.
        if (!obs->stalled) {
            obs->stall_start = time;
        }
        obs->stalled = true;
    } else {
        // Otherwise the motor is not stalled.
        obs->stalled = false;
    }
}

/**
 * Gets absolute observer feedback voltage.
 *
 * @param [in]  error          Absolute estimation error (mdeg).
 * @param [in]  s              Observer settings.
 * @return                     Feedback voltage in mV.
 */
static int32_t pbio_observer_get_feedback_voltage_abs(int32_t error, const pbio_observer_settings_t *s) {

    // Feedback voltage in first region is just linear in the low gain.
    if (error <= s->feedback_gain_threshold) {
        return error * s->feedback_gain_low / 1000;
    }

    // High region adds the increased gain for anything above the higher threshold.
    return (s->feedback_gain_threshold * s->feedback_gain_low + (error - s->feedback_gain_threshold) * s->feedback_gain_high) / 1000;
}

/**
 * Gets observer feedback voltage that keeps it close to measured value.
 *
 * @param [in]  obs            The observer instance.
 * @param [in]  angle          Measured angle used to correct the model.
 * @return                     Feedback voltage in mV.
 */
int32_t pbio_observer_get_feedback_voltage(const pbio_observer_t *obs, const pbio_angle_t *angle) {

    // Estimation error in millidegrees.
    int32_t error = pbio_angle_diff_mdeg(angle, &obs->angle);

    // Get matching absolute value of feedback voltage.
    int32_t feedback_voltage_abs = pbio_observer_get_feedback_voltage_abs(pbio_int_math_abs(error), &obs->settings);

    // Sign and clamp the feedback voltage.
    return pbio_int_math_clamp(feedback_voltage_abs * pbio_int_math_sign(error), MAX_NUM_VOLTAGE);
}

/**
 * Predicts next system state and corrects the model using a measurement.
 *
 * @param [in]  obs            The observer instance.
 * @param [in]  time           Wall time.
 * @param [in]  angle          Measured angle used to correct the model.
 * @param [in]  actuation      Actuation type currently applied to the motor.
 * @param [in]  voltage        If actuation type is voltage, this is the payload in mV.
 */
void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_t *angle, pbio_dcmotor_actuation_t actuation, int32_t voltage) {

    const pbio_observer_model_t *m = obs->model;

    // Update numerical derivative as speed sanity check.
    obs->speed_numeric = pbio_differentiator_update_and_get_speed(&obs->differentiator, angle);

    // Apply observer error feedback as voltage.
    int32_t feedback_voltage = pbio_observer_get_feedback_voltage(obs, angle);

    // Check stall condition.
    update_stall_state(obs, time, actuation, voltage, feedback_voltage);

    // The observer will get the applied voltage plus the feedback voltage to
    // keep it in sync with the real system.
    int32_t model_voltage = pbio_int_math_clamp(voltage + feedback_voltage, MAX_NUM_VOLTAGE);

    // Modified coulomb friction with transition linear in speed through origin.
    int32_t coulomb_friction = pbio_int_math_sign(obs->speed) * (
        pbio_int_math_abs(obs->speed) > obs->settings.coulomb_friction_speed_cutoff ?
        m->torque_friction:
        pbio_int_math_abs(obs->speed) * m->torque_friction / obs->settings.coulomb_friction_speed_cutoff
        );

    // Total torque equals friction plus any known external torques (currently none).
    int32_t torque = coulomb_friction;

    // Get next state based on current state and input: x(k+1) = Ax(k) + Bu(k)
    // This model assumes that the actuation mode is a voltage. If the real
    // mode is coast, back EMF is slightly overestimated, but an accurate
    // speed value is typically not needed in that use case.
    pbio_angle_add_mdeg(&obs->angle,
        PRESCALE_SPEED * obs->speed / m->d_angle_d_speed +
        PRESCALE_CURRENT * obs->current / m->d_angle_d_current +
        PRESCALE_VOLTAGE * model_voltage / m->d_angle_d_voltage +
        PRESCALE_TORQUE * torque / m->d_angle_d_torque);
    int32_t speed_next = pbio_int_math_clamp(0 +
        PRESCALE_SPEED * obs->speed / m->d_speed_d_speed +
        PRESCALE_CURRENT * obs->current / m->d_speed_d_current +
        PRESCALE_VOLTAGE * model_voltage / m->d_speed_d_voltage +
        PRESCALE_TORQUE * torque / m->d_speed_d_torque, MAX_NUM_SPEED);
    int32_t current_next = pbio_int_math_clamp(0 +
        PRESCALE_SPEED * obs->speed / m->d_current_d_speed +
        PRESCALE_CURRENT * obs->current / m->d_current_d_current +
        PRESCALE_VOLTAGE * model_voltage / m->d_current_d_voltage +
        PRESCALE_TORQUE * torque / m->d_current_d_torque, MAX_NUM_CURRENT);

    // In case of a speed transition through zero, undo (subtract) the effect
    // of friction, to avoid inducing chatter in the speed signal.
    if ((obs->speed < 0) != (speed_next < 0)) {
        speed_next -= PRESCALE_TORQUE * coulomb_friction / m->d_speed_d_torque;
    }

    // Save new state.
    obs->speed = speed_next;
    obs->current = current_next;
}

/**
 * Checks whether system is stalled by testing how far the estimate is ahead of
 * the measured angle, which is a measure for an unmodeled load.
 *
 * @param [in]  obs             The observer instance.
 * @param [in]  time            Wall time.
 * @param [out] stall_duration  For how long it has been stalled.
 * @return                      True if stalled, false if not.
 */
bool pbio_observer_is_stalled(const pbio_observer_t *obs, uint32_t time, uint32_t *stall_duration) {
    // Return stall flag, if stalled for some time.
    if (obs->stalled && time - obs->stall_start > obs->settings.stall_time) {
        *stall_duration = time - obs->stall_start;
        return true;
    }
    *stall_duration = 0;
    return false;
}

/**
 * Gets the maximum torque for use by user input validators.
 *
 * @returns      The maximum torque in uNm.
 *
*/
int32_t pbio_observer_get_max_torque(void) {
    return MAX_NUM_TORQUE;
}

/**
 * Calculates the feedforward torque needed to achieve the requested reference
 * rotational speed and acceleration.
 *
 * @param [in]  model               The observer model instance.
 * @param [in]  rate_ref            The reference rate in mdeg/s.
 * @param [in]  acceleration_ref    The reference acceleration in mdeg/s/s.
 * @returns                         The feedforward torque in uNm.
 *
*/
int32_t pbio_observer_get_feedforward_torque(const pbio_observer_model_t *model, int32_t rate_ref, int32_t acceleration_ref) {

    int32_t friction_compensation_torque = model->torque_friction / 2 * pbio_int_math_sign(rate_ref);
    int32_t back_emf_compensation_torque = PRESCALE_SPEED * pbio_int_math_clamp(rate_ref, MAX_NUM_SPEED) / model->d_torque_d_speed;
    int32_t acceleration_torque = PRESCALE_ACCELERATION * pbio_int_math_clamp(acceleration_ref, MAX_NUM_ACCELERATION) / model->d_torque_d_acceleration;

    // Total feedforward torque
    return pbio_int_math_clamp(friction_compensation_torque + back_emf_compensation_torque + acceleration_torque, MAX_NUM_TORQUE);
}

/**
 * Converts a torque to a voltage based on the given motor model.
 *
 * @param [in]  model               The observer model instance.
 * @param [in]  desired_torque      The torque in uNm.
 * @returns                         The voltage in mV.
*/
int32_t pbio_observer_torque_to_voltage(const pbio_observer_model_t *model, int32_t desired_torque) {
    return PRESCALE_TORQUE * pbio_int_math_clamp(desired_torque, MAX_NUM_TORQUE) / model->d_voltage_d_torque;
}

/**
 * Converts a voltage to a torque based on the given motor model.
 *
 * @param [in]  model               The observer model instance.
 * @param [in]  voltage             The voltage in mV.
 * @returns                         The torque in uNm.
*/
int32_t pbio_observer_voltage_to_torque(const pbio_observer_model_t *model, int32_t voltage) {
    return PRESCALE_VOLTAGE * pbio_int_math_clamp(voltage, MAX_NUM_VOLTAGE) / model->d_torque_d_voltage;
}

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2019-2023 The Pybricks Authors
3
4	// SPDX-License-Identifier: BSD-3-Clause
5	// Copyright (c) 2020-2023 LEGO System A/S
6
7	#include <stdbool.h>
8	#include <stdint.h>
9	#include <stdlib.h>
10	#include <math.h>
11
12	#include <pbio/angle.h>
13	#include <pbio/dcmotor.h>
14	#include <pbio/int_math.h>
15	#include <pbio/observer.h>
16	#include <pbio/trajectory.h>
17
18	// Values generated by pbio/doc/control/model.py
19	#define MAX_NUM_SPEED (2500000)
20	#define MAX_NUM_ACCELERATION (25000000)
21	#define MAX_NUM_CURRENT (30000)
22	#define MAX_NUM_VOLTAGE (12000)
23	#define MAX_NUM_TORQUE (1000000)
24	#define PRESCALE_SPEED (858)
25	#define PRESCALE_ACCELERATION (85)
26	#define PRESCALE_CURRENT (71582)
27	#define PRESCALE_VOLTAGE (178956)
28	#define PRESCALE_TORQUE (2147)
29
30	/**
31	* Resets the observer to a new angle. Speed and current are reset to zero.
32	*
33	* @param [in] obs The observer instance.
34	* @param [in] angle Angle to which the observer should be reset.
35	*/
36	void pbio_observer_reset(pbio_observer_t obs, const pbio_angle_t angle) {	27✔
37
38	// Reset angle to input and other states to zero.
39	obs->angle = *angle;	27✔
40	obs->speed = 0;	27✔
41	obs->current = 0;	27✔
42
43	// Reset stall state.
44	obs->stalled = false;	27✔
45
46	// Reset position differentiator.
47	pbio_differentiator_reset(&obs->differentiator, angle);	27✔
48	}	27✔
49
50	/**
51	* Gets the observer state, which is the estimated state of the real system.
52	*
53	* @param [in] obs The observer instance.
54	* @param [out] speed_num Speed in millidegrees/second as numeric derivative of angle.
55	* @param [out] angle_est Model estimate of angle in millidegrees.
56	* @param [out] speed_est Model estimate of speed in millidegrees/second.
57	*/
58	void pbio_observer_get_estimated_state(const pbio_observer_t obs, int32_t speed_num, pbio_angle_t angle_est, int32_t speed_est) {	89,175✔
59	// Return angle in millidegrees.
60	*angle_est = obs->angle;	89,175✔
61
62	// Return speed in millidegrees per second.
63	*speed_est = obs->speed;	89,175✔
64	*speed_num = obs->speed_numeric;	89,175✔
65	}	89,175✔
66
67	static void update_stall_state(pbio_observer_t *obs, uint32_t time, pbio_dcmotor_actuation_t actuation, int32_t voltage, int32_t feedback_voltage) {	53,713✔
68
69	// Anything other than voltage actuation is not included in the observer
70	// model, so it should not cause any stall flags to be raised.
71	if (actuation != PBIO_DCMOTOR_ACTUATION_VOLTAGE) {	53,713✔
72	obs->stalled = false;	2,455✔
73	return;	2,455✔
74	}
75
76	// Convert to forward motion to simplify checks.
77	int32_t speed = obs->speed;	51,258✔
78	if (voltage < 0) {	51,258✔
79	speed *= -1;	21,419✔
80	voltage *= -1;	21,419✔
81	feedback_voltage *= -1;	21,419✔
82	}
83
84	// Check stall conditions.
85	if (// Motor is going slow or even backward.	51,258✔
86	speed < obs->settings.stall_speed_limit &&	51,258✔
87	// Model is ahead of reality (and therefore pushing back negative),
88	// indicating an unmodelled load.
89	feedback_voltage < 0 &&	7,312✔
90	// Feedback exceeds the (ratio of) the voltage it would be on getting
91	// fully stuck (where applied voltage equals feedback).
92	-feedback_voltage * 100 > voltage * obs->settings.feedback_voltage_stall_ratio &&	7,312✔
93	// Feedback voltage is nonnegligible, i.e. larger than friction torque.
94	voltage > obs->settings.feedback_voltage_negligible	6,501✔
95	) {
96	// If this is the rising edge of the stall flag, reset start time.
97	if (!obs->stalled) {	1,106✔
98	obs->stall_start = time;	18✔
99	}
100	obs->stalled = true;	1,106✔
101	} else {
102	// Otherwise the motor is not stalled.
103	obs->stalled = false;	50,152✔
104	}
105	}
106
107	/**
108	* Gets absolute observer feedback voltage.
109	*
110	* @param [in] error Absolute estimation error (mdeg).
111	* @param [in] s Observer settings.
112	* @return Feedback voltage in mV.
113	*/
114	static int32_t pbio_observer_get_feedback_voltage_abs(int32_t error, const pbio_observer_settings_t *s) {	53,713✔
115
116	// Feedback voltage in first region is just linear in the low gain.
117	if (error <= s->feedback_gain_threshold) {	53,713✔
118	return error * s->feedback_gain_low / 1000;	52,124✔
119	}
120
121	// High region adds the increased gain for anything above the higher threshold.
122	return (s->feedback_gain_threshold * s->feedback_gain_low + (error - s->feedback_gain_threshold) * s->feedback_gain_high) / 1000;	1,589✔
123	}
124
125	/**
126	* Gets observer feedback voltage that keeps it close to measured value.
127	*
128	* @param [in] obs The observer instance.
129	* @param [in] angle Measured angle used to correct the model.
130	* @return Feedback voltage in mV.
131	*/
132	int32_t pbio_observer_get_feedback_voltage(const pbio_observer_t obs, const pbio_angle_t angle) {	53,713✔
133
134	// Estimation error in millidegrees.
135	int32_t error = pbio_angle_diff_mdeg(angle, &obs->angle);	53,713✔
136
137	// Get matching absolute value of feedback voltage.
138	int32_t feedback_voltage_abs = pbio_observer_get_feedback_voltage_abs(pbio_int_math_abs(error), &obs->settings);	53,713✔
139
140	// Sign and clamp the feedback voltage.
141	return pbio_int_math_clamp(feedback_voltage_abs * pbio_int_math_sign(error), MAX_NUM_VOLTAGE);	53,713✔
142	}
143
144	/**
145	* Predicts next system state and corrects the model using a measurement.
146	*
147	* @param [in] obs The observer instance.
148	* @param [in] time Wall time.
149	* @param [in] angle Measured angle used to correct the model.
150	* @param [in] actuation Actuation type currently applied to the motor.
151	* @param [in] voltage If actuation type is voltage, this is the payload in mV.
152	*/
153	void pbio_observer_update(pbio_observer_t obs, uint32_t time, const pbio_angle_t angle, pbio_dcmotor_actuation_t actuation, int32_t voltage) {	53,713✔
154
155	const pbio_observer_model_t *m = obs->model;	53,713✔
156
157	// Update numerical derivative as speed sanity check.
158	obs->speed_numeric = pbio_differentiator_update_and_get_speed(&obs->differentiator, angle);	53,713✔
159
160	// Apply observer error feedback as voltage.
161	int32_t feedback_voltage = pbio_observer_get_feedback_voltage(obs, angle);	53,713✔
162
163	// Check stall condition.
164	update_stall_state(obs, time, actuation, voltage, feedback_voltage);	53,713✔
165
166	// The observer will get the applied voltage plus the feedback voltage to
167	// keep it in sync with the real system.
168	int32_t model_voltage = pbio_int_math_clamp(voltage + feedback_voltage, MAX_NUM_VOLTAGE);	53,713✔
169
170	// Modified coulomb friction with transition linear in speed through origin.
171	int32_t coulomb_friction = pbio_int_math_sign(obs->speed) * (	53,713✔
172	pbio_int_math_abs(obs->speed) > obs->settings.coulomb_friction_speed_cutoff ?	53,713✔
173	m->torque_friction:	53,713✔
174	pbio_int_math_abs(obs->speed) * m->torque_friction / obs->settings.coulomb_friction_speed_cutoff	1,323✔
175	);
176
177	// Total torque equals friction plus any known external torques (currently none).
178	int32_t torque = coulomb_friction;	53,713✔
179
180	// Get next state based on current state and input: x(k+1) = Ax(k) + Bu(k)
181	// This model assumes that the actuation mode is a voltage. If the real
182	// mode is coast, back EMF is slightly overestimated, but an accurate
183	// speed value is typically not needed in that use case.
184	pbio_angle_add_mdeg(&obs->angle,	53,713✔
185	PRESCALE_SPEED * obs->speed / m->d_angle_d_speed +	53,713✔
186	PRESCALE_CURRENT * obs->current / m->d_angle_d_current +	53,713✔
187	PRESCALE_VOLTAGE * model_voltage / m->d_angle_d_voltage +	53,713✔
188	PRESCALE_TORQUE * torque / m->d_angle_d_torque);	53,713✔
189	int32_t speed_next = pbio_int_math_clamp(0 +	53,713✔
190	PRESCALE_SPEED * obs->speed / m->d_speed_d_speed +	53,713✔
191	PRESCALE_CURRENT * obs->current / m->d_speed_d_current +	53,713✔
192	PRESCALE_VOLTAGE * model_voltage / m->d_speed_d_voltage +	53,713✔
193	PRESCALE_TORQUE * torque / m->d_speed_d_torque, MAX_NUM_SPEED);	53,713✔
194	int32_t current_next = pbio_int_math_clamp(0 +	53,713✔
195	PRESCALE_SPEED * obs->speed / m->d_current_d_speed +	53,713✔
196	PRESCALE_CURRENT * obs->current / m->d_current_d_current +	53,713✔
197	PRESCALE_VOLTAGE * model_voltage / m->d_current_d_voltage +	53,713✔
198	PRESCALE_TORQUE * torque / m->d_current_d_torque, MAX_NUM_CURRENT);	53,713✔
199
200	// In case of a speed transition through zero, undo (subtract) the effect
201	// of friction, to avoid inducing chatter in the speed signal.
202	if ((obs->speed < 0) != (speed_next < 0)) {	53,713✔
203	speed_next -= PRESCALE_TORQUE * coulomb_friction / m->d_speed_d_torque;	6,839✔
204	}
205
206	// Save new state.
207	obs->speed = speed_next;	53,713✔
208	obs->current = current_next;	53,713✔
209	}	53,713✔
210
211	/**
212	* Checks whether system is stalled by testing how far the estimate is ahead of
213	* the measured angle, which is a measure for an unmodeled load.
214	*
215	* @param [in] obs The observer instance.
216	* @param [in] time Wall time.
217	* @param [out] stall_duration For how long it has been stalled.
218	* @return True if stalled, false if not.
219	*/
220	bool pbio_observer_is_stalled(const pbio_observer_t obs, uint32_t time, uint32_t stall_duration) {	×
221	// Return stall flag, if stalled for some time.
222	if (obs->stalled && time - obs->stall_start > obs->settings.stall_time) {	×
223	*stall_duration = time - obs->stall_start;	×
224	return true;	×
225	}
226	*stall_duration = 0;	×
227	return false;	×
228	}
229
230	/**
231	* Gets the maximum torque for use by user input validators.
232	*
233	* @returns The maximum torque in uNm.
234	*
235	*/
UNCOV 236	int32_t pbio_observer_get_max_torque(void) {	×
UNCOV 237	return MAX_NUM_TORQUE;	×
238	}
239
240	/**
241	* Calculates the feedforward torque needed to achieve the requested reference
242	* rotational speed and acceleration.
243	*
244	* @param [in] model The observer model instance.
245	* @param [in] rate_ref The reference rate in mdeg/s.
246	* @param [in] acceleration_ref The reference acceleration in mdeg/s/s.
247	* @returns The feedforward torque in uNm.
248	*
249	*/
250	int32_t pbio_observer_get_feedforward_torque(const pbio_observer_model_t *model, int32_t rate_ref, int32_t acceleration_ref) {	47,667✔
251
252	int32_t friction_compensation_torque = model->torque_friction / 2 * pbio_int_math_sign(rate_ref);	47,667✔
253	int32_t back_emf_compensation_torque = PRESCALE_SPEED * pbio_int_math_clamp(rate_ref, MAX_NUM_SPEED) / model->d_torque_d_speed;	47,667✔
254	int32_t acceleration_torque = PRESCALE_ACCELERATION * pbio_int_math_clamp(acceleration_ref, MAX_NUM_ACCELERATION) / model->d_torque_d_acceleration;	47,667✔
255
256	// Total feedforward torque
257	return pbio_int_math_clamp(friction_compensation_torque + back_emf_compensation_torque + acceleration_torque, MAX_NUM_TORQUE);	47,667✔
258	}
259
260	/**
261	* Converts a torque to a voltage based on the given motor model.
262	*
263	* @param [in] model The observer model instance.
264	* @param [in] desired_torque The torque in uNm.
265	* @returns The voltage in mV.
266	*/
267	int32_t pbio_observer_torque_to_voltage(const pbio_observer_model_t *model, int32_t desired_torque) {	47,681✔
268	return PRESCALE_TORQUE * pbio_int_math_clamp(desired_torque, MAX_NUM_TORQUE) / model->d_voltage_d_torque;	47,681✔
269	}
270
271	/**
272	* Converts a voltage to a torque based on the given motor model.
273	*
274	* @param [in] model The observer model instance.
275	* @param [in] voltage The voltage in mV.
276	* @returns The torque in uNm.
277	*/
278	int32_t pbio_observer_voltage_to_torque(const pbio_observer_model_t *model, int32_t voltage) {	69✔
279	return PRESCALE_VOLTAGE * pbio_int_math_clamp(voltage, MAX_NUM_VOLTAGE) / model->d_torque_d_voltage;	69✔
280	}

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