Servo.h

/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved. 
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX INC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX INC.
******************************************************************************/
/*
Servo model header file

Author: James Tabony
*/
/*
Metadata for MS GUI:
imdata = {"displayname" : "Servo",
          "exclude" : False,
          "category" : "Actuators"
}
aliases = {"max_angle" : "Maximum Servo Angle",
           "min_angle" : "Minimum Servo Angle",
           "servo_noise_std" : "Noise",
           "seed_value" : "EXCLUDE",
           "latency" : "EXCLUDE",
           "misalignment_angle" : "Misalignment Angle",
           "initial_servo_angle" : "Initial Servo Angle",
           "servo_speed" : "Servo Speed",
           "servo_angle_command" : "Command Servo Angle [rad]",
           "servo_angle_actual" : "Actual Servo Angle [rad]"
}
*/

#ifndef MODELS_ACTUATORS_SERVO_MODEL_H
#define MODELS_ACTUATORS_SERVO_MODEL_H

#include "simulation/Model.h"
#include "core/CartesianVector.hpp"
#include "models/support/BiasNoiseModel.h"
#include "utils/LatencyUtil.hpp"

namespace warptwin {
    /**
     * @brief   Model to simulate a servo's motion
     * 
     * This model simulates the behavior of a servo. The servo accounts for the following properties:
     *      Command noise - Represented as Gaussian additive noise, present due to physical imperfections
     *      Servo turn bounds - most servos cannot spin indefinitely in either direction, this model lets one
     *                          set those bounds
     *      Servo speed - Servos cannot instantly transition between command states, they must move at some
     *                    maximum speed
     *      Latency - Commands sent to the servo must traverse over physical wires, this will introduce delay
     * 
     * @author James Tabony <james.tabony@attx.tech>
    */
    MODEL(Servo)
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
        START_PARAMS
            /** The maximum angle to which the servo can spin to, in the positive angle direction. [radians] */
            SIGNAL(max_angle,               double,                 2.0*M_PI)
            /** The minimum angle to which the servo can spin to, in the negative angle direction. 
             *  This value should be not abs(angle), for example if the servo can spin to +- pi radians,
             *  then the minimum would be -pi. [radians] */
            SIGNAL(min_angle,               double,                 -2.0*M_PI)
            /** The standard deviation of servo actuation noise. [radians] */
            SIGNAL(servo_noise_std,         double,                 0.0)
            /** Value to seed the internal RNG for this model. */
            SIGNAL(seed_value,              int,                    0)
            /** The latency of the servo, this is the delay of between the command to the servo and realization
             *  of that command. [milliseconds] */
            SIGNAL(latency,                 int,                    0)
            /** The misalignment angle of the servo. This comes from imperfect installation. This sign of this
             *  value should be the same as the positive and negative angle representation in the servo point of
             *  reference. [radians] */
            SIGNAL(misalignment_angle,      double,                 0.0)
            /** The initial angle of the servo on model startup. [radians] */
            SIGNAL(initial_servo_angle,     double,                 0.0)
            /** The maximum speed at which the servo can spin. [radians/second] */
            SIGNAL(servo_speed,             double,                 0.0)
        END_PARAMS

        // Model inputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_INPUTS 
            /** The commanded angle of the servo. [radians] */
            SIGNAL(servo_angle_command,     double,                 0.0)
        END_INPUTS

        // Model outputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_OUTPUTS
            /** The actual current angle of the servo with noise, latency, and servo speed
             *  implemented. [radians] */
            SIGNAL(servo_angle_actual,      double,                 0.0)
        END_OUTPUTS

        int16 activate() override;
        int16 deactivate() override;

    protected:
        int16 start() override;
        int16 execute() override; 

        /// @brief The bias and noise model for servo angle output
        BiasNoiseModel _noise_model;

        /// @brief The latency model for servo angle output
        LatencyUtil<double> _latency_model;

        /// @brief Temporary value that has perturbations acted upon it
        double _servo_angle_actual;

        /// @brief Temporary value for the return of the latency value
        double _latency_return;

        /// @brief The turn direction of the servo for this time step
        double _turn_direction;

        /// @brief The maximum amount that the servo can spin in the time of a single simulation step
        double _max_turn_amount;
    };

}

#endif