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

Last change on this file since 379 was 379, checked in by Sanahuja Guillaume, 12 months ago

modifs ugv

  • Property svn:eol-style set to native
File size: 6.3 KB
RevLine 
[376]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>
[379]26#include <TabWidget.h>
27#include <DoubleSpinBox.h>
[376]28#include <Pid.h>
[379]29#include <Quaternion.h>
30#include <Euler.h>
[377]31#include <Ugv.h>
32#include <UgvControls.h>
[379]33#include <math.h>
[376]34
35using namespace std;
36using namespace flair::core;
37using namespace flair::gui;
38using namespace flair::sensor;
39using namespace flair::filter;
40using namespace flair::meta;
[377]41using namespace flair::actuator;
[376]42
[379]43CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
44    this->controller=controller;
[376]45    controller->Start();
46   
[377]47    Ugv* ugv=GetUgv();
48    ugv->UseDefaultPlot();
49   
50    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
51   
[376]52    if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
[377]53        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
[376]54        targetVrpn=new MetaVrpnObject("target",1);
55    } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
[377]56        ugvVrpn = new MetaVrpnObject(name);
[376]57        targetVrpn=new MetaVrpnObject("target");
58    }
59   
[377]60    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
[376]61    getFrameworkManager()->AddDeviceToLog(targetVrpn);
62    vrpnclient->Start();
63       
[379]64    Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
[376]65    GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
[379]66    button_kill = new PushButton(buttonslayout->NewRow(), "kill");
[376]67    startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
68    stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
69   
70    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
[377]71    ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
72    ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
73    ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
74    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
75    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
[379]76
77    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
78    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
79    Tab *setupLawTab = new Tab(tabWidget, "Setup");
80    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
[376]81    uX=new Pid(setupLawTab->At(1,0),"u_x");
82    uX->UseDefaultPlot(graphLawTab->NewRow());
83    uY=new Pid(setupLawTab->At(1,1),"u_y");
[379]84    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
85   
86    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
[376]87}
88
89CircleFollower::~CircleFollower() {
90}
91
92void CircleFollower::Run(void) {
93    WarnUponSwitches(true);
94    SetPeriodMS(100);
95   
96    if (getFrameworkManager()->ErrorOccured() == true) {
97        SafeStop();
98    }
99
[377]100    while (!ToBeStopped()) {
[379]101        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
102        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
103       
104        if (startCircle->Clicked() == true)
105            StartCircle();
106       
107        if (stopCircle->Clicked() == true)
108            StopCircle();
109       
110         //R1 and Circle
111        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
112            StartCircle();
113        }
114
115        //R1 and Cross
116        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
117            StopCircle();
118        }
119   
[376]120        if (button_kill->Clicked() == true)
121            SafeStop();
[377]122       
[376]123        WaitPeriod();
124    }
125}
126
[379]127void CircleFollower::ComputeManualControls(void) {
128  float speed=-controller->GetAxisValue(3);
129  float turn=controller->GetAxisValue(0);
130  GetUgv()->GetUgvControls()->SetControls(speed,turn);
131}
[376]132
[379]133void CircleFollower::ComputeCircleControls(void) {
134
135  Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
136  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
137  Vector2Df pos_error,vel_error;
138  Vector2Df circle_pos,circle_vel;
139   
140  ugvVrpn->GetPosition(ugv_pos);
141  ugvVrpn->GetSpeed(ugv_vel);
142
143  ugv_pos.To2Dxy(ugv_2Dpos);
144  ugv_vel.To2Dxy(ugv_2Dvel);
145 
146  targetVrpn->GetPosition(target_pos);
147  target_pos.To2Dxy(target_2Dpos);
148  circle->SetCenter(target_2Dpos);
149
150  //circle reference
151  circle->Update(GetTime());
152  circle->GetPosition(circle_pos);
153  circle->GetSpeed(circle_vel);
154
155  //error in optitrack frame
156  pos_error=ugv_2Dpos-circle_pos;
157  vel_error=ugv_2Dvel-circle_vel;
158   
159  uX->SetValues(pos_error.x, vel_error.x);
160  uX->Update(GetTime());
161  uY->SetValues(pos_error.y, vel_error.y);
162  uY->Update(GetTime());
163 
164  //get yaw from vrpn
165  Quaternion vrpnQuaternion;
166  ugvVrpn->GetQuaternion(vrpnQuaternion);
167  float yaw=vrpnQuaternion.ToEuler().yaw;
168  float L=1;
169  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
170  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
171  GetUgv()->GetUgvControls()->SetControls(-v,-w);
172}
173
174
175void CircleFollower::StartCircle(void) {
176    Vector3Df ugv_pos,target_pos;
177    Vector2Df ugv_2Dpos,target_2Dpos;
178
[376]179    targetVrpn->GetPosition(target_pos);
180    target_pos.To2Dxy(target_2Dpos);
181    circle->SetCenter(target_2Dpos);
182
[379]183    ugvVrpn->GetPosition(ugv_pos);
184    ugv_pos.To2Dxy(ugv_2Dpos);
185    circle->StartTraj(ugv_2Dpos);
[376]186
187    uX->Reset();
188    uY->Reset();
[379]189    behaviourMode=BehaviourMode_t::Circle;
190    Thread::Info("CircleFollower: start circle\n");
[376]191}
192
193void CircleFollower::StopCircle(void) {
[379]194       
[376]195    circle->FinishTraj();
196    //GetJoystick()->Rumble(0x70);
[379]197    behaviourMode=BehaviourMode_t::Manual;
198    Thread::Info("CircleFollower: finishing circle\n");
[376]199}
200
Note: See TracBrowser for help on using the repository browser.