mirror of
https://github.com/nasa/trick.git
synced 2024-12-23 06:52:26 +00:00
180 lines
5.0 KiB
Plaintext
180 lines
5.0 KiB
Plaintext
|
/************************TRICK HEADER*************************
|
||
|
PURPOSE:
|
||
|
(This comment lists out the other object files that are not included from c++ headers)
|
||
|
LIBRARY DEPENDENCIES:
|
||
|
(
|
||
|
)
|
||
|
*************************************************************/
|
||
|
|
||
|
#include "sim_objects/default_trick_sys.sm"
|
||
|
|
||
|
|
||
|
##include <math.h>
|
||
|
##include "sim_services/Message/include/message_proto.h"
|
||
|
##include "sim_services/Integrator/include/Integrator.hh"
|
||
|
##include "sim_services/Integrator/include/integrator_c_intf.h"
|
||
|
|
||
|
/* base ball... get it? :) */
|
||
|
class ballSimObject : public Trick::SimObject {
|
||
|
|
||
|
public:
|
||
|
double mass;
|
||
|
double input_position[2];
|
||
|
double speed;
|
||
|
double elevation;
|
||
|
|
||
|
double output_position[2];
|
||
|
double output_velocity[2];
|
||
|
double output_acceleration[2];
|
||
|
double output_external_force[2];
|
||
|
|
||
|
double origin[2];
|
||
|
double input_force;
|
||
|
double output_force[2];
|
||
|
|
||
|
|
||
|
int force_default_data () {
|
||
|
|
||
|
origin[0] = 0.0 ;
|
||
|
origin[1] = 2.0 ;
|
||
|
input_force = 8.0 ;
|
||
|
|
||
|
return(0) ;
|
||
|
} ;
|
||
|
|
||
|
int state_default_data () {
|
||
|
mass = 10.0 ;
|
||
|
speed = 3.5 ;
|
||
|
elevation = 0.785398163 ;
|
||
|
input_position[0] = 5.0 ;
|
||
|
input_position[1] = 5.0 ;
|
||
|
|
||
|
return(0) ;
|
||
|
} ;
|
||
|
|
||
|
int state_init() {
|
||
|
/* TRANSFER INPUT POSITION STATES TO OUTPUT POSITION STATES */
|
||
|
output_position[0] = input_position[0]; /* X state */
|
||
|
output_position[1] = input_position[1]; /* Y state */
|
||
|
|
||
|
/* TRANSFER INPUT SPEED AND ELEVATION INTO THE VELOCITY VECTOR */
|
||
|
output_velocity[0] = speed * cos( elevation ); /* X velocity */
|
||
|
output_velocity[1] = speed * sin( elevation ); /* Y velocity */
|
||
|
|
||
|
return(0) ;
|
||
|
} ;
|
||
|
|
||
|
int force_field() {
|
||
|
/* LOCAL VARIABLE DECLARATIONS */
|
||
|
double mag;
|
||
|
double rel_pos[2];
|
||
|
double unit[2];
|
||
|
|
||
|
/* GET RELATIVE VECTOR FROM BALL TO FORCE ORIGIN */
|
||
|
rel_pos[0] = origin[0] - output_position[0];
|
||
|
rel_pos[1] = origin[1] - output_position[1];
|
||
|
|
||
|
/* GET UNIT VECTOR AND POSITION MAGNITUDE FROM BALL TO FORCE ORIGIN */
|
||
|
/* Note sigularity when ball position = force origin. */
|
||
|
mag = sqrt( rel_pos[0]*rel_pos[0] + rel_pos[1]*rel_pos[1] );
|
||
|
unit[0] = rel_pos[0] / mag;
|
||
|
unit[1] = rel_pos[1] / mag;
|
||
|
|
||
|
/* COMPUTE EXTERNAL FORCE ON BALL IN THE DIRECTION OF THE UNIT VECTOR */
|
||
|
output_force[0] = input_force * (unit[0]);
|
||
|
output_force[1] = input_force * (unit[1]);
|
||
|
|
||
|
return(0) ;
|
||
|
} ;
|
||
|
|
||
|
int state_deriv() {
|
||
|
|
||
|
output_external_force[0] = output_force[0] ;
|
||
|
output_external_force[1] = output_force[1] ;
|
||
|
|
||
|
/* SOLVE FOR THE X AND Y ACCELERATIONS OF THE BALL */
|
||
|
output_acceleration[0] = output_external_force[0] / mass;
|
||
|
output_acceleration[1] = output_external_force[1] / mass;
|
||
|
|
||
|
return(0) ;
|
||
|
};
|
||
|
|
||
|
int state_integ() {
|
||
|
|
||
|
int ipass;
|
||
|
|
||
|
/* LOAD THE POSITION AND VELOCITY STATES */
|
||
|
load_state(
|
||
|
&output_position[0] ,
|
||
|
&output_position[1] ,
|
||
|
&output_velocity[0] ,
|
||
|
&output_velocity[1] ,
|
||
|
NULL
|
||
|
);
|
||
|
|
||
|
/* LOAD THE POSITION AND VELOCITY STATE DERIVATIVES */
|
||
|
load_deriv(
|
||
|
&output_velocity[0] ,
|
||
|
&output_velocity[1] ,
|
||
|
&output_acceleration[0] ,
|
||
|
&output_acceleration[1] ,
|
||
|
NULL
|
||
|
);
|
||
|
|
||
|
/* CALL THE TRICK INTEGRATION SERVICE */
|
||
|
ipass = integrate();
|
||
|
|
||
|
/* UNLOAD THE NEW POSITION AND VELOCITY STATES */
|
||
|
unload_state(
|
||
|
&output_position[0] ,
|
||
|
&output_position[1] ,
|
||
|
&output_velocity[0] ,
|
||
|
&output_velocity[1] ,
|
||
|
NULL
|
||
|
);
|
||
|
|
||
|
/* RETURN */
|
||
|
return( ipass );
|
||
|
|
||
|
} ;
|
||
|
|
||
|
int state_print() {
|
||
|
message_publish(8, "time = %f , position = %f , %f\n" , exec_get_sim_time() , output_position[0] , output_position[1]) ;
|
||
|
return 0 ;
|
||
|
} ;
|
||
|
|
||
|
ballSimObject() {
|
||
|
("default_data") force_default_data() ;
|
||
|
("default_data") state_default_data() ;
|
||
|
|
||
|
("initialization") state_init() ;
|
||
|
|
||
|
("derivative") force_field() ;
|
||
|
("derivative") state_deriv() ;
|
||
|
("integration") trick_ret = state_integ() ;
|
||
|
|
||
|
(10.0, "scheduled") trick_ret = state_print() ;
|
||
|
|
||
|
("freeze") state_print() ;
|
||
|
}
|
||
|
|
||
|
} ;
|
||
|
|
||
|
// Instantiations
|
||
|
ballSimObject ball ;
|
||
|
|
||
|
IntegLoop my_integ_loop (0.01) ball;
|
||
|
|
||
|
|
||
|
// Connect objects
|
||
|
void create_connections() {
|
||
|
|
||
|
// Set the default termination time
|
||
|
exec_set_terminate_time(300.0) ;
|
||
|
exec_set_freeze_frame(0.10) ;
|
||
|
|
||
|
my_integ_loop.getIntegrator(Runge_Kutta_2, 4);
|
||
|
|
||
|
}
|
||
|
|