fsi/motion-amroc/WindTurbine/src/TurbineMechanism.h

Go to the documentation of this file.

// -*- C++ -*-

#ifndef TURBINE_MECHANISM_H
#define TURBINE_MECHANISM_H

#define MAXBLADES 10

//#define TURBINE_DEBUG_ON
#define TURBINE_LOG_ON

#include "Point.h"
#include "Assembly.h"
#include "DH_Chain.h"
#include "Scene.h"

class TurbineControl : public controlable {
public:
  TurbineControl() : yaw_amp(0.), yaw_initial(180.), yaw_rate(0.), ontop(1) {}

  ~TurbineControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}

  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix, int num) {
    char VariableName[32];

    idNum = num;
    std::sprintf(VariableName,"Turbine(%02d)",num+1);

#ifdef TURBINE_DEBUG_ON
    std::cout << "Registering " << VariableName << std::endl;
#endif

    TLocCtrl = Ctrl.getSubDevice(std::string(VariableName));
    RegisterAt(TLocCtrl,"Turbine_name",turbine_name);
    RegisterAt(TLocCtrl,"Tower_name",tower_name);
    RegisterAt(TLocCtrl,"Nacelle_name",nacelle_name);
    RegisterAt(TLocCtrl,"Hub_name",hub_name);
    RegisterAt(TLocCtrl,"Tower_file",tower_file);
    RegisterAt(TLocCtrl,"Nacelle_file",nacelle_file);
    RegisterAt(TLocCtrl,"Hub_file",hub_file);
    RegisterAt(TLocCtrl,"Tower_filetype",tower_filetype);
    RegisterAt(TLocCtrl,"Nacelle_filetype",nacelle_filetype);
    RegisterAt(TLocCtrl,"Hub_filetype",hub_filetype);

    RegisterAt(TLocCtrl,"offset",part_offset);

    RegisterAt(TLocCtrl,"num_blades",num_blades);
    for (register int i = 0; i < MAXBLADES; i++) {
      std::sprintf(VariableName,"Blade(%02d)_name",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),blade_name[i]);
      std::sprintf(VariableName,"Blade(%02d)_file",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),blade_file[i]);
      std::sprintf(VariableName,"Blade(%02d)_filetype",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),blade_filetype[i]);
    }

    towerBase.register_at(TLocCtrl,"towerBase");
    tower_nacelle.register_at(TLocCtrl,"tower-nacelle");
    nacelleBase.register_at(TLocCtrl,"nacelleBase");
    nacelle_tower.register_at(TLocCtrl,"nacelle_tower");
    nacelle_rotor.register_at(TLocCtrl,"nacelle_rotor");
    for (register int i=0; i < MAXBLADES; i++) {
      std::sprintf(VariableName,"BladeJoint(%02d)",i+1);
      blades[i].register_at(TLocCtrl,std::string(VariableName));
    }

    scene_tower.register_at(TLocCtrl,"scene_tower");
    RegisterAt(TLocCtrl,"ontop",ontop);
    RegisterAt(TLocCtrl,"xpos",xpos);
    RegisterAt(TLocCtrl,"ypos",ypos);

    RegisterAt(TLocCtrl,"yaw_initial",yaw_initial);
    RegisterAt(TLocCtrl,"yaw_rate",yaw_rate);
    RegisterAt(TLocCtrl,"yaw_amplitude",yaw_amp);
    RegisterAt(TLocCtrl,"rotation_initial",rotation_initial);
    RegisterAt(TLocCtrl,"rotation_rate",rotation_rate);
    RegisterAt(TLocCtrl,"pitch_control",pitch_control);
    RegisterAt(TLocCtrl,"v_minPitch",v_minPitch);
    RegisterAt(TLocCtrl,"alpha",alpha);

    for (register int i=0; i < MAXBLADES; i++) {
      std::sprintf(VariableName,"theta(%02d)",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),theta[i]);
      std::sprintf(VariableName,"pitch_initial(%02d)",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),pitch_initial[i]);
      std::sprintf(VariableName,"pitch_rate(%02d)",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),pitch_rate[i]);
      std::sprintf(VariableName,"pitch_I(%02d)",i+1);
      RegisterAt(TLocCtrl,std::string(VariableName),pitch_I[i]);
    }


    RegisterAt(TLocCtrl,"I_rotor",I_rotor);
    RegisterAt(TLocCtrl,"brake_set",brake_set);
    RegisterAt(TLocCtrl,"brake_release",brake_release);

    RegisterAt(TLocCtrl,"Wind_avg",wind_avg);
    RegisterAt(TLocCtrl,"blade_length",blade_length);

  }

  void print() {
    ( mklog() << "TURBINE CONTROL DEVICE\n"
              << "\tnum_blades = " << num_blades << " tower_name = " << tower_name << std::endl).flush();
  }

