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

Timestamp:

Dec 21, 2020, 2:50:44 PM (4 years ago)

Author:

Sanahuja Guillaume

Message:

update demo code

File:

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

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

@@ -49 +49 @@
     
     VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
-    
-    if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
-        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
-        targetVrpn=new MetaVrpnObject("target",1);
-    } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
-        ugvVrpn = new MetaVrpnObject(name);
-        targetVrpn=new MetaVrpnObject("target");
-    }
+    ugvVrpn = new MetaVrpnObject(name);
+    targetVrpn=new MetaVrpnObject("target");
     
     getFrameworkManager()->AddDeviceToLog(ugvVrpn);
@@ -92 +86 @@
 void CircleFollower::Run(void) {
     WarnUponSwitches(true);
-    SetPeriodMS(100);
+    SetPeriodMS(20);
     
     if (getFrameworkManager()->ErrorOccured() == true) {
@@ -99 +93 @@
 
     while (!ToBeStopped()) {
+        SecurityCheck();
+        CheckJoystick();
+        CheckPushButton();
+       
         if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
         if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
+        WaitPeriod();
+    }
+}
+
+void CircleFollower::CheckPushButton(void) {
+  if (startCircle->Clicked() == true)
+      StartCircle();
         
-        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)) {
+  if (stopCircle->Clicked() == true)
+      StopCircle();
+
+  if (button_kill->Clicked() == true)
+      SafeStop();
+}
+
+void CircleFollower::CheckJoystick(void) {
+  //R1 and Circle
+  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
+      StartCircle();
+  }
+
+  //R1 and Cross
+  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
+      StopCircle();
+  }
+}
+
+void CircleFollower::SecurityCheck(void) {
+    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
+        if (!targetVrpn->IsTracked(500)) {
+            Thread::Err("VRPN, target lost\n");
+            vrpnLost=true;
             StopCircle();
         }
-    
-        if (button_kill->Clicked() == true)
-            SafeStop();
-        
-        WaitPeriod();
+        if (!ugvVrpn->IsTracked(500)) {
+            Thread::Err("VRPN, ugv lost\n");
+            vrpnLost=true;
+            StopCircle();
+        }
     }
 }
@@ -174 +190 @@
 
 void CircleFollower::StartCircle(void) {
+  if(behaviourMode!=BehaviourMode_t::Circle) {
     Vector3Df ugv_pos,target_pos;
     Vector2Df ugv_2Dpos,target_2Dpos;
@@ -189 +206 @@
     behaviourMode=BehaviourMode_t::Circle;
     Thread::Info("CircleFollower: start circle\n");
+  }
 }
 
 void CircleFollower::StopCircle(void) {
-        
+        if(behaviourMode==BehaviourMode_t::Circle) {
     circle->FinishTraj();
     //GetJoystick()->Rumble(0x70);
     behaviourMode=BehaviourMode_t::Manual;
     Thread::Info("CircleFollower: finishing circle\n");
-}
-
+  }
+}
+