Diff: Map/Objectifs/Obj_pince.cpp

Revision:

119:c45efcd706d9

Parent:

117:f8c147141a0c

Child:

123:55e5e9acc541

--- a/Map/Objectifs/Obj_pince.cpp	Wed May 06 11:23:08 2015 +0000
+++ b/Map/Objectifs/Obj_pince.cpp	Wed May 06 15:17:16 2015 +0000
@@ -10,19 +10,22 @@
 extern Odometry2 odometry;
 extern std::vector<Objectif*> objectifs;
 
-Obj_pince::Obj_pince(float x, float y, float theta, AX12 *ax12_pince)
+Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
 :Objectif(x,y,theta)
 {
     this->ax12_pince = ax12_pince;
+    this->xp = xp;
+    this->yp = yp;
 }
 
 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
+    asserv.setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
     while(!asserv.isArrived())wait(0.1);
     
+    ax12_pince->setMaxTorque(MAX_TORQUE);
     ax12_pince->setGoal(PINCE_FERMEE);
+    wait(1.5);
     done = true;
     
     for(unsigned int i=0;i < objectifs.size();i++)