Changeset 377 in flair-src for trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp

Timestamp:

Dec 16, 2020, 9:40:44 AM (4 years ago)

Author:

Sanahuja Guillaume

Message:

ugv update

File:

• trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp (modified) (4 diffs)

trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp

@@ -25 +25 @@
 #include <Tab.h>
 #include <Pid.h>
+#include <Ugv.h>
+#include <UgvControls.h>
 
 using namespace std;
@@ -32 +34 @@
 using namespace flair::filter;
 using namespace flair::meta;
+using namespace flair::actuator;
 
 CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    this->controller=controller;
     controller->Start();
-        
-    VrpnClient* vrpnclient=new VrpnClient("vrpn", "127.0.0.1:3883",80);
+    
+    Ugv* ugv=GetUgv();
+    ugv->UseDefaultPlot();
+    
+    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
     
     if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
-        uavVrpn = new MetaVrpnObject(name,(uint8_t)0);
+        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
         targetVrpn=new MetaVrpnObject("target",1);
     } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
-        uavVrpn = new MetaVrpnObject(name);
+        ugvVrpn = new MetaVrpnObject(name);
         targetVrpn=new MetaVrpnObject("target");
     }
     
-    getFrameworkManager()->AddDeviceToLog(uavVrpn);
+    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
     getFrameworkManager()->AddDeviceToLog(targetVrpn);
     vrpnclient->Start();
@@ -58 +64 @@
     
     circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
-    uavVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
-    uavVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
-    uavVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
-    uavVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
-    uavVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
+    ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
+    ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
+    ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
+    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
+    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
 /*
     uX=new Pid(setupLawTab->At(1,0),"u_x");
@@ -81 +87 @@
     }
 
-    while (!ToBeStopped()) {/*
-        float x=-controller->GetAxisValue(3)*forwardCoef->Value();
-        float rot=controller->GetAxisValue(0)*turnCoef->Value();
-        
-        sumoControl->move(x, rot);
-        
+    while (!ToBeStopped()) {
+        float speed=-controller->GetAxisValue(3);
+        float turn=controller->GetAxisValue(0);
+       
+        //sumoControl->move(x, rot);
+        GetUgv()->GetUgvControls()->SetControls(speed,turn);
         if (button_kill->Clicked() == true)
             SafeStop();
-        */
+        
         WaitPeriod();
     }