servo_sim.h

Go to the documentation of this file.

 
#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
typedef struct {
    uint16_t stop_lo_us;
    uint16_t stop_hi_us;
 
    float    free_a;        
    float    stall_a;       
    uint16_t bind_us;

    float    travel_a;
    uint16_t stiff_lo_us;
    uint16_t stiff_hi_us;
    float    stiff_a;
 
    float    slew_us_per_ms; 
    float    noise_a;        

    uint16_t limit_lo_us;
    uint16_t limit_hi_us;
} servo_sim_cfg_t;
 
typedef struct {
    servo_sim_cfg_t cfg;
    float    position_us;   
    uint32_t last_ms;                   
    uint32_t seed;                      
    bool     started;                   
} servo_sim_t;

void servo_sim_defaults(servo_sim_cfg_t *cfg);

void servo_sim_init(servo_sim_t *s, const servo_sim_cfg_t *cfg);

float servo_sim_step(servo_sim_t *s, uint16_t cmd_us, uint32_t now_ms);
 
/* ------------------------------------------ two servos on one surface */

typedef struct {
    uint16_t centre_us;
    int16_t  offset_us;
    float    travel;
    float    free_a;
    float    fight_a_per_us;
    float    travel_a;
    float    slew_us_per_ms;
    float    noise_a;
} servo_pair_cfg_t;
 
typedef struct {
    servo_pair_cfg_t cfg;
    float    pos_a_us;
    float    pos_b_us;
    uint32_t last_ms;
    bool     started;
    uint32_t seed;
} servo_pair_t;

void servo_pair_defaults(servo_pair_cfg_t *cfg, uint16_t centre_us);
void servo_pair_init(servo_pair_t *p, const servo_pair_cfg_t *cfg);

float servo_pair_step(servo_pair_t *p, uint16_t cmd_a_us, uint16_t cmd_b_us,
                      uint32_t now_ms);

float servo_pair_disagreement(const servo_pair_t *p, uint16_t cmd_a_us,
                              uint16_t cmd_b_us);