maglev_control.cc

Go to the documentation of this file.

#include "maglev_control.h"
#include <string.h>

maglev_control::maglev_control(const char * mod_name) : cd_control_module(mod_name)
{
        plog("maglev_control\n");
}

void maglev_control::initialize_data()
{
    
    double set_point    = 0.01; 
    double rate_gain    = 0.02;
    double forward_gain = 630.414316714576;
    double i_equilibrium = 3.15319192637756;
    
    forward_gain_ref    = create_data( NULL, "forward_gain",    sd_type_double, "A/m",  "Forward Gain",     &forward_gain );     
    mark_command( forward_gain_ref ); mark_telemetry(forward_gain_ref);
    
    rate_gain_ref       = create_data( NULL, "rate_gain",       sd_type_double, "A/m",  "Rate Gain" ,       &rate_gain );     
    mark_command( rate_gain_ref ); mark_telemetry(rate_gain_ref);
    
    set_point_ref       = create_data( NULL, "set_point",      sd_type_double, "A/m",  "Set Point",     &set_point );     
    mark_command( set_point_ref ); mark_telemetry(set_point_ref);
    
    i_equilibrium_ref   = create_data( NULL, "i_equilibrium",      sd_type_double, "A/m",  "Equilibrium current",     &i_equilibrium );     
    mark_command( i_equilibrium_ref ); mark_telemetry(i_equilibrium_ref);

        
        // Request events
        request_event_notification( NULL, "MAGLEV_CONTROL_TRIGGER", 1 );
}

void maglev_control::initialize_data_requests()
{
    i_ref           = request_data( NULL, "current_sim",    sd_type_double, true, "A", "Current" );
    position_ref    = request_data( NULL, "position_sim",   sd_type_double, true, "", "Position" );
    velocity_ref    = request_data( NULL, "velocity_sim",   sd_type_double, true, "", "Velocity" );

}

void maglev_control::handle_event(int code)
{
        if (code == 1)
        {
        int valid[6];
                
                double k_f, k_r, i_eq, x_eq, x, v;
                
        // Make sure that all incoming data is valide
                int k = 0;
        
                valid[k++] = value_as_type( forward_gain_ref,   sd_type_double, &k_f);
                valid[k++] = value_as_type( rate_gain_ref,      sd_type_double, &k_r);
                valid[k++] = value_as_type( set_point_ref,      sd_type_double, &x_eq);
                valid[k++] = value_as_type( i_equilibrium_ref,  sd_type_double, &i_eq);
                valid[k++] = value_as_type( position_ref,       sd_type_double, &x);
                valid[k++] = value_as_type( velocity_ref,       sd_type_double, &v);
                
        
                int ok = valid[0];
                
                for ( int j = 1; j < k; j++ )
                        ok = ok && valid[j];
                
                if( !ok )
                {
                        int k = 0;
                        if( !valid[k++] ) plog("forward_gain is not valid\n"); 
                        if( !valid[k++] ) plog("rate_gain is not valid\n"); 
                        if( !valid[k++] ) plog("set_point is not valid\n"); 
                        if( !valid[k++] ) plog("i_equilibrium is not valid\n"); 
                        if( !valid[k++] ) plog("position  is not valid\n"); 
                        if( !valid[k++] ) plog("velocity  is not valid\n"); 
                        return;
                }
        
        // Compute the control
        double x_err = x - x_eq;
        double i = k_f*(x_err + k_r*v) + i_eq;
 
        // Send to the simulation
        set_value_as_type( i_ref,     sd_type_double, &i    ); 

        }
}

cd_control_module *maglev_control_builder(const char *name)
{
    return new maglev_control(name);
}