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

Last change on this file since 372 was 372, checked in by Sanahuja Guillaume, 11 months ago

add twowheelrobot

File size: 5.7 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2020/11/20
6//  filename:   TwoWheelRobot.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    classe definissant un TwoWheelRobot
14//
15/*********************************************************************/
16
17#include "TwoWheelRobot.h"
18#include <TabWidget.h>
19#include <Tab.h>
20#include <DoubleSpinBox.h>
21#include <SpinBox.h>
22#include <GroupBox.h>
23#include <math.h>
24#ifdef GL
25#include <ISceneManager.h>
26#include <IMeshManipulator.h>
27#include "MeshSceneNode.h"
28#include "Gui.h"
29#include <Mutex.h>
30#endif
31
32#define G (float)9.81 // gravity ( N/(m/s²) )
33
34#ifdef GL
35using namespace irr::video;
36using namespace irr::scene;
37using namespace irr::core;
38#endif
39using namespace flair::core;
40using namespace flair::gui;
41using namespace flair::actuator;
42
43namespace flair {
44namespace simulator {
45
46TwoWheelRobot::TwoWheelRobot(std::string name, uint32_t modelId)
47    : Model(name,modelId) {
48  Tab *setup_tab = new Tab(GetTabWidget(), "model");
49  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2);
50  size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1);
51 
52 
53  Tab *visual_tab = new Tab(GetTabWidget(), "visual");
54  bodyColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
55  bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
56  bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
57 
58  SetIsReady(true);
59}
60
61TwoWheelRobot::~TwoWheelRobot() {
62  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
63  // pales) par parenté
64}
65
66#ifdef GL
67
68void TwoWheelRobot::Draw(void) {
69  // create unite (1m=100cm) robot; scale will be adapted according to settings in gcs
70  // parameter
71  // note that the frame used is irrlicht one:
72  // left handed, North East Up
73  const IGeometryCreator *geo;
74  geo = getGui()->getSceneManager()->getGeometryCreator();
75
76  colored_body = geo->createCubeMesh(vector3df(100,100,100));
77  MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
78 
79  IMesh *wheel = geo->createCylinderMesh(35, 10, 64, SColor(0, 0, 0, 0));
80  MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
81  MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
82  ExtraDraw();
83}
84
85void TwoWheelRobot::AnimateModel(void) {
86  if (bodyColorR->ValueChanged() == true || bodyColorG->ValueChanged() == true || bodyColorB->ValueChanged() == true) {
87    getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_body, SColor(0,bodyColorR->Value(), bodyColorG->Value(), bodyColorB->Value()));
88  }
89
90  // adapt robot size
91  if (size->ValueChanged() == true) {
92    setScale(size->Value());
93  }
94   
95}
96
97size_t TwoWheelRobot::dbtSize(void) const {
98  return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
99}
100
101void TwoWheelRobot::WritedbtBuf(
102    char *dbtbuf) { /*
103                       float *buf=(float*)dbtbuf;
104                       vector3df vect=getPosition();
105                       memcpy(buf,&vect.X,sizeof(float));
106                       buf++;
107                       memcpy(buf,&vect.Y,sizeof(float));
108                       buf++;
109                       memcpy(buf,&vect.Z,sizeof(float));
110                       buf++;
111                       vect=getRotation();
112                       memcpy(buf,&vect.X,sizeof(float));
113                       buf++;
114                       memcpy(buf,&vect.Y,sizeof(float));
115                       buf++;
116                       memcpy(buf,&vect.Z,sizeof(float));
117                       buf++;
118                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
119}
120
121void TwoWheelRobot::ReaddbtBuf(
122    char *dbtbuf) { /*
123                       float *buf=(float*)dbtbuf;
124                       vector3df vect;
125                       memcpy(&vect.X,buf,sizeof(float));
126                       buf++;
127                       memcpy(&vect.Y,buf,sizeof(float));
128                       buf++;
129                       memcpy(&vect.Z,buf,sizeof(float));
130                       buf++;
131                       setPosition(vect);
132                       memcpy(&vect.X,buf,sizeof(float));
133                       buf++;
134                       memcpy(&vect.Y,buf,sizeof(float));
135                       buf++;
136                       memcpy(&vect.Z,buf,sizeof(float));
137                       buf++;
138                       ((ISceneNode*)(this))->setRotation(vect);
139                       memcpy(&motors,buf,sizeof(rtsimu_motors));
140                       AnimateModele();*/
141}
142#endif // GL
143
144// states are computed on fixed frame NED
145// x north
146// y east
147// z down
148void 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 
154  //Printf("%f %f %f\n",state[0].Pos.x,state[-1].Pos.x,state[-2].Pos.x);
155  /*
156  ** ===================================================================
157  **     z double integrator
158  **
159  ** ===================================================================
160  */
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
169  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
170}
171
172} // end namespace simulator
173} // end namespace flair
Note: See TracBrowser for help on using the repository browser.