source: flair-src/tags/latest/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp@ 411

Last change on this file since 411 was 389, checked in by Sanahuja Guillaume, 17 months ago

update demo code

  • Property svn:eol-style set to native
File size: 6.5 KB
Line 
1// created: 2020/12/09
2// filename: CircleFollower.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: demo cercle avec optitrack
10//
11//
12/*********************************************************************/
13
14#include "CircleFollower.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
35using namespace std;
36using namespace flair::core;
37using namespace flair::gui;
38using namespace flair::sensor;
39using namespace flair::filter;
40using namespace flair::meta;
41using namespace flair::actuator;
42
43CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
44 this->controller=controller;
45 controller->Start();
46
47 Ugv* ugv=GetUgv();
48 ugv->UseDefaultPlot();
49
50 VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
51 ugvVrpn = new MetaVrpnObject(name);
52 targetVrpn=new MetaVrpnObject("target");
53
54 getFrameworkManager()->AddDeviceToLog(ugvVrpn);
55 getFrameworkManager()->AddDeviceToLog(targetVrpn);
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
64 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
65 ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
66 ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
67 ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
68 ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
69 ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
70
71 Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
72 TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
73 Tab *setupLawTab = new Tab(tabWidget, "Setup");
74 Tab *graphLawTab = new Tab(tabWidget, "Graphes");
75 uX=new Pid(setupLawTab->At(1,0),"u_x");
76 uX->UseDefaultPlot(graphLawTab->NewRow());
77 uY=new Pid(setupLawTab->At(1,1),"u_y");
78 uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
79
80 l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
81}
82
83CircleFollower::~CircleFollower() {
84}
85
86void CircleFollower::Run(void) {
87 WarnUponSwitches(true);
88 SetPeriodMS(20);
89
90 if (getFrameworkManager()->ErrorOccured() == true) {
91 SafeStop();
92 }
93
94 while (!ToBeStopped()) {
95 SecurityCheck();
96 CheckJoystick();
97 CheckPushButton();
98
99 if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
100 if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
101 WaitPeriod();
102 }
103}
104
105void CircleFollower::CheckPushButton(void) {
106 if (startCircle->Clicked() == true)
107 StartCircle();
108
109 if (stopCircle->Clicked() == true)
110 StopCircle();
111
112 if (button_kill->Clicked() == true)
113 SafeStop();
114}
115
116void CircleFollower::CheckJoystick(void) {
117 //R1 and Circle
118 if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
119 StartCircle();
120 }
121
122 //R1 and Cross
123 if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
124 StopCircle();
125 }
126}
127
128void CircleFollower::SecurityCheck(void) {
129 if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
130 if (!targetVrpn->IsTracked(500)) {
131 Thread::Err("VRPN, target lost\n");
132 vrpnLost=true;
133 StopCircle();
134 }
135 if (!ugvVrpn->IsTracked(500)) {
136 Thread::Err("VRPN, ugv lost\n");
137 vrpnLost=true;
138 StopCircle();
139 }
140 }
141}
142
143void CircleFollower::ComputeManualControls(void) {
144 float speed=-controller->GetAxisValue(3);
145 float turn=controller->GetAxisValue(0);
146 GetUgv()->GetUgvControls()->SetControls(speed,turn);
147}
148
149void CircleFollower::ComputeCircleControls(void) {
150
151 Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
152 Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
153 Vector2Df pos_error,vel_error;
154 Vector2Df circle_pos,circle_vel;
155
156 ugvVrpn->GetPosition(ugv_pos);
157 ugvVrpn->GetSpeed(ugv_vel);
158
159 ugv_pos.To2Dxy(ugv_2Dpos);
160 ugv_vel.To2Dxy(ugv_2Dvel);
161
162 targetVrpn->GetPosition(target_pos);
163 target_pos.To2Dxy(target_2Dpos);
164 circle->SetCenter(target_2Dpos);
165
166 //circle reference
167 circle->Update(GetTime());
168 circle->GetPosition(circle_pos);
169 circle->GetSpeed(circle_vel);
170
171 //error in optitrack frame
172 pos_error=ugv_2Dpos-circle_pos;
173 vel_error=ugv_2Dvel-circle_vel;
174
175 uX->SetValues(pos_error.x, vel_error.x);
176 uX->Update(GetTime());
177 uY->SetValues(pos_error.y, vel_error.y);
178 uY->Update(GetTime());
179
180 //get yaw from vrpn
181 Quaternion vrpnQuaternion;
182 ugvVrpn->GetQuaternion(vrpnQuaternion);
183 float yaw=vrpnQuaternion.ToEuler().yaw;
184 float L=1;
185 float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
186 float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
187 GetUgv()->GetUgvControls()->SetControls(-v,-w);
188}
189
190
191void CircleFollower::StartCircle(void) {
192 if(behaviourMode!=BehaviourMode_t::Circle) {
193 Vector3Df ugv_pos,target_pos;
194 Vector2Df ugv_2Dpos,target_2Dpos;
195
196 targetVrpn->GetPosition(target_pos);
197 target_pos.To2Dxy(target_2Dpos);
198 circle->SetCenter(target_2Dpos);
199
200 ugvVrpn->GetPosition(ugv_pos);
201 ugv_pos.To2Dxy(ugv_2Dpos);
202 circle->StartTraj(ugv_2Dpos);
203
204 uX->Reset();
205 uY->Reset();
206 behaviourMode=BehaviourMode_t::Circle;
207 Thread::Info("CircleFollower: start circle\n");
208 }
209}
210
211void CircleFollower::StopCircle(void) {
212 if(behaviourMode==BehaviourMode_t::Circle) {
213 circle->FinishTraj();
214 //GetJoystick()->Rumble(0x70);
215 behaviourMode=BehaviourMode_t::Manual;
216 Thread::Info("CircleFollower: finishing circle\n");
217 }
218}
219
Note: See TracBrowser for help on using the repository browser.