Controller.h

Go to the documentation of this file.

 
#ifndef Controller_h
#define Controller_h
 
// Arduino compiles everything in the src folder even if not included so it causes and error for the nano if this is not included.
#if defined(ARDUINO_TEENSY36)  || defined(ARDUINO_TEENSY41)
 
#include <Arduino.h>
 
#include "ExoData.h"
#include "Board.h"
#include "ParseIni.h"
#include <stdint.h>
#include "Utilities.h"
#include "config.h"
#include "Time_Helper.h"
#include <algorithm>
#include <utility>
 
class _Controller
{
    public:
        _Controller(config_defs::joint_id id, ExoData* exo_data);
        
        virtual ~_Controller(){};
        //virtual void set_controller(int joint, int controller) = 0; // Changes the controller for an individual joint
        
        virtual float calc_motor_cmd() = 0; 
        
        void reset_integral(); 
        
    protected:
        
        ExoData* _data; 
        ControllerData* _controller_data; 
        LegData* _leg_data; 
        JointData* _joint_data; 
        config_defs::joint_id _id;  
        Time_Helper* _t_helper;  
        float _t_helper_context; 
        float _t_helper_delta_t; 
        // Values for the PID controller
        float _integral_val; 
        float _prev_input;   
        float _prev_de_dt;   
        float _prev_pid_time; 
        float _pid(float cmd, float measurement, float p_gain, float i_gain, float d_gain);
        
        // Values for the Compact Form Model Free Adaptive Controller
        std::pair<float, float> measurements;
        std::pair<float, float> outputs;
        std::pair<float, float> phi; 
        float rho; 
        float lamda; 
        float etta; 
        float mu; 
        float upsilon; 
        float phi_1; 
        float _cf_mfac(float reference, float current_measurement);
};
 
class PropulsiveAssistive : public _Controller
{
    public:
        PropulsiveAssistive(config_defs::joint_id id, ExoData* exo_data);
        ~PropulsiveAssistive(){};
 
        float calc_motor_cmd();
    private:
        void _update_reference_angles(LegData* leg_data, ControllerData* controller_data, float percent_grf);
        void _capture_neutral_angle(LegData* leg_data, ControllerData* controller_data);
};
 
class ProportionalJointMoment : public _Controller
{
    public:
        ProportionalJointMoment(config_defs::joint_id id, ExoData* exo_data);
        ~ProportionalJointMoment(){};
        
        float calc_motor_cmd();
    private:
        std::pair<float, float> _stance_thresholds_left, _stance_thresholds_right;
        
        float _inclination_scaling{1.0f};
};
 
 
class ZeroTorque : public _Controller
{
    public:
        ZeroTorque(config_defs::joint_id id, ExoData* exo_data);
        ~ZeroTorque(){};
        
        float calc_motor_cmd();
};
 
class Stasis : public _Controller
{
    public:
        Stasis(config_defs::joint_id id, ExoData* exo_data);
        ~Stasis(){};
        
        float calc_motor_cmd();
};
 
class HeelToe: public _Controller
{
    public:
        HeelToe(config_defs::joint_id id, ExoData* exo_data);
        ~HeelToe(){};
        
        float calc_motor_cmd();
 
        float cmd_ff;                           
        float percent_gait;                      
        float prev_percent_gait;
 
        float fs;                               
        float fs_previous;                      
        int state;                              
        int prev_state;                         
        float state_4_start;                    
        float state_4_end;                      
        float previous_state_4_duration;        
        float prev_cmd;                         
        float state_4_start_cmd;                
        float swing_start;                      
        float swing_duration;                   
        float m;                                
        float x1;
        float y1;
        float x2;
        float y2;
        float x3;
        float y3;
 
        float A;
        float B;
        float C;
};
 
class ExtensionAngle: public _Controller
{
    public:
        ExtensionAngle(config_defs::joint_id id, ExoData* exo_data);
        ~ExtensionAngle(){};
        
        float calc_motor_cmd();
    private:
        void _update_max_angle(float angle);
        
        void _update_state(float angle);
        
        void _reset_angles();
        
        // the initial angles used for tracking the extent of the range of motions 
        const float _initial_max_angle = utils::degrees_to_radians(10); 
        const float _initial_min_angle = utils::degrees_to_radians(-5); 
        // Used to track the range of motion, angles are in rad.
        float _max_angle; 
        float _min_angle; 
        uint8_t _state; 
};
 
class BangBang: public _Controller
{
    public:
        BangBang(config_defs::joint_id id, ExoData* exo_data);
        ~BangBang(){};
        
        float calc_motor_cmd();
    private:
        void _update_max_angle(float angle);
        
        void _update_state(float angle);
        
        void _reset_angles();
        
