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