Arnaud Suire / Labo_TRSE_Drone

ajout module_mouvement

Dependencies:   mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID

Fork of Labo_TRSE_Drone by HERBERT Nicolas

Module_Asservissement/Sous_Module_Mouvement/Module_Mouvement.cpp@30:8374d1017296, 2013-04-22 (annotated)

Committer:
arnaudsuire
Date:
Mon Apr 22 19:09:53 2013 +0000
Revision:
30:8374d1017296
Parent:
29:f8bda24002f6 
lundi 22

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 29:f8bda24002f6 1 /* Copyright (c) 2012 - 2013 AUTHEUR
Gaetan 10:c8d73680b9fd 2 *
Gaetan 10:c8d73680b9fd 3 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED
Gaetan 10:c8d73680b9fd 4 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
Gaetan 10:c8d73680b9fd 5 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
Gaetan 10:c8d73680b9fd 6 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
Gaetan 10:c8d73680b9fd 7 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
Gaetan 10:c8d73680b9fd 8 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
Gaetan 10:c8d73680b9fd 9 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
Gaetan 10:c8d73680b9fd 10 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
Gaetan 10:c8d73680b9fd 11 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Gaetan 10:c8d73680b9fd 12 */
Gaetan 10:c8d73680b9fd 13
Gaetan 10:c8d73680b9fd 14 /*
Gaetan 10:c8d73680b9fd 15 * Description
Gaetan 10:c8d73680b9fd 16 * Input
Gaetan 10:c8d73680b9fd 17 * Output
NicolasH 14:ca1bcc05e423 18 */
NicolasH 14:ca1bcc05e423 19
NicolasH 14:ca1bcc05e423 20 #include "Module_Mouvement.h"
NicolasH 14:ca1bcc05e423 21
NicolasH 14:ca1bcc05e423 22 // Pointeur sur la classe systeme d'exploitation instancié dans le main
NicolasH 14:ca1bcc05e423 23 extern mbos *os;
NicolasH 14:ca1bcc05e423 24
arnaudsuire 30:8374d1017296 25 //communication série USB
arnaudsuire 30:8374d1017296 26 Serial pc1(USBTX,USBRX);
arnaudsuire 30:8374d1017296 27
arnaudsuire 30:8374d1017296 28
arnaudsuire 30:8374d1017296 29
NicolasH 14:ca1bcc05e423 30
NicolasH 14:ca1bcc05e423 31 /* CONSRTRUCTEUR(S) */
NicolasH 14:ca1bcc05e423 32 ModuleMouvement::ModuleMouvement()
NicolasH 14:ca1bcc05e423 33 {
arnaudsuire 30:8374d1017296 34 m_motor1 = new PwmOut (p21);
arnaudsuire 30:8374d1017296 35 m_motor2 = new PwmOut (p22);
arnaudsuire 30:8374d1017296 36 m_motor3 = new PwmOut (p23);
arnaudsuire 30:8374d1017296 37 m_motor4 = new PwmOut (p24);
NicolasH 14:ca1bcc05e423 38 }
NicolasH 14:ca1bcc05e423 39
NicolasH 14:ca1bcc05e423 40 /* DESTRUCTEUR */
NicolasH 14:ca1bcc05e423 41 ModuleMouvement::~ModuleMouvement()
NicolasH 14:ca1bcc05e423 42 {
arnaudsuire 29:f8bda24002f6 43 delete m_motor1;
arnaudsuire 29:f8bda24002f6 44 delete m_motor2;
arnaudsuire 29:f8bda24002f6 45 delete m_motor3;
arnaudsuire 29:f8bda24002f6 46 delete m_motor4;
arnaudsuire 29:f8bda24002f6 47 }
NicolasH 14:ca1bcc05e423 48
arnaudsuire 29:f8bda24002f6 49
arnaudsuire 29:f8bda24002f6 50 /*Get*/
arnaudsuire 29:f8bda24002f6 51 int ModuleMouvement::GetpVitesseFonctionnement()
arnaudsuire 29:f8bda24002f6 52 { return pVitesseFonctionnement; }
arnaudsuire 29:f8bda24002f6 53
arnaudsuire 29:f8bda24002f6 54 int ModuleMouvement::GetuiCommand()
arnaudsuire 29:f8bda24002f6 55 { return m_uiCommand; }
arnaudsuire 29:f8bda24002f6 56
arnaudsuire 30:8374d1017296 57 float ModuleMouvement::GetuiDistance()
arnaudsuire 29:f8bda24002f6 58 { return m_uiDistance; }
arnaudsuire 29:f8bda24002f6 59
arnaudsuire 29:f8bda24002f6 60
arnaudsuire 29:f8bda24002f6 61 /*Set*/
arnaudsuire 29:f8bda24002f6 62 void ModuleMouvement::SetpVitesseFonctionnement(int m)
arnaudsuire 29:f8bda24002f6 63 { pVitesseFonctionnement = m ; }
arnaudsuire 29:f8bda24002f6 64
arnaudsuire 29:f8bda24002f6 65 void ModuleMouvement::SetuiCommand(int n)
arnaudsuire 29:f8bda24002f6 66 { m_uiCommand = n ; }
arnaudsuire 29:f8bda24002f6 67
arnaudsuire 30:8374d1017296 68 void ModuleMouvement::SetuiDistance(float o)
arnaudsuire 29:f8bda24002f6 69 { m_uiDistance = o ; }
arnaudsuire 29:f8bda24002f6 70
arnaudsuire 29:f8bda24002f6 71
NicolasH 14:ca1bcc05e423 72
NicolasH 14:ca1bcc05e423 73 /* Point d'entrée de la tache Module Mouvement */
NicolasH 14:ca1bcc05e423 74 void ModuleMouvement::ModuleMouvement_Task(void)
NicolasH 14:ca1bcc05e423 75 {
arnaudsuire 30:8374d1017296 76 //init moteurs
arnaudsuire 30:8374d1017296 77 InitMotor();
arnaudsuire 30:8374d1017296 78
arnaudsuire 30:8374d1017296 79 //décollage
arnaudsuire 30:8374d1017296 80 m_uiCommand = 0;
arnaudsuire 30:8374d1017296 81 m_uiDistance = 0.6; //60cm
arnaudsuire 30:8374d1017296 82 GestionVitesseMotors(m_uiCommand, m_uiDistance);
arnaudsuire 30:8374d1017296 83
arnaudsuire 30:8374d1017296 84 //vol stationnaire
arnaudsuire 30:8374d1017296 85 VolStationnaire();
arnaudsuire 30:8374d1017296 86
arnaudsuire 30:8374d1017296 87
NicolasH 14:ca1bcc05e423 88 while(1){
arnaudsuire 30:8374d1017296 89
arnaudsuire 29:f8bda24002f6 90 // recuperation commande provenant de trajectoire, comment recup, quel format, quel taille?
arnaudsuire 29:f8bda24002f6 91 // gestion motors
arnaudsuire 30:8374d1017296 92
arnaudsuire 29:f8bda24002f6 93 // gestion stabilisation, get services?
NicolasH 14:ca1bcc05e423 94 }
NicolasH 14:ca1bcc05e423 95 }
NicolasH 14:ca1bcc05e423 96
arnaudsuire 29:f8bda24002f6 97 void ModuleMouvement::VolStationnaire(void)
arnaudsuire 29:f8bda24002f6 98 {
arnaudsuire 30:8374d1017296 99 m_motor1->write(0.68); // a def
arnaudsuire 30:8374d1017296 100 m_motor2->write(0.68); // a def
arnaudsuire 30:8374d1017296 101 m_motor3->write(0.68); // a def
arnaudsuire 30:8374d1017296 102 m_motor4->write(0.68); // a def
arnaudsuire 30:8374d1017296 103 /*while ( pas de nouvelle commande )
arnaudsuire 29:f8bda24002f6 104 {
arnaudsuire 29:f8bda24002f6 105 wait(0.1);
arnaudsuire 30:8374d1017296 106 }*/
arnaudsuire 30:8374d1017296 107
arnaudsuire 29:f8bda24002f6 108 return;
arnaudsuire 29:f8bda24002f6 109 }
arnaudsuire 29:f8bda24002f6 110
arnaudsuire 30:8374d1017296 111 void ModuleMouvement::GestionVitesseMotors(int m_uiCommand, float m_uiDistance)
arnaudsuire 29:f8bda24002f6 112 {
arnaudsuire 29:f8bda24002f6 113 bool CommandFin = false;
arnaudsuire 29:f8bda24002f6 114
arnaudsuire 29:f8bda24002f6 115 switch (m_uiCommand)
arnaudsuire 29:f8bda24002f6 116 {
arnaudsuire 29:f8bda24002f6 117 /*haut*/
arnaudsuire 29:f8bda24002f6 118 case 0 :
arnaudsuire 29:f8bda24002f6 119 //Liste infos;
arnaudsuire 29:f8bda24002f6 120 break;
arnaudsuire 29:f8bda24002f6 121
arnaudsuire 29:f8bda24002f6 122 /*bas*/
arnaudsuire 29:f8bda24002f6 123 case 1 :
arnaudsuire 29:f8bda24002f6 124 //liste infos;
arnaudsuire 29:f8bda24002f6 125 break;
arnaudsuire 29:f8bda24002f6 126
arnaudsuire 29:f8bda24002f6 127 /*rotation gauche*/
arnaudsuire 29:f8bda24002f6 128 case 2 :
arnaudsuire 29:f8bda24002f6 129 //liste infos;
arnaudsuire 29:f8bda24002f6 130 break;
arnaudsuire 29:f8bda24002f6 131
arnaudsuire 29:f8bda24002f6 132 /*rotation droite*/
arnaudsuire 29:f8bda24002f6 133 case 3 :
arnaudsuire 29:f8bda24002f6 134 //Liste infos;
arnaudsuire 29:f8bda24002f6 135 break;
arnaudsuire 29:f8bda24002f6 136
arnaudsuire 29:f8bda24002f6 137 /*gauche*/
arnaudsuire 29:f8bda24002f6 138 case 4 :
arnaudsuire 29:f8bda24002f6 139 //Liste infos;
arnaudsuire 29:f8bda24002f6 140 break;
arnaudsuire 29:f8bda24002f6 141
arnaudsuire 29:f8bda24002f6 142 /*droite*/
arnaudsuire 29:f8bda24002f6 143 case 5 :
arnaudsuire 29:f8bda24002f6 144 //liste infos;
arnaudsuire 29:f8bda24002f6 145 break;
arnaudsuire 29:f8bda24002f6 146
arnaudsuire 29:f8bda24002f6 147 /*avance*/
arnaudsuire 29:f8bda24002f6 148 case 6 :
arnaudsuire 29:f8bda24002f6 149 //Liste infos;
arnaudsuire 29:f8bda24002f6 150 break;
arnaudsuire 29:f8bda24002f6 151
arnaudsuire 29:f8bda24002f6 152 /*recule*/
arnaudsuire 29:f8bda24002f6 153 case 7 :
arnaudsuire 29:f8bda24002f6 154 //Liste infos
arnaudsuire 29:f8bda24002f6 155 break;
arnaudsuire 29:f8bda24002f6 156
arnaudsuire 30:8374d1017296 157 default :
arnaudsuire 30:8374d1017296 158 /*erreur pas de commande*/
arnaudsuire 30:8374d1017296 159
arnaudsuire 30:8374d1017296 160
arnaudsuire 29:f8bda24002f6 161 }
arnaudsuire 30:8374d1017296 162 CommandFin = true;
arnaudsuire 29:f8bda24002f6 163 }
arnaudsuire 29:f8bda24002f6 164
arnaudsuire 29:f8bda24002f6 165 void ModuleMouvement::GestionStabilisation(void)
arnaudsuire 29:f8bda24002f6 166 {
arnaudsuire 29:f8bda24002f6 167
arnaudsuire 29:f8bda24002f6 168 }
arnaudsuire 29:f8bda24002f6 169
arnaudsuire 30:8374d1017296 170 void ModuleMouvement::TestMotor(void)
arnaudsuire 29:f8bda24002f6 171 {
arnaudsuire 30:8374d1017296 172
arnaudsuire 30:8374d1017296 173 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 30:8374d1017296 174 m_motor1->period(0.002);
arnaudsuire 30:8374d1017296 175
arnaudsuire 30:8374d1017296 176 //initialisation, attends du premier bip
arnaudsuire 30:8374d1017296 177 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 29:f8bda24002f6 178 {
arnaudsuire 30:8374d1017296 179 m_motor1->write(s);
arnaudsuire 30:8374d1017296 180 wait(0.2);
arnaudsuire 30:8374d1017296 181 pc1.printf("%f", s);
arnaudsuire 29:f8bda24002f6 182 }
arnaudsuire 30:8374d1017296 183 wait(2);
arnaudsuire 30:8374d1017296 184
arnaudsuire 30:8374d1017296 185 m_motor1->write(0.96);
arnaudsuire 30:8374d1017296 186 pc1.printf("test motor");
arnaudsuire 30:8374d1017296 187
arnaudsuire 29:f8bda24002f6 188
arnaudsuire 30:8374d1017296 189 }
arnaudsuire 30:8374d1017296 190
arnaudsuire 30:8374d1017296 191
arnaudsuire 30:8374d1017296 192 void ModuleMouvement::InitMotor(void)
arnaudsuire 30:8374d1017296 193 {
arnaudsuire 30:8374d1017296 194 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 30:8374d1017296 195 m_motor1->period(0.002);
arnaudsuire 30:8374d1017296 196 m_motor2->period(0.002);
arnaudsuire 30:8374d1017296 197 m_motor3->period(0.002);
arnaudsuire 30:8374d1017296 198 m_motor4->period(0.002);
arnaudsuire 30:8374d1017296 199
arnaudsuire 30:8374d1017296 200 //frequence à 400HZ (attention : init à 0.30 et PWMmax=0.85, trés rapide à l'acceleration )
arnaudsuire 30:8374d1017296 201 //m_motor1->period(0.0025);
arnaudsuire 30:8374d1017296 202
arnaudsuire 30:8374d1017296 203 //initialisation, attends du premier bip
arnaudsuire 30:8374d1017296 204 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 29:f8bda24002f6 205 {
arnaudsuire 30:8374d1017296 206 m_motor1->write(s);
arnaudsuire 30:8374d1017296 207 wait(0.2);
arnaudsuire 30:8374d1017296 208 pc1.printf("%f", s);
arnaudsuire 29:f8bda24002f6 209 }
arnaudsuire 30:8374d1017296 210 wait(1);
arnaudsuire 30:8374d1017296 211 }
arnaudsuire 30:8374d1017296 212
arnaudsuire 30:8374d1017296 213
arnaudsuire 30:8374d1017296 214 void ModuleMouvement::DeconnexionMotor(void)
arnaudsuire 30:8374d1017296 215 {
arnaudsuire 30:8374d1017296 216 //deconnexion
arnaudsuire 30:8374d1017296 217 for(float s= 0.40; s > 0; s-= 0.01)
arnaudsuire 29:f8bda24002f6 218 {
arnaudsuire 30:8374d1017296 219 m_motor1->write(s);
arnaudsuire 30:8374d1017296 220 wait(0.01);
arnaudsuire 30:8374d1017296 221 pc1.printf("%f", s);
arnaudsuire 29:f8bda24002f6 222 }
arnaudsuire 30:8374d1017296 223 wait(1);
arnaudsuire 30:8374d1017296 224 }