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

Last change on this file since 372 was 372, checked in by Sanahuja Guillaume, 2 years 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.