source: flair-src/trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.cpp@ 441

Last change on this file since 441 was 435, checked in by Sanahuja Guillaume, 3 years ago

add logs to ugv

File size: 7.7 KB
Line 
1// created: 2020/12/21
2// filename: SimpleFleet.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: demo simple fleet avec optitrack
10//
11//
12/*********************************************************************/
13
14#include "SimpleFleet.h"
15#include <TargetController.h>
16#include <GridLayout.h>
17#include <PushButton.h>
18#include <DataPlot1D.h>
19#include <DataPlot2D.h>
20#include <FrameworkManager.h>
21#include <VrpnClient.h>
22#include <MetaVrpnObject.h>
23#include <TrajectoryGenerator2DCircle.h>
24#include <Matrix.h>
25#include <Tab.h>
26#include <TabWidget.h>
27#include <DoubleSpinBox.h>
28#include <Pid.h>
29#include <Quaternion.h>
30#include <Euler.h>
31#include <Ugv.h>
32#include <UgvControls.h>
33#include <math.h>
34#include <UdpSocket.h>
35#include <string.h>
36
37using namespace std;
38using namespace flair::core;
39using namespace flair::gui;
40using namespace flair::sensor;
41using namespace flair::filter;
42using namespace flair::meta;
43using namespace flair::actuator;
44
45SimpleFleet::SimpleFleet(string name,string broadcast,TargetController *controller): Thread(getFrameworkManager(),"SimpleFleet",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
46 this->controller=controller;
47 controller->Start();
48
49 Ugv* ugv=GetUgv();
50 ugv->UseDefaultPlot();
51
52 VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
53 ugvVrpn = new MetaVrpnObject(name);
54
55 getFrameworkManager()->AddDeviceToLog(ugvVrpn);
56 vrpnclient->Start();
57
58 Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
59 GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
60 button_kill = new PushButton(buttonslayout->NewRow(), "kill");
61 startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
62 stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
63 button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
64 button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
65
66 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
67 ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
68 ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
69 ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
70 ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
71 ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
72
73 xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
74 yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
75
76 Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
77 TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
78 Tab *setupLawTab = new Tab(tabWidget, "Setup");
79 Tab *graphLawTab = new Tab(tabWidget, "Graphes");
80 uX=new Pid(setupLawTab->At(1,0),"u_x");
81 uX->UseDefaultPlot(graphLawTab->NewRow());
82 uY=new Pid(setupLawTab->At(1,1),"u_y");
83 uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
84
85 getFrameworkManager()->AddDeviceToLog(uX);
86 getFrameworkManager()->AddDeviceToLog(uY);
87
88 l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
89
90 message=new UdpSocket(ugv,"Message",broadcast,true);
91}
92
93SimpleFleet::~SimpleFleet() {
94}
95
96void SimpleFleet::Run(void) {
97 WarnUponSwitches(true);
98 SetPeriodMS(20);
99
100 if (getFrameworkManager()->ErrorOccured() == true) {
101 SafeStop();
102 }
103
104 while (!ToBeStopped()) {
105 SecurityCheck();
106 CheckMessages();
107 CheckJoystick();
108 CheckPushButton();
109
110 if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
111 if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
112 WaitPeriod();
113 }
114}
115
116void SimpleFleet::CheckPushButton(void) {
117 if (button_start_log->Clicked() == true)
118 getFrameworkManager()->StartLog();
119 if (button_stop_log->Clicked() == true)
120 getFrameworkManager()->StopLog();
121
122 if (startCircle->Clicked() == true)
123 StartCircle();
124
125 if (stopCircle->Clicked() == true)
126 StopCircle();
127
128 if (button_kill->Clicked() == true)
129 SafeStop();
130}
131
132void SimpleFleet::CheckJoystick(void) {
133 //R1 and Circle
134 if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
135 StartCircle();
136 }
137
138 //R1 and Cross
139 if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
140 StopCircle();
141 }
142}
143
144void SimpleFleet::SecurityCheck(void) {
145 if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
146 if (!ugvVrpn->IsTracked(500)) {
147 Thread::Err("VRPN, ugv lost\n");
148 vrpnLost=true;
149 StopCircle();
150 }
151 }
152}
153
154void SimpleFleet::CheckMessages(void) {
155 char msg[64];
156 char src[64];
157 size_t src_size=sizeof(src);
158 while(message->RecvMessage(msg,sizeof(msg),TIME_NONBLOCK,src,&src_size)>0) {
159 //printf("%s %s\n",GetUav()->ObjectName().c_str(),src);
160 if(strcmp(src,GetUgv()->ObjectName().c_str())!=0) {
161 if(strcmp(msg,"StartCircle")==0) {
162 Printf("StartCircle fleet\n");
163 StartCircle();
164 }
165 if(strcmp(msg,"StopCircle")==0) {
166 Printf("StopCircle fleet\n");
167 StopCircle();
168 }
169 }
170 }
171}
172
173void SimpleFleet::ComputeManualControls(void) {
174 float speed=-controller->GetAxisValue(3);
175 float turn=controller->GetAxisValue(0);
176 GetUgv()->GetUgvControls()->SetControls(speed,turn);
177}
178
179void SimpleFleet::ComputeCircleControls(void) {
180
181 Vector3Df ugv_pos,ugv_vel; // in VRPN coordinate system
182 Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
183 Vector2Df pos_error,vel_error;
184 Vector2Df circle_pos,circle_vel;
185
186 ugvVrpn->GetPosition(ugv_pos);
187 ugvVrpn->GetSpeed(ugv_vel);
188
189 ugv_pos.To2Dxy(ugv_2Dpos);
190 ugv_vel.To2Dxy(ugv_2Dvel);
191
192 target_2Dpos.x=xCircleCenter->Value();
193 target_2Dpos.y=yCircleCenter->Value();
194 circle->SetCenter(target_2Dpos);
195
196 //circle reference
197 circle->Update(GetTime());
198 circle->GetPosition(circle_pos);
199 circle->GetSpeed(circle_vel);
200
201 //error in optitrack frame
202 pos_error=ugv_2Dpos-circle_pos;
203 vel_error=ugv_2Dvel-circle_vel;
204
205 uX->SetValues(pos_error.x, vel_error.x);
206 uX->Update(GetTime());
207 uY->SetValues(pos_error.y, vel_error.y);
208 uY->Update(GetTime());
209
210 //get yaw from vrpn
211 Quaternion vrpnQuaternion;
212 ugvVrpn->GetQuaternion(vrpnQuaternion);
213 float yaw=vrpnQuaternion.ToEuler().yaw;
214 float L=1;
215 float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
216 float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
217 GetUgv()->GetUgvControls()->SetControls(-v,-w);
218}
219
220
221void SimpleFleet::StartCircle(void) {
222 if(behaviourMode!=BehaviourMode_t::Circle) {
223 Vector3Df ugv_pos;
224 Vector2Df ugv_2Dpos,target_2Dpos;
225
226 target_2Dpos.x=xCircleCenter->Value();
227 target_2Dpos.y=yCircleCenter->Value();
228 circle->SetCenter(target_2Dpos);
229
230 ugvVrpn->GetPosition(ugv_pos);
231 ugv_pos.To2Dxy(ugv_2Dpos);
232 circle->StartTraj(ugv_2Dpos);
233
234 uX->Reset();
235 uY->Reset();
236 behaviourMode=BehaviourMode_t::Circle;
237 Thread::Info("SimpleFleet: start circle\n");
238 message->SendMessage("StartCircle");
239 }
240}
241
242void SimpleFleet::StopCircle(void) {
243 if(behaviourMode==BehaviourMode_t::Circle) {
244 circle->FinishTraj();
245 //GetJoystick()->Rumble(0x70);
246 behaviourMode=BehaviourMode_t::Manual;
247 Thread::Info("SimpleFleet: finishing circle\n");
248 message->SendMessage("StopCircle");
249 }
250}
251
Note: See TracBrowser for help on using the repository browser.