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 |
|
---|
30 | using namespace std;
|
---|
31 | using namespace flair::core;
|
---|
32 | using namespace flair::gui;
|
---|
33 | using namespace flair::sensor;
|
---|
34 | using namespace flair::filter;
|
---|
35 | using namespace flair::meta;
|
---|
36 | using namespace flair::actuator;
|
---|
37 |
|
---|
38 | CircleFollower::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 |
|
---|
78 | CircleFollower::~CircleFollower() {
|
---|
79 | }
|
---|
80 |
|
---|
81 | void 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 |
|
---|
102 | void 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 |
|
---|
125 | void CircleFollower::StopCircle(void) {
|
---|
126 | /*
|
---|
127 | circle->FinishTraj();
|
---|
128 | //GetJoystick()->Rumble(0x70);
|
---|
129 | Thread::Info("CircleFollower: finishing circle\n");*/
|
---|
130 | }
|
---|
131 |
|
---|