        // the initial angles used for tracking the extent of the range of motions 
        const float _initial_max_angle = utils::degrees_to_radians(20); 
        const float _initial_min_angle = utils::degrees_to_radians(-5); 
        // Used to track the range of motion, angles are in rad.
        float _max_angle; 
        float _min_angle; 
        uint8_t _state; 
};
 
/*
 * LateStance Controller
 * This controller is for the hip joint
 * Applies const torque during late stance
 *
 * see ControllerData.h for details on the parameters used.
 */
class LateStance : public _Controller
{
public:
    LateStance(config_defs::joint_id id, ExoData* exo_data);
    ~LateStance(){};
 
    float calc_motor_cmd();
private:
    void _update_max_angle(float angle);
    void _update_state(float angle);
    void _reset_angles();
 
    // the initial angles used for tracking the extent of the range of motions 
    const float _initial_max_angle = utils::degrees_to_radians(20);
    const float _initial_min_angle = utils::degrees_to_radians(-5);
 
    // Used to track the range of motion, angles are in rad.
    float _max_angle;
    float _min_angle;
 
    // Used to track the state 0 is extension mode, 1 is flexion mode.
    uint8_t _state;
 
};
 
/*
 * GaitPhase Controller
 * This controller is for the hip joint
 * Applies flexion or extension torque as a function of the gait phase (needs heel and toe FSRs)
 *
 * see ControllerData.h for details on the parameters used.
 * 
 * This controller is still in development, this is just meant to be a framework for the controller that will be filled out with time
 */
class GaitPhase : public _Controller
{
    public:
        GaitPhase(config_defs::joint_id id, ExoData* exo_data);
        ~GaitPhase(){};
 
        float calc_motor_cmd();
 
        float slope;
        float state;
 
};
 
/*
 * Parabolic Controller
 * This controller is for the hip joint
 * Applies flexion or extension torque as a function of the gait phase (needs heel and toe FSRs)
 *
 * see ControllerData.h for details on the parameters used.
 *
 * This controller is still in development, this is just meant to be a framework for the controller that will be filled out with time
 */
class Parabolic : public _Controller
{
public:
    Parabolic(config_defs::joint_id id, ExoData* exo_data);
    ~Parabolic() {};
 
    float calc_motor_cmd();
};
 
class ZhangCollins: public _Controller
{
    public:
        ZhangCollins(config_defs::joint_id id, ExoData* exo_data);
        ~ZhangCollins(){};
        
        float calc_motor_cmd();
 
        float _spline_generation(float node1, float node2, float node3, float torque_magnitude, float percent_gait);
 
        float torque_cmd;
        
        float cmd;
};
 
class FranksCollinsHip: public _Controller
{
    public:
        FranksCollinsHip(config_defs::joint_id id, ExoData* exo_data);
        ~FranksCollinsHip(){};
       
        float calc_motor_cmd();
 
        float _spline_generation(float node1, float node2, float node3, float torque_magnitude, float shifted_percent_gait);
 
        float last_percent_gait;
        float last_start_time;
       
};
 
// class UserDefined: public _Controller
// {
    // public:
        // UserDefined(config_defs::joint_id id, ExoData* exo_data);
        // ~UserDefined(){};
        
        // float calc_motor_cmd();
    // private:
        // float _percent_x[controller_defs::user_defined::num_sample_points];
        // const float _step_size = 100/controller_defs::user_defined::num_sample_points;
        
        
// };
 
class Sine: public _Controller
{
    public:
        Sine(config_defs::joint_id id, ExoData* exo_data);
        ~Sine(){};
        
        float calc_motor_cmd();
    private:
        
        
};
 
class Perturbation : public _Controller
{
public:
    Perturbation(config_defs::joint_id id, ExoData* exo_data);
    ~Perturbation() {};
 
    float calc_motor_cmd();
};
 
class ConstantTorque : public _Controller
{
public:
    ConstantTorque(config_defs::joint_id id, ExoData* exo_data);
    ~ConstantTorque() {};
 
    float calc_motor_cmd();
 
    float current_torque;
    int counter;
};
 
class ElbowMinMax : public _Controller
{
public:
    ElbowMinMax(config_defs::joint_id id, ExoData* exo_data);
    ~ElbowMinMax() {};
 
    float calc_motor_cmd();
    
};
 
class PtbGeneral : public _Controller
{
public:
    PtbGeneral(config_defs::joint_id id, ExoData* exo_data);
    ~PtbGeneral() {};
 
    float calc_motor_cmd();
};
 
class CalibrManager : public _Controller
{
public:
    CalibrManager(config_defs::joint_id id, ExoData* exo_data);
    ~CalibrManager() {};
 
    float calc_motor_cmd();
};
 
#endif
#endif