public:
  double LastTime;
  int Outputs, OutputEvery, CheckPointEvery;
  int idNum;

  std::string turbine_name, tower_name, tower_file, nacelle_name, nacelle_file, hub_name, hub_file;
  std::string blade_name[MAXBLADES], blade_file[MAXBLADES];
  int num_blades, pitch_control;
  int tower_filetype, nacelle_filetype, hub_filetype, blade_filetype[MAXBLADES];

  int ontop;
  DataType xpos, ypos, part_offset;

  DataType yaw_initial, yaw_rate, yaw_amp, rotation_initial, rotation_rate, theta[MAXBLADES];
  DataType pitch_initial[MAXBLADES], pitch_rate[MAXBLADES], pitch_I[MAXBLADES], v_minPitch, alpha;

  JointControl towerBase, tower_nacelle, nacelleBase, nacelle_tower, nacelle_rotor, rotorBase, blades[MAXBLADES];

  LinkControl scene_tower;

  ControlDevice TLocCtrl;

  DataType I_rotor, I_driveTrain, I_nacelleTower, I_rotorTower, I_DriveTower;
  DataType brake_set, brake_release, wind_avg, blade_length;

};

class SolidSolverSpecific;

class Turbine {
  typedef Assembly<DataType,3> AssemblyType;
  typedef Scene<DataType,3> SceneType;
  typedef Loft<DataType> LoftType;
  typedef Joint<DataType,3> JointType;
  typedef DH_Link<DataType,3> LinkType;
  typedef DH_Chain<DataType,3> ChainType;

public:
  friend class Terrain;

  enum pitchType { free, collective, linear, relWind };

  Turbine() {
    p_rotor = 0.;
    p_nacelle = 0.;
    v_rotor = 0.;
    v_nacelle = 0;

    for (register int i = 0; i < MAXBLADES; i++) {
      a_blades[i] = 0.;
      v_blades[i] = 0.;
      p_blades[i] = 0.;
    }

  }

  ~Turbine() {}

  bool build(int i, std::string name, std::string file, int type) {
    Turbine0pt->GetNthPart(i)->read(type,file);
    Turbine0pt->GetNthPart(i)->setName(name);
    return 1;
  }

