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

Last change on this file since 376 was 376, checked in by Sanahuja Guillaume, 10 months ago

up ugv

  • Property svn:eol-style set to native
File size: 3.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 <Pid.h>
27
28using namespace std;
29using namespace flair::core;
30using namespace flair::gui;
31using namespace flair::sensor;
32using namespace flair::filter;
33using namespace flair::meta;
34
35CircleFollower::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
72CircleFollower::~CircleFollower() {
73}
74
75void 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
96void 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
119void CircleFollower::StopCircle(void) {
120        /*
121    circle->FinishTraj();
122    //GetJoystick()->Rumble(0x70);
123    Thread::Info("CircleFollower: finishing circle\n");*/
124}
125
Note: See TracBrowser for help on using the repository browser.