Diff: Map/Objectifs/Obj_clap.cpp

Revision:

0:b127c787a51b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_clap.cpp	Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,59 @@
+#include "Obj_clap.h"
+
+extern Asservissement *asserv;
+
+extern bool interruption;
+
+Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
+:Objectif(x,y,theta)
+{
+    this->ax12_brasG = ax12_brasG;
+    this->ax12_brasD = ax12_brasD;
+    this->x2 = x2;
+    this->y2 = y2;
+    this->theta2 = theta2;
+}
+
+void Obj_clap::run()
+{
+    if(theta2 == PI/2)
+    {
+        if(!interruption)
+        {
+            ax12_brasD->setGoal(BRASD_OUVERT);
+            wait(1);
+        }
+        if(!interruption) asserv->setGoal(x2,y2,theta2);
+        while(!asserv->isArrived() && !interruption)wait(0.1);
+
+        ax12_brasD->setGoal(BRASD_FERME);
+        
+        if(!interruption) wait(1);
+    }
+    else
+    {
+        if(!interruption)
+        {
+            ax12_brasG->setGoal(BRASG_OUVERT);
+            wait(1);
+        }
+        if(!interruption) asserv->setGoal(x2,y2,theta2);
+        while(!asserv->isArrived() && !interruption)wait(0.1);
+        
+        ax12_brasG->setGoal(BRASG_FERME);
+        if(!interruption) wait(1);
+    }
+    
+    done = true;
+}
+
+int Obj_clap::isActive()
+{
+    if(!active)
+        return false;
+    
+    if(ax12_brasG->getGoal() == BRASG_OUVERT || ax12_brasD->getGoal() == BRASD_OUVERT)
+        return false;
+    
+    return true;
+}