Ignore:
Timestamp:
Dec 9, 2020, 2:32:49 PM (10 months ago)
Author:
Sanahuja Guillaume
Message:

update ugv

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp

    r372 r375  
    2222#include <GroupBox.h>
    2323#include <math.h>
     24#include <SimuUgvControls.h>
    2425#ifdef GL
    2526#include <ISceneManager.h>
     
    5657  bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
    5758 
     59  controls = new SimuUgvControls(this, name, modelId,0);
     60 
    5861  SetIsReady(true);
    5962}
    6063
    6164TwoWheelRobot::~TwoWheelRobot() {
    62   // les objets irrlicht seront automatiquement detruits (moteurs, helices,
    63   // pales) par parenté
     65  // les objets irrlicht seront automatiquement detruits par parenté
    6466}
    6567
     
    7577
    7678  colored_body = geo->createCubeMesh(vector3df(100,100,100));
     79  colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
    7780  MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
    7881 
     
    8083  MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
    8184  MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
    82   ExtraDraw();
     85 
     86 ExtraDraw();
    8387}
    8488
     
    147151// z down
    148152void TwoWheelRobot::CalcModel(void) {
    149   state[0].Pos.x=state[-1].Pos.x;
    150   state[0].Pos.y=state[-1].Pos.y;
    151   state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
    152   state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     153  float speed=0.4, turn=0;
     154  Time motorTime;
     155
     156  controls->GetControls(&speed,&turn,&motorTime);
     157         
     158  // compute quaternion from W
     159  // Quaternion derivative: dQ = 0.5*(  Q*Qw)
     160  state[0].W.x=0;
     161  state[0].W.y=0;
     162  state[0].W.z=turn;
     163  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     164
     165  // Quaternion integration
     166  state[0].Quat = state[-1].Quat + dQ * dT();
     167  state[0].Quat.Normalize();
     168
     169  Vector3D<double> dir = Vector3D<double>(speed,0,0);
     170  dir.Rotate(state[0].Quat);
     171  state[0].Pos = state[-1].Pos + dT() * dir;
     172 
    153173 
    154   //Printf("%f %f %f\n",state[0].Pos.x,state[-1].Pos.x,state[-2].Pos.x);
    155174  /*
    156175  ** ===================================================================
     
    159178  ** ===================================================================
    160179  */
    161   state[0].Pos.z =
    162       (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
    163   state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    164 //Printf("%f %f %f\n",state[0].Pos.z,state[-1].Pos.z,state[-2].Pos.z);
    165 #ifndef GL
    166   if (state[0].Pos.z < 0)
    167     state[0].Pos.z = 0;
    168 #endif
     180  state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
    169181  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    170182}
Note: See TracChangeset for help on using the changeset viewer.