Index: trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml (revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml (revision 379)
@@ -5,5 +5,5 @@
-
+
@@ -11,11 +11,11 @@
-
-
+
+
-
+
@@ -38,6 +38,6 @@
-
-
+
+
Index: trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp (revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp (revision 379)
@@ -96,11 +96,9 @@
gui = new Parser(960, 480, 960, 480, media_path, scene_file);
#endif
-
robot = new TwoWheelRobot(name, 0);
-
#ifdef GL
- //man = new Man("target",1);
+ man = new Man("target",1);
#endif
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml (revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml (revision 379)
@@ -40,15 +40,15 @@
-
-
-
+
+
+
-
+
-
-
+
+
@@ -72,9 +72,9 @@
-
-
+
+
-
+
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp (revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp (revision 379)
@@ -24,7 +24,12 @@
#include
#include
+#include
+#include
#include
+#include
+#include
#include
#include
+#include
using namespace std;
@@ -36,6 +41,6 @@
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,7 +62,7 @@
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,9 +74,15 @@
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,9 +99,23 @@
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,13 +125,55 @@
}
-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,18 +181,20 @@
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");
+}
+
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h (revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h (revision 379)
@@ -20,4 +20,5 @@
namespace gui {
class PushButton;
+ class DoubleSpinBox;
}
namespace filter {
@@ -41,22 +42,22 @@
enum class BehaviourMode_t {
- Default,
+ Manual,
Circle
};
+ void Run(void);
+ void StartCircle(void);
+ void StopCircle(void);
+ void ComputeManualControls(void);
+ void ComputeCircleControls(void);
+
+ flair::filter::Pid *uX, *uY;
+ flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
+ flair::gui::DoubleSpinBox *l;
+ flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
+ flair::filter::TrajectoryGenerator2DCircle *circle;
BehaviourMode_t behaviourMode;
bool vrpnLost;
- flair::sensor::TargetController *controller;
-
- void Run(void);
- void StartCircle(void);
- void StopCircle(void);
-
- flair::filter::Pid *uX, *uY;
-
- flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
- flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
- flair::filter::TrajectoryGenerator2DCircle *circle;
-
+ flair::sensor::TargetController *controller;
};