Obj_recalage.cpp

00001 #include "Obj_recalage.h"
00002 
00003 extern Motor motorL;
00004 extern Motor motorR;
00005 extern Odometry odometry;
00006 extern bool interruption;
00007 
00008 Obj_recalage::Obj_recalage(float x, float y, float theta)
00009 :Objectif(x,y,theta)
00010 {
00011     
00012 }
00013 
00014 void Obj_recalage::run()
00015 {
00016     motorL.setSpeed(-0.3);
00017     motorR.setSpeed(-0.3);
00018     
00019     float lastPos = 0;
00020     if(theta == -PI/2)
00021     {
00022         lastPos = odometry.getY();
00023         while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
00024         {
00025             lastPos = odometry.getY();
00026         }
00027         // On a finit
00028         if(abs((3000-82.2)-odometry.getY()) < 200) // bonne erreur, on prend en compte
00029         {
00030             odometry.setY(3000-82.2f);
00031             odometry.setTheta(theta);
00032         }
00033     }
00034     
00035     if(theta == PI/2)
00036     {
00037         lastPos = odometry.getY();
00038         while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
00039         {
00040             lastPos = odometry.getY();
00041         }
00042         // On a finit
00043         if(abs(82.2f-odometry.getY()) < 200.0f) // bonne erreur, on prend en compte
00044         {
00045             odometry.setY(82.2f);
00046             odometry.setTheta(theta);
00047         }
00048     }
00049     
00050     motorL.setSpeed(0);
00051     motorR.setSpeed(0);
00052     
00053     wait(0.1);
00054     
00055     done = true;
00056 }
00057 
00058 int Obj_recalage::isActive()
00059 {
00060     if(!active)
00061         return false;
00062     return true;
00063 }