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

Timestamp:

12/17/20 15:30:33 (3 years ago)

Author:

Sanahuja Guillaume

Message:

modifs ugv

File:

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

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

@@ -24 +24 @@
 #include <Matrix.h>
 #include <Tab.h>
+#include <TabWidget.h>
+#include <DoubleSpinBox.h>
 #include <Pid.h>
+#include <Quaternion.h>
+#include <Euler.h>
 #include <Ugv.h>
 #include <UgvControls.h>
+#include <math.h>
 
 using namespace std;
@@ -36 +41 @@
 using namespace flair::actuator;
 
-CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
-   this->controller=controller;
+CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
+    this->controller=controller;
     controller->Start();
     
@@ -57 +62 @@
     vrpnclient->Start();
         
-        Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
+    Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
     GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
-        button_kill = new PushButton(buttonslayout->NewRow(), "kill");
+    button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
     stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
@@ -69 +74 @@
     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");
-/*
+
+    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
+    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
+    Tab *setupLawTab = new Tab(tabWidget, "Setup");
+    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
     uX=new Pid(setupLawTab->At(1,0),"u_x");
     uX->UseDefaultPlot(graphLawTab->NewRow());
     uY=new Pid(setupLawTab->At(1,1),"u_y");
-    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());*/
+    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
+    
+    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
 }
 
@@ -88 +99 @@
 
     while (!ToBeStopped()) {
-        float speed=-controller->GetAxisValue(3);
-        float turn=controller->GetAxisValue(0);
-       
-        //sumoControl->move(x, rot);
-        GetUgv()->GetUgvControls()->SetControls(speed,turn);
+        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
+        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
+        
+        if (startCircle->Clicked() == true)
+            StartCircle();
+        
+        if (stopCircle->Clicked() == true)
+            StopCircle();
+        
+         //R1 and Circle
+        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
+            StartCircle();
+        }
+
+        //R1 and Cross
+        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
+            StopCircle();
+        }
+    
         if (button_kill->Clicked() == true)
             SafeStop();
@@ -100 +125 @@
 }
 
-void CircleFollower::StartCircle(void) {/*
-    if (SetOrientationMode(OrientationMode_t::Custom)) {
-        Thread::Info("CircleFollower: start circle\n");
-    } else {
-        Thread::Warn("CircleFollower: could not start circle\n");
-        return;
-    }
-    Vector3Df uav_pos,target_pos;
-    Vector2Df uav_2Dpos,target_2Dpos;
+void CircleFollower::ComputeManualControls(void) {
+  float speed=-controller->GetAxisValue(3);
+  float turn=controller->GetAxisValue(0);
+  GetUgv()->GetUgvControls()->SetControls(speed,turn);
+}
+
+void CircleFollower::ComputeCircleControls(void) {
+
+  Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
+  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
+  Vector2Df pos_error,vel_error;
+  Vector2Df circle_pos,circle_vel;
+    
+  ugvVrpn->GetPosition(ugv_pos);
+  ugvVrpn->GetSpeed(ugv_vel);
+
+  ugv_pos.To2Dxy(ugv_2Dpos);
+  ugv_vel.To2Dxy(ugv_2Dvel);
+  
+  targetVrpn->GetPosition(target_pos);
+  target_pos.To2Dxy(target_2Dpos);
+  circle->SetCenter(target_2Dpos);
+
+  //circle reference
+  circle->Update(GetTime());
+  circle->GetPosition(circle_pos);
+  circle->GetSpeed(circle_vel);
+
+  //error in optitrack frame
+  pos_error=ugv_2Dpos-circle_pos;
+  vel_error=ugv_2Dvel-circle_vel;
+    
+  uX->SetValues(pos_error.x, vel_error.x);
+  uX->Update(GetTime());
+  uY->SetValues(pos_error.y, vel_error.y);
+  uY->Update(GetTime());
+  
+  //get yaw from vrpn
+  Quaternion vrpnQuaternion;
+  ugvVrpn->GetQuaternion(vrpnQuaternion);
+  float yaw=vrpnQuaternion.ToEuler().yaw;
+  float L=1;
+  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
+  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
+  GetUgv()->GetUgvControls()->SetControls(-v,-w);
+}
+
+
+void CircleFollower::StartCircle(void) {
+    Vector3Df ugv_pos,target_pos;
+    Vector2Df ugv_2Dpos,target_2Dpos;
 
     targetVrpn->GetPosition(target_pos);
@@ -114 +181 @@
     circle->SetCenter(target_2Dpos);
 
-    uavVrpn->GetPosition(uav_pos);
-    uav_pos.To2Dxy(uav_2Dpos);
-    circle->StartTraj(uav_2Dpos);
+    ugvVrpn->GetPosition(ugv_pos);
+    ugv_pos.To2Dxy(ugv_2Dpos);
+    circle->StartTraj(ugv_2Dpos);
 
     uX->Reset();
     uY->Reset();
-    behaviourMode=BehaviourMode_t::Circle;*/
+    behaviourMode=BehaviourMode_t::Circle;
+    Thread::Info("CircleFollower: start circle\n");
 }
 
 void CircleFollower::StopCircle(void) {
-        /*
+        
     circle->FinishTraj();
     //GetJoystick()->Rumble(0x70);
-    Thread::Info("CircleFollower: finishing circle\n");*/
-}
-
+    behaviourMode=BehaviourMode_t::Manual;
+    Thread::Info("CircleFollower: finishing circle\n");
+}
+