Diff: Map/Objectifs/Obj_pince.cpp

Revision:

117:f8c147141a0c

Parent:

114:be06d518b4a7

Child:

119:c45efcd706d9

--- a/Map/Objectifs/Obj_pince.cpp	Wed May 06 06:51:49 2015 +0000
+++ b/Map/Objectifs/Obj_pince.cpp	Wed May 06 11:22:17 2015 +0000
@@ -1,1 +1,47 @@
-#include "Obj_pince.h"
\ No newline at end of file
+#include "Obj_pince.h"
+#include <vector>
+
+#ifdef PLAN_A
+    extern Asserv<float> asserv;
+#else
+    extern aserv_planB asserv;
+#endif
+
+extern Odometry2 odometry;
+extern std::vector<Objectif*> objectifs;
+
+Obj_pince::Obj_pince(float x, float y, float theta, AX12 *ax12_pince)
+:Objectif(x,y,theta)
+{
+    this->ax12_pince = ax12_pince;
+}
+
+void Obj_pince::run()
+{
+    float thetaGoal = atan2(y-odometry.getY(),x-odometry.getX());
+    asserv.setGoal(odometry.getX()+cos(thetaGoal)*200,odometry.getY()+sin(thetaGoal)*200,thetaGoal); //On avance jusqu'au goblet/spot
+    while(!asserv.isArrived())wait(0.1);
+    
+    ax12_pince->setGoal(PINCE_FERMEE);
+    done = true;
+    
+    for(unsigned int i=0;i < objectifs.size();i++)
+    {
+        if(objectifs[i]->getId() == IDO_DEPOT && !objectifs[i]->isDone())
+        {
+            objectifs[i]->activate();
+            break;
+        }
+    }
+}
+
+int Obj_pince::isActive()
+{
+    if(!active)
+        return false;
+    
+    if(ax12_pince->getGoal() == PINCE_FERMEE)
+        return false;
+    
+    return true;
+}