Diff: Map/Objectifs/Obj_clap.cpp

Revision:

123:55e5e9acc541

Parent:

117:f8c147141a0c

--- a/Map/Objectifs/Obj_clap.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Map/Objectifs/Obj_clap.cpp	Mon May 11 20:32:11 2015 +0000
@@ -1,16 +1,47 @@
 #include "Obj_clap.h"
 
+#ifdef PLAN_A
+    extern Asserv<float> asserv;
+#else
+    extern aserv_planB asserv;
+#endif
 
-Obj_clap::Obj_clap(float x, float y, float theta, AX12 *ax12_brasG, AX12 *ax12_brasD)
+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)
+    {
+        ax12_brasD->setMaxTorque(MAX_TORQUE);
+        ax12_brasD->setGoal(BRASD_OUVERT);
+        wait(1);
+        asserv.setGoal(x2,y2,theta2);
+        while(!asserv.isArrived())wait(0.1);
+        ax12_brasD->setGoal(BRASD_FERME);
+        wait(1);
+        ax12_brasD->setMaxTorque(0);
+    }
+    else
+    {
+        ax12_brasG->setMaxTorque(MAX_TORQUE);
+        ax12_brasG->setGoal(BRASG_OUVERT);
+        wait(1);
+        asserv.setGoal(x2,y2,theta2);
+        while(!asserv.isArrived())wait(0.1);
+        ax12_brasG->setGoal(BRASG_FERME);
+        wait(1);
+        ax12_brasG->setMaxTorque(0);
+    }
     
+    done = true;
 }
 
 int Obj_clap::isActive()