  bool specificSetup(SceneType& ThisScene, TurbineControl * TurbineCtrl ) {
    std::string nameTmp;
    ( mklog() << "\n\n*********\t Turbine specificSetup \tBegin\n*********" << std::endl ).flush();

    nameTmp = "Turbine_Assembly0";
    Turbine0pt = new AssemblyType;
    Turbine0pt->setName(TurbineCtrl->turbine_name);

    ThisScene.AddAssembly(*Turbine0pt);
    ThisScene.GetNthSceneAssembly(TurbineCtrl->idNum).print(0);

    Turbine0pt = ThisScene.GetNthAssembly(TurbineCtrl->idNum);

    Tower0 = new LoftType;
    Turbine0pt->AddPart(Tower0);

    Turbine0pt->GetNthPart(0)->read(TurbineCtrl->tower_filetype,TurbineCtrl->tower_file);
    Turbine0pt->GetNthPart(0)->setName(TurbineCtrl->tower_name);

    Tower0 = (LoftType *) Turbine0pt->GetNthPart(0);
    Tower0->measure();

    Nacel0 = new LoftType;
    Turbine0pt->AddPart(Nacel0);
    Turbine0pt->GetNthPart(1)->read(TurbineCtrl->nacelle_filetype,TurbineCtrl->nacelle_file);
    nameTmp = "Nacel_Loft0";
    Turbine0pt->GetNthPart(1)->setName(TurbineCtrl->nacelle_name);
    Nacel0 =  (LoftType *) Turbine0pt->GetNthPart(1);

    Rotor0 = new AssemblyType;
    nameTmp = "Rotor_Assembly0";
    char VariableName[16];
    std::sprintf(VariableName,"Rotor_%02d",TurbineCtrl->idNum+1);
    Rotor0->setName(std::string(VariableName));

    Turbine0pt->AddSubAssembly(*Rotor0);
    Rotor0 = Turbine0pt->GetNthSubAssembly(0);

    for (register int ib = 0; ib < TurbineCtrl->num_blades; ib++) {
      Blades[ib] = new LoftType;
      Rotor0->AddPart(Blades[ib]);
      Rotor0->GetNthPart(ib)->read(TurbineCtrl->blade_filetype[ib],TurbineCtrl->blade_file[ib]);
      Rotor0->GetNthPart(ib)->setName(TurbineCtrl->blade_name[ib]);
      Blades[ib] = (LoftType *) Rotor0->GetNthPart(ib);
    }

    Hub0 = new LoftType;
    Rotor0->AddPart(Hub0);
    Rotor0->GetNthPart(Rotor0->GetNumParts()-1 )->read(TurbineCtrl->hub_filetype,TurbineCtrl->hub_file);
    nameTmp = "Hub_Loft0";
    Rotor0->GetNthPart(Rotor0->GetNumParts()-1 )->setName(TurbineCtrl->hub_name);
    Hub0 =  (LoftType *) Rotor0->GetNthPart(Rotor0->GetNumParts()-1);

    ( mklog() << "\nparts added\n").flush();

    // Kinematic Chain
    PType o(0.), x(0.), y(0.), z(0.);

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    y(0) = 0.; y(1) = 1.; y(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    JointType * zero;
    zero = new JointType(TurbineCtrl->towerBase.origin,TurbineCtrl->towerBase.zaxis,TurbineCtrl->towerBase.xaxis,
                         JointType::revolute,Tower0->IdTag()); // Joint at base of tower

    L0 = new LinkType(*ThisScene.GetCoordFrame(),*zero, TurbineCtrl->scene_tower.r, TurbineCtrl->scene_tower.a, TurbineCtrl->scene_tower.d, TurbineCtrl->scene_tower.t); //link connecting scene to tower
    L0->setId(-1);

    o(0) = 0.; o(1) = 0.; o(2) = 31.5;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * one;
    one = new JointType(o,z,x,JointType::sphericalWrist,Tower0->IdTag()); // Joint at top center of tower
    LinkType * L1;
    L1 = new LinkType(*zero,*one);  //tower link == tower native link

    o(0) = -0.05; o(1) = 0.; o(2) = 0.;
    z(0) = 1.; z(1) = 0.; z(2) = 0.;
    x(0) = 0.; x(1) = 0.; x(2) = 1.;
    JointType * two;
    two = new JointType(o,z,x,JointType::revolute,Nacel0->IdTag()); // Joint at where nacell attaches to tower
    LinkType * L2;
    L2 = new LinkType(*one,*two, 0., 0., 0.2, 0.); //link connecting tower to nacel

    o(0) = 0.; o(1) = 0.; o(2) = 2.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * three;
    three = new JointType(o,z,x,JointType::revolute,Nacel0->IdTag()); // Joint where nacell attaches to rotor
    LinkType * L3;
    L3 = new LinkType(*two,*three); // nacel link: from joint with tower to joint with rotor

    o(0) = 0.; o(1) = 0.; o(2) = 1.6;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * four;
    four = new JointType(o,z,x,JointType::prismatic,Nacel0->IdTag()); // Joint at nacell's native origin
    LinkType * L4;
    L4 = new LinkType(*three,*four); //nacel native link

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    z(0) = 1.; z(1) = 0.; z(2) = 0.;
    x(0) = 0.; x(1) = 0.; x(2) = 1.;
    JointType * five;
    five = new JointType(o,z,x,JointType::revolute,Rotor0->IdTag()); // Joint where rotor attaches to nacelle
    LinkType * L5;
    L5 = new LinkType(*three,*five, 0., 0., 2.5, 0.); //link connecting nacel to rotor

    o(0) = 0.0; o(1) = 0.; o(2) = 0.2;//1.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 0.; x(1) = 1.; x(2) = 0.;
    JointType * six;
    six = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(0)->IdTag());
    LinkType * L6;
    L6 = new LinkType(*five,*six); //blade link

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * six_n;
    six_n = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(0)->IdTag());
    LinkType * L6_n;
    L6_n = new LinkType(*six,*six_n); //blade native link

