observer.h

Go to the documentation of this file.

// 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

#ifndef _PBIO_OBSERVER_H_
#define _PBIO_OBSERVER_H_

#include <stdint.h>

#include <pbio/control_settings.h>
#include <pbio/dcmotor.h>
#include <pbio/differentiator.h>
#include <pbio/angle.h>

typedef struct _pbio_observer_model_t {
    int32_t d_angle_d_speed;
    int32_t d_speed_d_speed;
    int32_t d_current_d_speed;
    int32_t d_angle_d_current;
    int32_t d_speed_d_current;
    int32_t d_current_d_current;
    int32_t d_angle_d_voltage;
    int32_t d_speed_d_voltage;
    int32_t d_current_d_voltage;
    int32_t d_angle_d_torque;
    int32_t d_speed_d_torque;
    int32_t d_current_d_torque;
    int32_t d_voltage_d_torque;
    int32_t d_torque_d_voltage;
    int32_t d_torque_d_speed;
    int32_t d_torque_d_acceleration;
    int32_t torque_friction;
} pbio_observer_model_t;

typedef struct _pbio_observer_settings_t {
    int32_t stall_speed_limit;
    uint32_t stall_time;
    int32_t feedback_voltage_negligible;
    int32_t feedback_voltage_stall_ratio;
    int32_t feedback_gain_low;
    int32_t feedback_gain_high;
    int32_t feedback_gain_threshold;
    int32_t coulomb_friction_speed_cutoff;
} pbio_observer_settings_t;

typedef struct _pbio_observer_t {
    pbio_angle_t angle;
    int32_t speed;
    int32_t current;
    pbio_differentiator_t differentiator;
    int32_t speed_numeric;
    bool stalled;
    uint32_t stall_start;
    const pbio_observer_model_t *model;
    pbio_observer_settings_t settings;
} pbio_observer_t;

// Observer state functions:

void pbio_observer_reset(pbio_observer_t *obs, const pbio_angle_t *angle);
void pbio_observer_get_estimated_state(const pbio_observer_t *obs, int32_t *speed_num, pbio_angle_t *angle_est, int32_t *speed_est);
void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_t *angle, pbio_dcmotor_actuation_t actuation, int32_t voltage);
bool pbio_observer_is_stalled(const pbio_observer_t *obs, uint32_t time, uint32_t *stall_duration);
int32_t pbio_observer_get_feedback_voltage(const pbio_observer_t *obs, const pbio_angle_t *angle);

// Model conversion functions:

int32_t pbio_observer_get_max_torque(void);
int32_t pbio_observer_get_feedforward_torque(const pbio_observer_model_t *model, int32_t rate_ref, int32_t acceleration_ref);
int32_t pbio_observer_torque_to_voltage(const pbio_observer_model_t *model, int32_t desired_torque);
int32_t pbio_observer_voltage_to_torque(const pbio_observer_model_t *model, int32_t voltage);

#endif // _PBIO_OBSERVER_H_