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

Last change on this file since 377 was 377, checked in by Sanahuja Guillaume, 9 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.