source: flair-src/trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp@ 372

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2020/11/20
//  filename:   TwoWheelRobot.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe definissant un TwoWheelRobot
//
/*********************************************************************/

#include "TwoWheelRobot.h"
#include <TabWidget.h>
#include <Tab.h>
#include <DoubleSpinBox.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <math.h>
#ifdef GL
#include <ISceneManager.h>
#include <IMeshManipulator.h>
#include "MeshSceneNode.h"
#include "Gui.h"
#include <Mutex.h>
#endif

#define G (float)9.81 // gravity ( N/(m/s²) )

#ifdef GL
using namespace irr::video;
using namespace irr::scene;
using namespace irr::core;
#endif
using namespace flair::core;
using namespace flair::gui;
using namespace flair::actuator;

namespace flair {
namespace simulator {

TwoWheelRobot::TwoWheelRobot(std::string name, uint32_t modelId)
    : Model(name,modelId) {
  Tab *setup_tab = new Tab(GetTabWidget(), "model");
  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2);
  size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1);
  
  
  Tab *visual_tab = new Tab(GetTabWidget(), "visual");
  bodyColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
  bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
  bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
  
  SetIsReady(true);
}

TwoWheelRobot::~TwoWheelRobot() {
  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
  // pales) par parenté
}

#ifdef GL

void TwoWheelRobot::Draw(void) {
  // create unite (1m=100cm) robot; scale will be adapted according to settings in gcs
  // parameter
  // note that the frame used is irrlicht one:
  // left handed, North East Up
  const IGeometryCreator *geo;
  geo = getGui()->getSceneManager()->getGeometryCreator();

  colored_body = geo->createCubeMesh(vector3df(100,100,100));
  MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
  
  IMesh *wheel = geo->createCylinderMesh(35, 10, 64, SColor(0, 0, 0, 0));
  MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
  MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
  ExtraDraw();
}

void TwoWheelRobot::AnimateModel(void) {
  if (bodyColorR->ValueChanged() == true || bodyColorG->ValueChanged() == true || bodyColorB->ValueChanged() == true) {
    getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_body, SColor(0,bodyColorR->Value(), bodyColorG->Value(), bodyColorB->Value()));
  }

  // adapt robot size
  if (size->ValueChanged() == true) {
    setScale(size->Value());
  }
   
}

size_t TwoWheelRobot::dbtSize(void) const {
  return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
}

void TwoWheelRobot::WritedbtBuf(
    char *dbtbuf) { /*
                       float *buf=(float*)dbtbuf;
                       vector3df vect=getPosition();
                       memcpy(buf,&vect.X,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Y,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Z,sizeof(float));
                       buf++;
                       vect=getRotation();
                       memcpy(buf,&vect.X,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Y,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Z,sizeof(float));
                       buf++;
                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
}

void TwoWheelRobot::ReaddbtBuf(
    char *dbtbuf) { /*
                       float *buf=(float*)dbtbuf;
                       vector3df vect;
                       memcpy(&vect.X,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Y,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Z,buf,sizeof(float));
                       buf++;
                       setPosition(vect);
                       memcpy(&vect.X,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Y,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Z,buf,sizeof(float));
                       buf++;
                       ((ISceneNode*)(this))->setRotation(vect);
                       memcpy(&motors,buf,sizeof(rtsimu_motors));
                       AnimateModele();*/
}
#endif // GL

// states are computed on fixed frame NED
// x north
// y east
// z down
void TwoWheelRobot::CalcModel(void) {
  state[0].Pos.x=state[-1].Pos.x;
  state[0].Pos.y=state[-1].Pos.y;
  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
 
  //Printf("%f %f %f\n",state[0].Pos.x,state[-1].Pos.x,state[-2].Pos.x);
  /*
  ** ===================================================================
  **     z double integrator
  **
  ** ===================================================================
  */
  state[0].Pos.z =
      (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
//Printf("%f %f %f\n",state[0].Pos.z,state[-1].Pos.z,state[-2].Pos.z);
#ifndef GL
  if (state[0].Pos.z < 0)
    state[0].Pos.z = 0;
#endif
  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
}

} // end namespace simulator
} // end namespace flair