source: flair-src/trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp

Last change on this file was 435, checked in by Sanahuja Guillaume, 3 months ago

add logs to ugv

  • Property svn:eol-style set to native
File size: 6.9 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    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    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
74    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
75    Tab *setupLawTab = new Tab(tabWidget, "Setup");
76    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
77    uX=new Pid(setupLawTab->At(1,0),"u_x");
78    uX->UseDefaultPlot(graphLawTab->NewRow());
79    uY=new Pid(setupLawTab->At(1,1),"u_y");
80    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
81   
82    getFrameworkManager()->AddDeviceToLog(uX);
83    getFrameworkManager()->AddDeviceToLog(uY);
84   
85    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
86}
87
88CircleFollower::~CircleFollower() {
89}
90
91void CircleFollower::Run(void) {
92    WarnUponSwitches(true);
93    SetPeriodMS(20);
94   
95    if (getFrameworkManager()->ErrorOccured() == true) {
96        SafeStop();
97    }
98
99    while (!ToBeStopped()) {
100        SecurityCheck();
101        CheckJoystick();
102        CheckPushButton();
103       
104        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
105        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
106        WaitPeriod();
107    }
108}
109
110void CircleFollower::CheckPushButton(void) {
111  if (button_start_log->Clicked() == true)
112    getFrameworkManager()->StartLog();
113  if (button_stop_log->Clicked() == true)
114    getFrameworkManager()->StopLog();
115 
116  if (startCircle->Clicked() == true)
117      StartCircle();
118       
119  if (stopCircle->Clicked() == true)
120      StopCircle();
121
122  if (button_kill->Clicked() == true)
123      SafeStop();
124}
125
126void CircleFollower::CheckJoystick(void) {
127  //R1 and Circle
128  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
129      StartCircle();
130  }
131
132  //R1 and Cross
133  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
134      StopCircle();
135  }
136}
137
138void CircleFollower::SecurityCheck(void) {
139    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
140        if (!targetVrpn->IsTracked(500)) {
141            Thread::Err("VRPN, target lost\n");
142            vrpnLost=true;
143            StopCircle();
144        }
145        if (!ugvVrpn->IsTracked(500)) {
146            Thread::Err("VRPN, ugv lost\n");
147            vrpnLost=true;
148            StopCircle();
149        }
150    }
151}
152
153void CircleFollower::ComputeManualControls(void) {
154  float speed=-controller->GetAxisValue(3);
155  float turn=controller->GetAxisValue(0);
156  GetUgv()->GetUgvControls()->SetControls(speed,turn);
157}
158
159void CircleFollower::ComputeCircleControls(void) {
160
161  Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
162  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
163  Vector2Df pos_error,vel_error;
164  Vector2Df circle_pos,circle_vel;
165   
166  ugvVrpn->GetPosition(ugv_pos);
167  ugvVrpn->GetSpeed(ugv_vel);
168
169  ugv_pos.To2Dxy(ugv_2Dpos);
170  ugv_vel.To2Dxy(ugv_2Dvel);
171 
172  targetVrpn->GetPosition(target_pos);
173  target_pos.To2Dxy(target_2Dpos);
174  circle->SetCenter(target_2Dpos);
175
176  //circle reference
177  circle->Update(GetTime());
178  circle->GetPosition(circle_pos);
179  circle->GetSpeed(circle_vel);
180
181  //error in optitrack frame
182  pos_error=ugv_2Dpos-circle_pos;
183  vel_error=ugv_2Dvel-circle_vel;
184   
185  uX->SetValues(pos_error.x, vel_error.x);
186  uX->Update(GetTime());
187  uY->SetValues(pos_error.y, vel_error.y);
188  uY->Update(GetTime());
189 
190  //get yaw from vrpn
191  Quaternion vrpnQuaternion;
192  ugvVrpn->GetQuaternion(vrpnQuaternion);
193  float yaw=vrpnQuaternion.ToEuler().yaw;
194  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
195  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
196  GetUgv()->GetUgvControls()->SetControls(-v,-w);
197}
198
199
200void CircleFollower::StartCircle(void) {
201  if(behaviourMode!=BehaviourMode_t::Circle) {
202    Vector3Df ugv_pos,target_pos;
203    Vector2Df ugv_2Dpos,target_2Dpos;
204
205    targetVrpn->GetPosition(target_pos);
206    target_pos.To2Dxy(target_2Dpos);
207    circle->SetCenter(target_2Dpos);
208
209    ugvVrpn->GetPosition(ugv_pos);
210    ugv_pos.To2Dxy(ugv_2Dpos);
211    circle->StartTraj(ugv_2Dpos);
212
213    uX->Reset();
214    uY->Reset();
215    behaviourMode=BehaviourMode_t::Circle;
216    Thread::Info("CircleFollower: start circle\n");
217  }
218}
219
220void CircleFollower::StopCircle(void) {
221        if(behaviourMode==BehaviourMode_t::Circle) {
222    circle->FinishTraj();
223    //GetJoystick()->Rumble(0x70);
224    behaviourMode=BehaviourMode_t::Manual;
225    Thread::Info("CircleFollower: finishing circle\n");
226  }
227}
228
Note: See TracBrowser for help on using the repository browser.