    o(0) = 0.0; o(1) = 0.; o(2) = 0.2;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * seven;
    seven = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(1)->IdTag());
    LinkType * L7;
    L7 = new LinkType(*five,*seven); //blade link

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * seven_n;
    seven_n = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(1)->IdTag());
    LinkType * L7_n;
    L7_n = new LinkType(*seven,*seven_n); //blade native link

    o(0) = 0.0; o(1) = 0.; o(2) = 0.2;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * eight;
    eight = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(2)->IdTag());
    LinkType * L8;
    L8 = new LinkType(*five,*eight); //blade link

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * eight_n;
    eight_n = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(2)->IdTag());
    LinkType * L8_n;
    L8_n = new LinkType(*eight,*eight_n); //blade native link

    o(0) = 0.0; o(1) = 0.; o(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * nine;
    nine = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(3)->IdTag());
    LinkType * L9;
    L9 = new LinkType(*five,*nine); //hub link

    o(0) = 0.; o(1) = 0.; o(2) = -0.25;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    JointType * nine_n;
    nine_n = new JointType(o,z,x,JointType::revolute,Rotor0->GetNthPart(3)->IdTag());
    LinkType * L9_n;
    L9_n = new LinkType(*nine,*nine_n); //Hub native link

    ThisScene.addLink(L0);
    L0_index = ThisScene.GetNumLinks();

    C1 = new ChainType;
    C1->AddJoint(zero);
    C1->AddJoint(one);
    C1->AddJoint(two);
    C1->AddJoint(three);
    C1->AddJoint(four);
    C1->AddJoint(five);
    C1->addLink(L1);
    C1->addLink(L2);
    C1->addLink(L3);
    C1->addLink(L5);
    C1->addNativeLink(L4); // nacelle
    C1->addTernLink(2,0); // nacelle
    C1->setParentChain(-L0_index);
    C1->GetNthLink(0)->setQ_pre(0.0);

    ThisScene.AddChain(C1);
    C1_index = ThisScene.GetNumChains()-1;

    C2 = new ChainType;
    C2->AddJoint(five);
    C2->AddJoint(six);
    C2->AddJoint(six_n);
    C2->addLink(L6);
    C2->addNativeLink(L6_n); // blade0
    C2->addTernLink(0,0); // blade0
    C2->setParentChain(C1_index);

    C3 = new ChainType;
    C3->AddJoint(five);
    C3->AddJoint(seven);
    C3->AddJoint(seven_n);
    C3->addLink(L7);
    C3->addNativeLink(L7_n); // blade1
    C3->addTernLink(0,0); // blade1
    C3->setParentChain(C1_index);

    C4 = new ChainType;
    C4->AddJoint(five);
    C4->AddJoint(eight);
    C4->AddJoint(eight_n);
    C4->addLink(L8);
    C4->addNativeLink(L8_n); // blade2
    C4->addTernLink(0,0); // blade2
    C4->setParentChain(C1_index);

    C5  = new ChainType;
    C5->AddJoint(five);
    C5->AddJoint(nine);
    C5->AddJoint(nine_n);
    C5->addLink(L9);
    C5->addNativeLink(L9_n); // hub
    C5->addTernLink(0,0); // hub
    C5->setParentChain(C1_index);

    ThisScene.AddChain(C2);
    ThisScene.AddChain(C3);
    ThisScene.AddChain(C4);
    ThisScene.AddChain(C5);

    yaw(TurbineCtrl->yaw_initial);

    for ( register int i = 0; i < TurbineCtrl->num_blades; i++ ) {
      positionBlade(i,TurbineCtrl->theta[i]);
      pitch(i,TurbineCtrl->pitch_initial[i]);
    }
    rotate(TurbineCtrl->rotation_initial );

    ( mklog() << "Turbine "<<TurbineCtrl->turbine_name<<"specific setup end\n" ).flush();
    return true;
  }


  void yaw(DataType val) {
    C1->GetNthLink(1)->actuate(val,0.,0.,1.);// + 5.0*std::sin(0.5*time*d2r),0,0,1); // (scene Z, scene Y, scene Z!, offset) at top of tower Y = nacelle tilt
  }

  void ontop(SceneType& ThisScene, TurbineControl * TurbineCtrl, DataType offset ) {
    PType o(0.), x(0.), y(0.), z(0.), tmp(0.);

    o(0) = 0.; o(1) = 0.; o(2) = 0.;
    x(0) = 1.; x(1) = 0.; x(2) = 0.;
    z(0) = 0.; z(1) = 0.; z(2) = 1.;
    ( mklog() << "===================  Joint at base of" << TurbineCtrl->tower_name << " ===================\n").flush();
    PType targ;
    targ(0) = TurbineCtrl->xpos; targ(1) = TurbineCtrl->ypos;
    targ(2) = 100.;
    DataType zmin = 0.;
    int flag = 0;

    o = ThisScene.GetNthSceneAssembly( ThisScene.GetNumAssemblies()-1 ).surfacePoint(targ,zmin,flag,offset);
    L0 = ThisScene.GetNthLink(L0_index-1);
    ( mklog() << " PLACING ONTOP " << o << std::endl).flush();
    L0->getChild()->setOrigin(o);
    y = o - ThisScene.GetCoordFrame()->getOrigin();
    y = normalize(y);
    L0->update();

  }

  void positionBlade(int n, DataType val) {
    PType norm;
    norm(0) = 1.;   norm(1) = 0.;  norm(2) = 0.;

    reflect( Blades[n]->GetCPoints(), Blades[n]->GetCPoints_org(), norm );
    std::copy (Blades[n]->GetCPoints_org().begin (), Blades[n]->GetCPoints_org().end (), std::back_inserter(Blades[n]->GetCPoints()) );

    switch (n) {
    case 0 : {
      C2->GetNthLink(0)->actuate(val);
      break;
    }
    case 1 : {
      C3->GetNthLink(0)->actuate(val);
      break;
    }
    case 2 : {
      C4->GetNthLink(0)->actuate(val);
      break;
    }
    }

  }

  void rotate(DataType time, DataType period) {
    C1->GetNthLink(3)->actuate(-(time*DataType(360.0/period)));
  }

  void rotate(DataType val) {
    C1->GetNthLink(3)->actuate(val);
  }


  void pitch(int n, DataType val) {
    switch (n) {
    case 0 : {
      C2->GetNthNative(0)->actuate(val);
      break;
    }
    case 1 : {
      C3->GetNthNative(0)->actuate(val);
      break;
    }
    case 2 : {
      C4->GetNthNative(0)->actuate(val);
      break;
    }
    }

  }

  AssemblyType * GetRotor() { return Rotor0; }

  void response(Turbine * ThisTurbine, TurbineControl * TurbineCtrl, double& t, double& dt, int nsteps, int logevery) {

    PType rotorCenter, arm, ftmp, ratmp;
    DataType rtmp;
    MType mtmp;

    if ( (t-TurbineCtrl->brake_release) < dt) {
      p_rotor = TurbineCtrl->rotation_initial;
      for ( register int i = 0; i < MAXBLADES; i++) {
        p_blades[i] = TurbineCtrl->pitch_initial[i];
      }
    }

    if ( (TurbineCtrl->brake_set > 0 && t >= TurbineCtrl->brake_release) || (TurbineCtrl->brake_set <= 0 ) ) {
#ifdef TURBINE_DEBUG_ON
      if (nsteps % logevery == 0 )
        ( mklog() << "\t" << TurbineCtrl->turbine_name << " FSI response at time = " << t << " , dt = " << dt << std::endl ).flush();
#endif
      rotorCenter(0) = 0.;  rotorCenter(1) = 0.; rotorCenter(2) = 0.;
      for (register int b = 0; b < TurbineCtrl->num_blades; b++) {
        rotorCenter += ThisTurbine->Blades[b]->Centroid();
      }
      rotorCenter *= DataType(1.)/DataType(TurbineCtrl->num_blades);
      T_rotor = 0.;
      for (register int i = 0; i < TurbineCtrl->num_blades; i++) {
        arm = ThisTurbine->Blades[i]->Centroid() - rotorCenter;
        ftmp =  ThisTurbine->Blades[i]->GetNetP();
        T_rotor += cross3D(arm, ftmp);
      }
      ratmp = ThisTurbine->Blades[0]->Centroid() - rotorCenter;
      ratmp = cross3D(arm,ratmp);
      T_rotor = project(T_rotor,ratmp);
      rtmp = projectScalar(T_rotor,ratmp);
      a_rotor = ( rtmp/TurbineCtrl->I_rotor ); //blade1

      if ( std::fabs(a_rotor/d2r) > 2.e3) {
#ifdef TURBINE_LOG_ON
        ( mklog() << "\t _ACCELERATION LIMIT " << a_rotor/d2r << " reset to " << sgn(a_rotor)*2.e3 << std::endl ).flush();
#endif
        a_rotor = sgn(a_rotor)*2.e3*d2r;
      }

      v_rotor += a_rotor*dt;
      p_rotor += (DataType(0.5)*a_rotor*dt*dt + v_rotor*dt)/d2r;

      ThisTurbine->rotate(-p_rotor);

#ifdef MOTION_ONLY
      DataType t_off = t - TurbineCtrl->brake_release;
      ThisTurbine->rotate(TurbineCtrl->rotation_initial+TurbineCtrl->rotation_rate*t_off);
      ThisTurbine->yaw(TurbineCtrl->yaw_initial+TurbineCtrl->yaw_amp*std::sin(TurbineCtrl->yaw_rate*d2r*t_off) );
#endif

      DataType pBlade = 0., vBlade = 0.;

      if ( TurbineCtrl->pitch_control == linear ) {
#ifdef TURBINE_DEBUG_ON
        if (nsteps % logevery == 0 )
          ( mklog() << "\n\t=-linear Pitch Control v_minPitch " << TurbineCtrl->v_minPitch;
#endif
        DataType vRtmp = std::fabs(v_rotor/d2r);
        for (register int i = 0; i < TurbineCtrl->num_blades; i++) {
          if ( vRtmp > TurbineCtrl->v_minPitch ) {
            pBlade = TurbineCtrl->pitch_initial[i] + TurbineCtrl->pitch_rate[i]*(vRtmp-TurbineCtrl->v_minPitch);
#ifdef TURBINE_DEBUG_ON
            if (nsteps % logevery == 0 )
              ( mklog() << "\t\t" << i << " pitching : " << TurbineCtrl->pitch_initial[i]  << " + "
                        << TurbineCtrl->pitch_rate[i] << " * " << (vRtmp-TurbineCtrl->v_minPitch) << " = " << pBlade << std::endl ).flush();
#endif
            ThisTurbine->pitch(i,pBlade);
            p_blades[i] = pBlade;
          }
        }
      }
      if ( TurbineCtrl->pitch_control == relWind ) {
#ifdef TURBINE_DEBUG_ON
        if (nsteps % logevery == 0 )
          ( mklog() << "\t| Relative Wind Pitch Control\n";//v_minPitch " << TurbineCtrl->v_minPitch << std::endl ).flush();
#endif

        DataType wind = TurbineCtrl->wind_avg;//7.0;
        DataType blade_length = TurbineCtrl->blade_length;//13.5;
        DataType blade_speed = blade_length*std::fabs(v_rotor);
        pBlade = 180. - std::atan(wind/blade_speed)/d2r;

        for (register int i = 0; i < TurbineCtrl->num_blades; i++) {
          p_blades[i] = DataType(0.5)*(pBlade + p_blades[i]);
          ThisTurbine->pitch(i,p_blades[i]);

        }

#ifdef TURBINE_DEBUG_ON
        if (nsteps % logevery == 0 )
          ( mklog() << "\t blade tip speed [m/s] " << blade_speed << "\t tip speed ratio " << blade_speed/wind
                    << "\t pitch [deg] : " <<  p_blades[0] << std::endl ).flush();
#endif

      }

      else if ( TurbineCtrl->pitch_control == free ) {
        for (register int i = 0; i < TurbineCtrl->num_blades; i++) {
          // yaw of nacel temporarily neglected
          arm = ThisTurbine->Blades[i]->Centroid() - rotorCenter;
          ftmp = ThisTurbine->Blades[i]->GetMomP();
          rtmp = projectScalar(ftmp, arm);
          a_blades[i] =  rtmp / TurbineCtrl->pitch_I[i];
          v_blades[i] += a_blades[i]*dt;
          p_blades[i] += DataType(0.5)*a_blades[i]*dt*dt + v_blades[i]*dt;
          ThisTurbine->pitch(i,p_blades[i]);
#ifdef TURBINE_DEBUG_ON
          if (nsteps % logevery == 0 )
            ( mklog() << "\t " << i << " ThisTurbine->Blades[i]->GetMomP() " << ThisTurbine->Blades[i]->GetMomP()
                      << " \t arm " << arm << " rtmp " << rtmp << std::endl
                      << "\t\t" << i << " pitching : " << p_blades[i] << std::endl ).flush();
#endif
        }
      }
      else if ( TurbineCtrl->pitch_control == collective ) {
        pBlade = 0.; vBlade = 0.;
        for (register int i = 0; i < TurbineCtrl->num_blades; i++) {

          // yaw of nacel temporarily neglected
          arm = ThisTurbine->Blades[i]->Centroid() - rotorCenter;
          ftmp = ThisTurbine->Blades[i]->GetMomP();
          rtmp = projectScalar(ftmp, arm);
          a_blades[i] = rtmp / TurbineCtrl->pitch_I[i];
          v_blades[i] += a_blades[i]*dt;
          p_blades[i] += (DataType(0.5)*a_blades[i]*dt*dt + v_blades[i]*dt)/d2r;
          vBlade += v_blades[i];
          pBlade += p_blades[i];
#ifdef TURBINE_DEBUG_ON
          ( mklog() << "\t p_blades["<<i<<"] " << p_blades[i] << " Blades["<<i<<"]->GetMomP() " << ThisTurbine->Blades[i]->GetMomP()
                    << " \t arm " << arm << " rtmp " << rtmp << std::endl ).flush();
#endif
        }
        pBlade /= TurbineCtrl->num_blades;
        vBlade /= TurbineCtrl->num_blades;

        for (register int i = 0; i < TurbineCtrl->num_blades; i++) {
          v_blades[i] = vBlade;
          p_blades[i] = pBlade;
          ThisTurbine->pitch(i,pBlade);
        }
#ifdef TURBINE_DEBUG_ON
        ( mklog() << "\n\t\t pitching collectively : " << pBlade << std::endl ).flush();
#endif
      }

      if ( nsteps % logevery == 0 ) {
        char fname[256];
        sprintf(fname,"motion_Kinetics.dat");
        std::ofstream ofs;

        if ( ! fexists(fname) ) { //t < dt ) {
          ofs.open(fname, std::ios::out);
          ofs << "# 1 NSteps\t2 time\t3 TurbineName\t4 a_rotor\t5 v_rotor\6 tp_rotor";
          for (int b=0; b < TurbineCtrl->num_blades; b++) {
            ofs << "\t" << 6+b << " pitch[" << b << "]";
          }
          ofs << std::endl;
        }
        else {
          ofs.open(fname, std::ios::app);
        }
        ofs << nsteps << "\t" << t << "\t" << TurbineCtrl->turbine_name << "\t"
            << a_rotor/d2r << "\t" << v_rotor/d2r << "\t" << p_rotor << "\t";
#ifdef TURBINE_DEBUG_ON
        ( mklog() << "\t " << nsteps << "\t time " << t << "\t" << TurbineCtrl->turbine_name << "\t accel. [deg s^-2] "
                  << a_rotor/d2r << "\t vel. [deg/s] " << v_rotor/d2r << "\t pos. " << p_rotor << "\n";
#endif
        for (int b=0; b < TurbineCtrl->num_blades; b++) {
          ofs << "\t" << p_blades[b];
        }
        ofs << std::endl;
        ofs.close();
      }

      if ( nsteps % logevery == 0 ) {

#ifdef TURBINE_DEBUG_ON
        ( mklog() << "\t " << nsteps << "\t time " << t << "\t" << TurbineCtrl->turbine_name << "\t accel. [deg s^-2] "
                  << a_rotor/d2r << "\t vel. [deg/s] " << v_rotor/d2r << "\t pos. " << p_rotor << "\n";
#endif
      }
    }
  }

  virtual void logKinetics(Turbine * ThisTurbine, TurbineControl * TurbineCtrl, std::ofstream& ofs, DataType time_, int steps ) {

    PType cen, ftmp, axis;

    ofs << "\t\t\"" << TurbineCtrl->turbine_name << "\": {\n";
    ofs << "\t\t\t\"rotor\": {\n\t\t\t\t\"acc\": " << a_rotor/d2r;
    ofs << ", \"vel\": " << v_rotor/d2r;
    ofs << ", \"pos\": " << p_rotor/d2r;
    ofs << ", \"colPitch\": " << p_blades[0] << ",";
    for (int b=0; b < TurbineCtrl->num_blades; b++) {
      cen = ThisTurbine->Blades[b]->Centroid();
      ftmp =  ThisTurbine->Blades[b]->GetNetP();
      axis = ThisTurbine->Blades[b]->GetXaxis();
      ofs << "\n\t\t\t\t\"blade" << b << "\": {\"lift\": " << projectScalar(ftmp,axis);
      axis = ThisTurbine->Blades[b]->GetYaxis();
      ofs << ", \"drag\": " << projectScalar(ftmp,axis);
      axis = ThisTurbine->Blades[b]->GetZaxis();
      ofs << ", \"radial\": " << projectScalar(ftmp,axis);
      ofs << ", \"cenX\": " << cen(0);
      ofs << ", \"cenY\": " << cen(1);
      ofs << ", \"cenZ\": " << cen(2);
      ofs << ", \"ZaxiX\": " << axis(0);
      ofs << ", \"ZaxiY\": " << axis(1);
      ofs << ", \"ZaxiZ\": " << axis(2) << " }";
      if (b<TurbineCtrl->num_blades-1) ofs << ",";
    }
    ofs << "\n\t\t\t},";
    cen = ThisTurbine->Hub0->Centroid();
    ftmp =  ThisTurbine->Hub0->GetNetP();
    axis = ThisTurbine->Hub0->GetXaxis();
    ofs << "\n\t\t\t\"hub\": { \"lateral\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Hub0->GetYaxis();
    ofs << ", \"drag\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Hub0->GetZaxis();
    ofs << ", \"lift\": " << projectScalar(ftmp,axis);
    ofs << ", \"cenX\": " << cen(0);
    ofs << ", \"cenY\": " << cen(1);
    ofs << ", \"cenZ\": " << cen(2);
    ofs << ", \"axiX\": " << axis(0);
    ofs << ", \"axiY\": " << axis(1);
    ofs << ", \"axiZ\": " << axis(2) << " },";
    cen = ThisTurbine->Nacel0->Centroid();
    ftmp =  ThisTurbine->Nacel0->GetNetP();
    axis = ThisTurbine->Nacel0->GetXaxis();
    ofs << "\n\t\t\t\"nacelle\": { \"lateral\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Nacel0->GetYaxis();
    ofs << ", \"drag\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Nacel0->GetZaxis();
    ofs << ", \"lift\": " << projectScalar(ftmp,axis);
    ofs << ", \"cenX\": " << cen(0);
    ofs << ", \"cenY\": " << cen(1);
    ofs << ", \"cenZ\": " << cen(2);
    ofs << ", \"ZaxiX\": " << axis(0);
    ofs << ", \"ZaxiY\": " << axis(1);
    ofs << ", \"ZaxiZ\": " << axis(2) << " },";
    cen = ThisTurbine->Tower0->Centroid();
    ftmp =  ThisTurbine->Tower0->GetNetP();
    axis = ThisTurbine->Tower0->GetXaxis();
    ofs << "\n\t\t\t\"tower\": { \"lateral\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Tower0->GetYaxis();
    ofs << ", \"drag\": " << projectScalar(ftmp,axis);
    axis = ThisTurbine->Tower0->GetZaxis();
    ofs << ", \"lift\": " << projectScalar(ftmp,axis);
    ofs << ", \"cenX\": " << cen(0);
    ofs << ", \"cenY\": " << cen(1);
    ofs << ", \"cenZ\": " << cen(2);
    ofs << ", \"ZaxiX\": " << axis(0);
    ofs << ", \"ZaxiY\": " << axis(1);
    ofs << ", \"ZaxiZ\": " << axis(2) << " }";
    ofs << "\n\t\t}";//\n ]," << std::endl;

  }



  void movement(Turbine * ThisTurbine, TurbineControl * TurbineCtrl, double& t, double& dt) {

#ifdef TURBINE_DEBUG_ON
    ( mklog() << "\n\n*********\tSolidSolverSpecific movement aDVANCE bEGIN\n*********" << std::endl ).flush();
    ( mklog() << " time = " << t << " time*DataType(360.0/6.) = " << t*DataType(360.0/6.)
              << " time*DataType(360.0/6.)*d2r = " << t*DataType(360.0/6.)*d2r << std::endl
              << " dt = " << dt << std::endl ).flush();
#endif

    if ( (TurbineCtrl->brake_set > 0 && t >= TurbineCtrl->brake_release) || (TurbineCtrl->brake_set <= 0 ) ) {
      DataType t_off = t - TurbineCtrl->brake_release;
      ThisTurbine->yaw(TurbineCtrl->yaw_initial+TurbineCtrl->yaw_amp*std::sin(TurbineCtrl->yaw_rate*d2r*t_off) );

      ThisTurbine->rotate(TurbineCtrl->rotation_initial+TurbineCtrl->rotation_rate*t_off);

    }

#ifdef TURBINE_DEBUG_ON
    ( mklog() << "\n\n*********\tSolidSolverSpecific movement aDVANCE fINISHED\n*********" << std::endl ).flush();
#endif

  }


  virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nRestarting TURBINEMECHANISM SOLVER t= " << t << " dt = " << dt << " pos = " << pos << std::endl ).flush();
#endif

    L0->Restart(ifs,pos,t,dt);
    pos = ifs.tellg();
    C1->Restart(ifs,pos,t,dt);
    pos = ifs.tellg();
    C2->Restart(ifs,pos,t,dt);
    pos = ifs.tellg();
    C3->Restart(ifs,pos,t,dt);
    pos = ifs.tellg();
    C4->Restart(ifs,pos,t,dt);
    pos = ifs.tellg();

    ifs.read((char*)&L0_index,sizeof(int));
    ifs.read((char*)&C1_index,sizeof(int));

    ifs.read((char*)&T_rotor(0),sizeof(DataType));
    ifs.read((char*)&T_rotor(1),sizeof(DataType));
    ifs.read((char*)&T_rotor(2),sizeof(DataType));

    ifs.read((char*)&T_nacelle(0),sizeof(DataType));
    ifs.read((char*)&T_nacelle(1),sizeof(DataType));
    ifs.read((char*)&T_nacelle(2),sizeof(DataType));

    ifs.read((char*)&a_rotor,sizeof(DataType));
    ifs.read((char*)&v_rotor,sizeof(DataType));
    ifs.read((char*)&p_rotor,sizeof(DataType));

    ifs.read((char*)&a_nacelle,sizeof(DataType));
    ifs.read((char*)&v_nacelle,sizeof(DataType));
    ifs.read((char*)&p_nacelle,sizeof(DataType));

    pos = ifs.tellg();
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nDone Restarting TURBINEMECHANISM t = " << t << " dt = " << dt << " pos = " << pos << std::endl ).flush();
#endif
  }

  virtual void Checkpointing(std::ofstream& ofs) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nCHECKPOINTING TURBINEMECHANISM SOLVER pos = " << ofs.tellp() << std::endl ).flush();
#endif

    L0->Checkpointing(ofs);
    C1->Checkpointing(ofs);
    C2->Checkpointing(ofs);
    C3->Checkpointing(ofs);
    C4->Checkpointing(ofs);

    ofs.write((char*)&L0_index,sizeof(int));
    ofs.write((char*)&C1_index,sizeof(int));

    ofs.write((char*)&T_rotor(0),sizeof(DataType));
    ofs.write((char*)&T_rotor(1),sizeof(DataType));
    ofs.write((char*)&T_rotor(2),sizeof(DataType));

    ofs.write((char*)&T_nacelle(0),sizeof(DataType));
    ofs.write((char*)&T_nacelle(1),sizeof(DataType));
    ofs.write((char*)&T_nacelle(2),sizeof(DataType));

    ofs.write((char*)&a_rotor,sizeof(DataType));
    ofs.write((char*)&v_rotor,sizeof(DataType));
    ofs.write((char*)&p_rotor,sizeof(DataType));

    ofs.write((char*)&a_nacelle,sizeof(DataType));
    ofs.write((char*)&v_nacelle,sizeof(DataType));
    ofs.write((char*)&p_nacelle,sizeof(DataType));

#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\n+++ CHECKPOINTING TURBINEMECHANISM COMPLETE pos = " << ofs.tellp() <<std::endl ).flush();
#endif
  }

protected:

  AssemblyType * Turbine0pt;
  AssemblyType * Rotor0;
  LoftType * Tower0;
  LoftType * Nacel0;
  LoftType * Blades[MAXBLADES];
  LoftType * Hub0;
  LinkType * L0;

  ChainType * C1;
  ChainType * C2;
  ChainType * C3;
  ChainType * C4;
  ChainType * C5;

  int L0_index, C1_index;

  PType T_rotor, T_nacelle;

  DataType a_rotor, a_nacelle;
  DataType v_rotor, v_nacelle;
  DataType p_rotor, p_nacelle;

  DataType a_blades[MAXBLADES];
  DataType v_blades[MAXBLADES];
  DataType p_blades[MAXBLADES];

};

#endif

---