DUECA/DUSIME
PulsedBody.cxx
/* ------------------------------------------------------------------ */
/* item : PulsedBody.cxx
made by : Rene' van Paassen
date : 060502
category : body file
description :
changes : 060502 first version
language : C++
copyright : (c) 2016 TUDelft-AE-C&S
copyright : (c) 2022 René van Paassen
license : EUPL-1.2
*/
#define PulsedBody_cxx
#include "PulsedBody.hxx"
#include <iostream>
using namespace std;
// create a RigidBody, with mass and inertia. Request one additional
// state for the engine.
RigidBody(100.0, 5.0, 6.0, 9.0, 0.0, 0.0, 0.0, 1),
// time constant of the engine, 1.2 s
tau_e(1.2),
// time step
dt(dt),
// force vector is initially zero, 3 elements.
force(Vector::Zero(3)),
// same for the point on the body
fpoint(Vector::Zero(3)),
// vector for the gravitational pull
gravity(Vector::Zero(3)),
// workspace integration, 13 states for body, 1 for me
workspace(14)
{
// modify point where forces are applied, index 2 is z-coordinate
// there is a slight offset in x coordinate
fpoint[2] = -1.0;
fpoint[0] = 1e-8;
// in z-direction
gravity[2] = 9.810665;
// with this, construction of the PulsedBody is complete.
}
{
// nothing to delete
}
void PulsedBody::derivative(VectorE& xd, double dt_offset)
{
// we added one state, engine thrust. The engine thrust is state
// variable x[13]. Calculate the derivative, using the time
// constant, assume we have a constant setpoint of 1330 N for the thrust:
xd[13] = (1330.0 - x[13]) / tau_e;
// now, all other motions are calculated by the RigidBody parent.
// use this to clear all forces from the previous step
// apply the thrust to the body
force.setZero(); // for clarity, all elements of force vector
// are zeroed here.
force[2] = -x[13]; // force in z direction = thruster state
applyBodyForce(force, fpoint); // apply force on the body
addInertialGravity(gravity); // add gravity field
// if we now call derivative from the RigidBody parent, the
// remaining 13 states (0 -- 12) are calculated. Expressly call the
// parent method, otherwise we end up calling ourselves!
// at this point, the derivative of the state vector has been put in
// xd. Return to the calling function, most likely the integration
// routine.
return;
}
{
// use Runge-kutta integration on myself
// just to prove that we did something
cout << X() << endl;
// after a call to output, things like Psi, theta and phi should be
// calculated
output();
cout << "psi=" << psi() << " theta=" << theta() << " phi=" << phi()
<< " V=" << V() << " alpha=" << alpha() << " beta=" << beta() << endl;
// specific forces. Note that it is really inefficient (especially
// for a real-time program) to be creating and removing vector sp
// for each step.
Vector sp(6);
specific(sp);
cout << sp << endl;
}
int main()
{
// main function
PulsedBody pb(0.1);
for (int ii = 1000; ii--; ) {
pb.step();
}
return 0;
}