Module_Mouvement.cpp

00001      /* Copyright (c) 2012 - 2013 AUTHEUR
00002  *
00003  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED 
00004  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
00005  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
00006  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00007  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
00008  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
00009  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 
00010  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 
00011  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00012  */
00013  
00014  /*
00015  * Description
00016  * Input
00017  * Output
00018  */
00019  
00020  #include "Module_Mouvement.h"
00021  
00022  // Pointeur sur la classe systeme d'exploitation instancié dans le main
00023  extern mbos *os;
00024  
00025  //communication série USB
00026  Serial pc1(USBTX,USBRX);
00027     
00028  
00029  /* CONSRTRUCTEUR(S) */
00030  ModuleMouvement::ModuleMouvement()
00031  {
00032  m_motor1 = new PwmOut (p23);
00033  m_motor2 = new PwmOut (p24);
00034  m_motor3 = new PwmOut (p25);
00035  m_motor4 = new PwmOut (p26); 
00036  }
00037  
00038  /* DESTRUCTEUR */
00039  ModuleMouvement::~ModuleMouvement()
00040  {
00041  delete m_motor1;
00042  delete m_motor2;
00043  delete m_motor3;
00044  delete m_motor4;
00045  }
00046  
00047  
00048  /*Get*/ 
00049  int ModuleMouvement::GetpVitesseFonctionnement()
00050   { return pVitesseFonctionnement; } 
00051   
00052  int ModuleMouvement::GetuiCommand()
00053   { return m_uiCommand; }
00054   
00055  float ModuleMouvement::GetuiDistance()
00056   { return m_uiDistance; } 
00057    
00058   
00059   /*Set*/
00060   void ModuleMouvement::SetpVitesseFonctionnement(int m)
00061    { pVitesseFonctionnement = m ; }
00062    
00063   void ModuleMouvement::SetuiCommand(int n)
00064    { m_uiCommand = n ; }
00065    
00066   void ModuleMouvement::SetuiDistance(float o)
00067    { m_uiDistance = o ; }
00068    
00069    
00070  
00071  /* Point d'entrée de la tache Module Mouvement */
00072  void ModuleMouvement::ModuleMouvement_Task(void)
00073  {
00074     //init moteurs
00075     InitMotor();
00076     
00077     //décollage
00078     m_uiCommand = 0;
00079     m_uiDistance = 0.6; //60cm 
00080     GestionVitesseMotors(m_uiCommand, m_uiDistance);
00081      
00082     
00083     while(1){
00084    
00085     //vol stationnaire
00086     //attente recuperation commande provenant de trajectoire,
00087     VolStationnaire();
00088      
00089     // gestion motors
00090     GestionVitesseMotors(m_uiCommand, m_uiDistance);
00091     
00092     
00093     // gestion stabilisation
00094     }
00095  }
00096  
00097  void ModuleMouvement::VolStationnaire(void)
00098  {
00099      m_motor1->write(0.68); // a def
00100      m_motor2->write(0.68); // a def
00101      m_motor3->write(0.68); // a def
00102      m_motor4->write(0.68); // a def
00103      /*while ( pas de nouvelle commande )
00104       {
00105         wait(0.1);
00106       }*/
00107       
00108      return;
00109  }
00110  
00111  void ModuleMouvement::GestionVitesseMotors(int m_uiCommand, float m_uiDistance)
00112  {
00113     bool CommandFin = false;
00114     
00115      switch (m_uiCommand)
00116       {
00117         /*haut*/      
00118         case 0 :
00119         //Liste infos;
00120         break;
00121         
00122         /*bas*/
00123         case 1 :
00124         //liste infos;
00125         break;
00126         
00127         /*rotation gauche*/
00128         case 2 :
00129         //liste infos;
00130         break;
00131         
00132         /*rotation droite*/
00133         case 3 :
00134         //Liste infos;
00135         break;
00136         
00137         /*gauche*/
00138         case 4 :
00139         //Liste infos;
00140         break;
00141         
00142         /*droite*/
00143         case 5 :
00144         //liste infos;
00145         break; 
00146         
00147         /*avance*/
00148         case 6 :
00149         //Liste infos;
00150         break;
00151         
00152         /*recule*/
00153         case 7 :
00154         //Liste infos
00155         break;
00156      
00157         default :
00158         break;
00159         /*erreur pas de commande*/
00160    
00161         
00162       }
00163       CommandFin = true;
00164  }
00165  
00166  void ModuleMouvement::GestionStabilisation(void)
00167  {
00168  
00169  }
00170  
00171  void ModuleMouvement::TestMotor(void)
00172  {
00173 
00174        //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
00175        m_motor1->period(0.002); 
00176        
00177        //initialisation, attends du premier bip
00178        for (float s= 0; s < 0.40; s += 0.01) 
00179        {
00180        m_motor1->write(s);
00181        m_motor2->write(s);
00182        m_motor3->write(s);
00183        m_motor4->write(s);
00184        wait(0.2); 
00185        pc1.printf("%f", s);
00186        }
00187        wait(2);
00188        
00189        m_motor1->write(0.96);
00190        m_motor2->write(0.96);
00191        m_motor3->write(0.96);
00192        m_motor4->write(0.96);
00193        pc1.printf("test motor");
00194        
00195        
00196  } 
00197  
00198  
00199  void ModuleMouvement::InitMotor(void)
00200  {
00201        //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
00202        m_motor1->period(0.002);
00203        m_motor2->period(0.002);
00204        m_motor3->period(0.002);
00205        m_motor4->period(0.002);
00206        
00207        //frequence à 400HZ  (attention : init à 0.30 et PWMmax=0.85, trés rapide à l'acceleration )
00208        //m_motor1->period(0.0025);
00209        
00210        //initialisation, attends du premier bip
00211        for (float s= 0; s < 0.40; s += 0.01) 
00212        {
00213        m_motor1->write(s);
00214        m_motor2->write(s);
00215        m_motor3->write(s);
00216        m_motor4->write(s); 
00217        wait(0.02); 
00218        pc1.printf("%f", s);
00219        }
00220        wait(1);      
00221  }
00222  
00223  
00224   void ModuleMouvement::DeconnexionMotor(void)
00225  {
00226        //deconnexion 
00227        for(float s= 0.40; s > 0; s-= 0.01)
00228        {
00229        m_motor1->write(s); 
00230        m_motor2->write(s);
00231        m_motor3->write(s);
00232        m_motor4->write(s); 
00233        wait(0.02);
00234        pc1.printf("%f", s);
00235        }
00236        wait(1);
00237  }