servo_move.h

Go to the documentation of this file.

 
#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
#ifdef __cplusplus
extern "C" {
#endif

#define SERVO_MOVE_MIN_A        0.020f
#define SERVO_MOVE_NOISE_K      3.0f
#define SERVO_MOVE_BAND_A       0.05f
#define SERVO_MOVE_TIMEOUT_MS   3000u
#define SERVO_MOVE_FILTER_MAX   8u

typedef enum {
    SERVO_MOVE_IDLE = 0,    
    SERVO_MOVE_WAITING,     
    SERVO_MOVE_MOVING,      
    SERVO_MOVE_ARRIVED,     
    SERVO_MOVE_SETTLED,     
    SERVO_MOVE_LATE,        
    SERVO_MOVE_UNSEEN,      
} servo_move_state_t;

typedef enum {
    SERVO_MOVE_CLIP_LOW  = -1,  
    SERVO_MOVE_CLIP_NONE =  0,
    SERVO_MOVE_CLIP_HIGH =  1,  
} servo_move_clip_t;
 
typedef struct {
    uint32_t cmd_t;         
    uint32_t window_t;      
    float    rise_a;        
    float    ref_a;         
    float    move_a;        
    float    band_a;        
    uint8_t  settle_n;      
    uint8_t  filter_n;      
} servo_move_cfg_t;
 
typedef struct {
    servo_move_cfg_t   cfg;
    servo_move_state_t state;
    bool     left;          
    /* The samples in a row near ref_a, after movement. */
    uint8_t  run_n;
    float    run_lo, run_hi;
    uint32_t run_t;         
    /* The filter. */
    float    fbuf[SERVO_MOVE_FILTER_MAX];
    int8_t   fclip[SERVO_MOVE_FILTER_MAX];
    uint8_t  f_n, f_head;
    /* What it measured. */
    uint32_t moved_t;       
    uint32_t end_t;         
    float    sum;           
    uint32_t n;
    float    peak;          
    bool     clipped;       
} servo_move_t;

float servo_move_noise_a(uint32_t n, float sum, float sum_sq);

float servo_move_threshold_a(float noise_a);

uint32_t servo_move_window_ms(uint32_t lag_ms);

void servo_move_begin(servo_move_t *m, const servo_move_cfg_t *cfg);

void servo_move_prime(servo_move_t *m, float a, servo_move_clip_t clip);

servo_move_state_t servo_move_sample(servo_move_t *m, uint32_t at, float a,
                                     servo_move_clip_t clip);

servo_move_state_t servo_move_tick(servo_move_t *m, uint32_t now);

bool servo_move_over(const servo_move_t *m);

bool servo_move_arrived(const servo_move_t *m);

bool servo_move_moved(const servo_move_t *m);
 
#ifdef __cplusplus
}
#endif