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

Last change on this file since 378 was 377, checked in by Sanahuja Guillaume, 19 months ago

ugv update

  • Property svn:eol-style set to native
File size: 4.1 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 <Pid.h>
27#include <Ugv.h>
28#include <UgvControls.h>
29
30using namespace std;
31using namespace flair::core;
32using namespace flair::gui;
33using namespace flair::sensor;
34using namespace flair::filter;
35using namespace flair::meta;
36using namespace flair::actuator;
37
38CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
39 this->controller=controller;
40 controller->Start();
41
42 Ugv* ugv=GetUgv();
43 ugv->UseDefaultPlot();
44
45 VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
46
47 if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
48 ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
49 targetVrpn=new MetaVrpnObject("target",1);
50 } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
51 ugvVrpn = new MetaVrpnObject(name);
52 targetVrpn=new MetaVrpnObject("target");
53 }
54
55 getFrameworkManager()->AddDeviceToLog(ugvVrpn);
56 getFrameworkManager()->AddDeviceToLog(targetVrpn);
57 vrpnclient->Start();
58
59 Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
60 GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
61 button_kill = new PushButton(buttonslayout->NewRow(), "kill");
62 startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
63 stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
64
65 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
66 ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
67 ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
68 ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
69 ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
70 ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
71/*
72 uX=new Pid(setupLawTab->At(1,0),"u_x");
73 uX->UseDefaultPlot(graphLawTab->NewRow());
74 uY=new Pid(setupLawTab->At(1,1),"u_y");
75 uY->UseDefaultPlot(graphLawTab->LastRowLastCol());*/
76}
77
78CircleFollower::~CircleFollower() {
79}
80
81void CircleFollower::Run(void) {
82 WarnUponSwitches(true);
83 SetPeriodMS(100);
84
85 if (getFrameworkManager()->ErrorOccured() == true) {
86 SafeStop();
87 }
88
89 while (!ToBeStopped()) {
90 float speed=-controller->GetAxisValue(3);
91 float turn=controller->GetAxisValue(0);
92
93 //sumoControl->move(x, rot);
94 GetUgv()->GetUgvControls()->SetControls(speed,turn);
95 if (button_kill->Clicked() == true)
96 SafeStop();
97
98 WaitPeriod();
99 }
100}
101
102void CircleFollower::StartCircle(void) {/*
103 if (SetOrientationMode(OrientationMode_t::Custom)) {
104 Thread::Info("CircleFollower: start circle\n");
105 } else {
106 Thread::Warn("CircleFollower: could not start circle\n");
107 return;
108 }
109 Vector3Df uav_pos,target_pos;
110 Vector2Df uav_2Dpos,target_2Dpos;
111
112 targetVrpn->GetPosition(target_pos);
113 target_pos.To2Dxy(target_2Dpos);
114 circle->SetCenter(target_2Dpos);
115
116 uavVrpn->GetPosition(uav_pos);
117 uav_pos.To2Dxy(uav_2Dpos);
118 circle->StartTraj(uav_2Dpos);
119
120 uX->Reset();
121 uY->Reset();
122 behaviourMode=BehaviourMode_t::Circle;*/
123}
124
125void CircleFollower::StopCircle(void) {
126 /*
127 circle->FinishTraj();
128 //GetJoystick()->Rumble(0x70);
129 Thread::Info("CircleFollower: finishing circle\n");*/
130}
131
Note: See TracBrowser for help on using the repository browser.