servo_limit.h

Go to the documentation of this file.

 
#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
typedef enum {
    SERVO_LIMIT_RUNNING = 0,
    SERVO_LIMIT_FOUND,
    SERVO_LIMIT_FAULT,
} servo_limit_state_t;
 
typedef enum {
    SERVO_LIMIT_FAULT_NONE = 0,
    SERVO_LIMIT_FAULT_OVERCURRENT,
    SERVO_LIMIT_FAULT_STALL,
    SERVO_LIMIT_FAULT_NO_LIMIT,
    SERVO_LIMIT_FAULT_BAD_CONFIG,
} servo_limit_fault_t;

#define SERVO_LIMIT_MIN_US 500u
#define SERVO_LIMIT_MAX_US 2500u
 
typedef struct {
    uint16_t centre_us;      
    int16_t  step_us;        
    uint16_t max_travel_us;  
 
    uint16_t settle_ms;      
    uint16_t measure_ms;     

    float    knee_ratio;
    float    knee_margin_a;
 
    float    hard_limit_a;   

    float    stall_above_a;
    uint16_t stall_ms;       
    uint16_t backoff_us;     
} servo_limit_cfg_t;

void servo_limit_defaults(servo_limit_cfg_t *cfg, uint16_t centre_us,
                          bool positive);
 
typedef struct {
    servo_limit_cfg_t   cfg;
    servo_limit_state_t state;
    servo_limit_fault_t fault;
 
    uint16_t cmd_us;         
    uint16_t last_free_us;   
    uint16_t limit_us;       
 
    float    baseline_a;     
    float    knee_a;         
 
    /* Internals: the settle/measure cycle at each position. */
    uint32_t phase_start_ms;
    bool     measuring;
    double   sum_a;
    uint32_t samples;
    uint32_t baseline_steps; 
    double   baseline_sum;
    uint32_t elevated_since_ms;
    bool     elevated;
    bool     started;
} servo_limit_t;

void servo_limit_init(servo_limit_t *lf, const servo_limit_cfg_t *cfg);

uint16_t servo_limit_step(servo_limit_t *lf, float current_a, uint32_t now_ms);

float servo_limit_progress(const servo_limit_t *lf);