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@38:7ab036d94678, 2014-02-26 (annotated)

Committer:
arnaudsuire
Date:
Wed Feb 26 08:47:14 2014 +0000
Revision:
38:7ab036d94678
Parent:
37:e1ad11fe3fe4 
arnaud

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 34:acc8ea8694b4 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 34:acc8ea8694b4 25 //communication série USB
arnaudsuire 34:acc8ea8694b4 26 Serial pc1(USBTX,USBRX);
arnaudsuire 34:acc8ea8694b4 27
NicolasH 14:ca1bcc05e423 28
NicolasH 14:ca1bcc05e423 29 /* CONSRTRUCTEUR(S) */
NicolasH 14:ca1bcc05e423 30 ModuleMouvement::ModuleMouvement()
NicolasH 14:ca1bcc05e423 31 {
arnaudsuire 36:660be809a49d 32 m_motor1 = new PwmOut (p23);
arnaudsuire 36:660be809a49d 33 m_motor2 = new PwmOut (p24);
arnaudsuire 36:660be809a49d 34 m_motor3 = new PwmOut (p25);
arnaudsuire 36:660be809a49d 35 m_motor4 = new PwmOut (p26);
NicolasH 14:ca1bcc05e423 36 }
NicolasH 14:ca1bcc05e423 37
NicolasH 14:ca1bcc05e423 38 /* DESTRUCTEUR */
NicolasH 14:ca1bcc05e423 39 ModuleMouvement::~ModuleMouvement()
NicolasH 14:ca1bcc05e423 40 {
arnaudsuire 34:acc8ea8694b4 41 delete m_motor1;
arnaudsuire 34:acc8ea8694b4 42 delete m_motor2;
arnaudsuire 34:acc8ea8694b4 43 delete m_motor3;
arnaudsuire 34:acc8ea8694b4 44 delete m_motor4;
arnaudsuire 34:acc8ea8694b4 45 }
NicolasH 14:ca1bcc05e423 46
arnaudsuire 34:acc8ea8694b4 47
arnaudsuire 34:acc8ea8694b4 48 /*Get*/
arnaudsuire 34:acc8ea8694b4 49 int ModuleMouvement::GetpVitesseFonctionnement()
arnaudsuire 34:acc8ea8694b4 50 { return pVitesseFonctionnement; }
arnaudsuire 34:acc8ea8694b4 51
arnaudsuire 34:acc8ea8694b4 52 int ModuleMouvement::GetuiCommand()
arnaudsuire 34:acc8ea8694b4 53 { return m_uiCommand; }
arnaudsuire 34:acc8ea8694b4 54
arnaudsuire 34:acc8ea8694b4 55 float ModuleMouvement::GetuiDistance()
arnaudsuire 34:acc8ea8694b4 56 { return m_uiDistance; }
arnaudsuire 34:acc8ea8694b4 57
arnaudsuire 34:acc8ea8694b4 58
arnaudsuire 34:acc8ea8694b4 59 /*Set*/
arnaudsuire 34:acc8ea8694b4 60 void ModuleMouvement::SetpVitesseFonctionnement(int m)
arnaudsuire 34:acc8ea8694b4 61 { pVitesseFonctionnement = m ; }
arnaudsuire 34:acc8ea8694b4 62
arnaudsuire 34:acc8ea8694b4 63 void ModuleMouvement::SetuiCommand(int n)
arnaudsuire 34:acc8ea8694b4 64 { m_uiCommand = n ; }
arnaudsuire 34:acc8ea8694b4 65
arnaudsuire 34:acc8ea8694b4 66 void ModuleMouvement::SetuiDistance(float o)
arnaudsuire 34:acc8ea8694b4 67 { m_uiDistance = o ; }
arnaudsuire 34:acc8ea8694b4 68
arnaudsuire 34:acc8ea8694b4 69
NicolasH 14:ca1bcc05e423 70
NicolasH 14:ca1bcc05e423 71 /* Point d'entrée de la tache Module Mouvement */
NicolasH 14:ca1bcc05e423 72 void ModuleMouvement::ModuleMouvement_Task(void)
NicolasH 14:ca1bcc05e423 73 {
arnaudsuire 34:acc8ea8694b4 74 //init moteurs
arnaudsuire 34:acc8ea8694b4 75 InitMotor();
arnaudsuire 34:acc8ea8694b4 76
arnaudsuire 34:acc8ea8694b4 77 //décollage
arnaudsuire 34:acc8ea8694b4 78 m_uiCommand = 0;
arnaudsuire 34:acc8ea8694b4 79 m_uiDistance = 0.6; //60cm
arnaudsuire 34:acc8ea8694b4 80 GestionVitesseMotors(m_uiCommand, m_uiDistance);
arnaudsuire 34:acc8ea8694b4 81
arnaudsuire 34:acc8ea8694b4 82
NicolasH 14:ca1bcc05e423 83 while(1){
arnaudsuire 35:3c410cdc4792 84
arnaudsuire 35:3c410cdc4792 85 //vol stationnaire
arnaudsuire 35:3c410cdc4792 86 //attente recuperation commande provenant de trajectoire,
arnaudsuire 35:3c410cdc4792 87 VolStationnaire();
arnaudsuire 34:acc8ea8694b4 88
arnaudsuire 34:acc8ea8694b4 89 // gestion motors
arnaudsuire 35:3c410cdc4792 90 GestionVitesseMotors(m_uiCommand, m_uiDistance);
arnaudsuire 34:acc8ea8694b4 91
arnaudsuire 35:3c410cdc4792 92
arnaudsuire 35:3c410cdc4792 93 // gestion stabilisation
NicolasH 14:ca1bcc05e423 94 }
NicolasH 14:ca1bcc05e423 95 }
NicolasH 14:ca1bcc05e423 96
arnaudsuire 34:acc8ea8694b4 97 void ModuleMouvement::VolStationnaire(void)
arnaudsuire 34:acc8ea8694b4 98 {
arnaudsuire 34:acc8ea8694b4 99 m_motor1->write(0.68); // a def
arnaudsuire 34:acc8ea8694b4 100 m_motor2->write(0.68); // a def
arnaudsuire 34:acc8ea8694b4 101 m_motor3->write(0.68); // a def
arnaudsuire 34:acc8ea8694b4 102 m_motor4->write(0.68); // a def
arnaudsuire 34:acc8ea8694b4 103 /*while ( pas de nouvelle commande )
arnaudsuire 34:acc8ea8694b4 104 {
arnaudsuire 34:acc8ea8694b4 105 wait(0.1);
arnaudsuire 34:acc8ea8694b4 106 }*/
arnaudsuire 34:acc8ea8694b4 107
arnaudsuire 34:acc8ea8694b4 108 return;
arnaudsuire 34:acc8ea8694b4 109 }
arnaudsuire 34:acc8ea8694b4 110
arnaudsuire 34:acc8ea8694b4 111 void ModuleMouvement::GestionVitesseMotors(int m_uiCommand, float m_uiDistance)
arnaudsuire 34:acc8ea8694b4 112 {
arnaudsuire 34:acc8ea8694b4 113 bool CommandFin = false;
arnaudsuire 34:acc8ea8694b4 114
arnaudsuire 34:acc8ea8694b4 115 switch (m_uiCommand)
arnaudsuire 34:acc8ea8694b4 116 {
arnaudsuire 34:acc8ea8694b4 117 /*haut*/
arnaudsuire 34:acc8ea8694b4 118 case 0 :
arnaudsuire 34:acc8ea8694b4 119 //Liste infos;
arnaudsuire 34:acc8ea8694b4 120 break;
arnaudsuire 34:acc8ea8694b4 121
arnaudsuire 34:acc8ea8694b4 122 /*bas*/
arnaudsuire 34:acc8ea8694b4 123 case 1 :
arnaudsuire 34:acc8ea8694b4 124 //liste infos;
arnaudsuire 34:acc8ea8694b4 125 break;
arnaudsuire 34:acc8ea8694b4 126
arnaudsuire 34:acc8ea8694b4 127 /*rotation gauche*/
arnaudsuire 34:acc8ea8694b4 128 case 2 :
arnaudsuire 34:acc8ea8694b4 129 //liste infos;
arnaudsuire 34:acc8ea8694b4 130 break;
arnaudsuire 34:acc8ea8694b4 131
arnaudsuire 34:acc8ea8694b4 132 /*rotation droite*/
arnaudsuire 34:acc8ea8694b4 133 case 3 :
arnaudsuire 34:acc8ea8694b4 134 //Liste infos;
arnaudsuire 34:acc8ea8694b4 135 break;
arnaudsuire 34:acc8ea8694b4 136
arnaudsuire 34:acc8ea8694b4 137 /*gauche*/
arnaudsuire 34:acc8ea8694b4 138 case 4 :
arnaudsuire 34:acc8ea8694b4 139 //Liste infos;
arnaudsuire 34:acc8ea8694b4 140 break;
arnaudsuire 34:acc8ea8694b4 141
arnaudsuire 34:acc8ea8694b4 142 /*droite*/
arnaudsuire 34:acc8ea8694b4 143 case 5 :
arnaudsuire 34:acc8ea8694b4 144 //liste infos;
arnaudsuire 34:acc8ea8694b4 145 break;
arnaudsuire 34:acc8ea8694b4 146
arnaudsuire 34:acc8ea8694b4 147 /*avance*/
arnaudsuire 34:acc8ea8694b4 148 case 6 :
arnaudsuire 34:acc8ea8694b4 149 //Liste infos;
arnaudsuire 34:acc8ea8694b4 150 break;
arnaudsuire 34:acc8ea8694b4 151
arnaudsuire 34:acc8ea8694b4 152 /*recule*/
arnaudsuire 34:acc8ea8694b4 153 case 7 :
arnaudsuire 34:acc8ea8694b4 154 //Liste infos
arnaudsuire 34:acc8ea8694b4 155 break;
arnaudsuire 34:acc8ea8694b4 156
arnaudsuire 34:acc8ea8694b4 157 default :
arnaudsuire 37:e1ad11fe3fe4 158 break;
arnaudsuire 34:acc8ea8694b4 159 /*erreur pas de commande*/
arnaudsuire 34:acc8ea8694b4 160
arnaudsuire 34:acc8ea8694b4 161
arnaudsuire 34:acc8ea8694b4 162 }
arnaudsuire 34:acc8ea8694b4 163 CommandFin = true;
arnaudsuire 34:acc8ea8694b4 164 }
arnaudsuire 34:acc8ea8694b4 165
arnaudsuire 34:acc8ea8694b4 166 void ModuleMouvement::GestionStabilisation(void)
arnaudsuire 34:acc8ea8694b4 167 {
arnaudsuire 34:acc8ea8694b4 168
arnaudsuire 34:acc8ea8694b4 169 }
arnaudsuire 34:acc8ea8694b4 170
arnaudsuire 34:acc8ea8694b4 171 void ModuleMouvement::TestMotor(void)
arnaudsuire 34:acc8ea8694b4 172 {
arnaudsuire 34:acc8ea8694b4 173
arnaudsuire 34:acc8ea8694b4 174 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 34:acc8ea8694b4 175 m_motor1->period(0.002);
arnaudsuire 34:acc8ea8694b4 176
arnaudsuire 34:acc8ea8694b4 177 //initialisation, attends du premier bip
arnaudsuire 34:acc8ea8694b4 178 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 34:acc8ea8694b4 179 {
arnaudsuire 36:660be809a49d 180 m_motor1->write(s);
arnaudsuire 36:660be809a49d 181 m_motor2->write(s);
arnaudsuire 36:660be809a49d 182 m_motor3->write(s);
arnaudsuire 36:660be809a49d 183 m_motor4->write(s);
arnaudsuire 34:acc8ea8694b4 184 wait(0.2);
arnaudsuire 34:acc8ea8694b4 185 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 186 }
arnaudsuire 34:acc8ea8694b4 187 wait(2);
arnaudsuire 34:acc8ea8694b4 188
arnaudsuire 34:acc8ea8694b4 189 m_motor1->write(0.96);
arnaudsuire 36:660be809a49d 190 m_motor2->write(0.96);
arnaudsuire 36:660be809a49d 191 m_motor3->write(0.96);
arnaudsuire 36:660be809a49d 192 m_motor4->write(0.96);
arnaudsuire 34:acc8ea8694b4 193 pc1.printf("test motor");
arnaudsuire 34:acc8ea8694b4 194
arnaudsuire 34:acc8ea8694b4 195
arnaudsuire 34:acc8ea8694b4 196 }
arnaudsuire 34:acc8ea8694b4 197
arnaudsuire 34:acc8ea8694b4 198
arnaudsuire 34:acc8ea8694b4 199 void ModuleMouvement::InitMotor(void)
arnaudsuire 34:acc8ea8694b4 200 {
arnaudsuire 34:acc8ea8694b4 201 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 34:acc8ea8694b4 202 m_motor1->period(0.002);
arnaudsuire 34:acc8ea8694b4 203 m_motor2->period(0.002);
arnaudsuire 34:acc8ea8694b4 204 m_motor3->period(0.002);
arnaudsuire 34:acc8ea8694b4 205 m_motor4->period(0.002);
arnaudsuire 34:acc8ea8694b4 206
arnaudsuire 34:acc8ea8694b4 207 //frequence à 400HZ (attention : init à 0.30 et PWMmax=0.85, trés rapide à l'acceleration )
arnaudsuire 34:acc8ea8694b4 208 //m_motor1->period(0.0025);
arnaudsuire 34:acc8ea8694b4 209
arnaudsuire 34:acc8ea8694b4 210 //initialisation, attends du premier bip
arnaudsuire 34:acc8ea8694b4 211 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 34:acc8ea8694b4 212 {
arnaudsuire 35:3c410cdc4792 213 m_motor1->write(s);
arnaudsuire 35:3c410cdc4792 214 m_motor2->write(s);
arnaudsuire 35:3c410cdc4792 215 m_motor3->write(s);
arnaudsuire 35:3c410cdc4792 216 m_motor4->write(s);
arnaudsuire 35:3c410cdc4792 217 wait(0.02);
arnaudsuire 34:acc8ea8694b4 218 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 219 }
arnaudsuire 34:acc8ea8694b4 220 wait(1);
arnaudsuire 34:acc8ea8694b4 221 }
arnaudsuire 34:acc8ea8694b4 222
arnaudsuire 34:acc8ea8694b4 223
arnaudsuire 34:acc8ea8694b4 224 void ModuleMouvement::DeconnexionMotor(void)
arnaudsuire 34:acc8ea8694b4 225 {
arnaudsuire 34:acc8ea8694b4 226 //deconnexion
arnaudsuire 34:acc8ea8694b4 227 for(float s= 0.40; s > 0; s-= 0.01)
arnaudsuire 34:acc8ea8694b4 228 {
arnaudsuire 34:acc8ea8694b4 229 m_motor1->write(s);
arnaudsuire 35:3c410cdc4792 230 m_motor2->write(s);
arnaudsuire 35:3c410cdc4792 231 m_motor3->write(s);
arnaudsuire 35:3c410cdc4792 232 m_motor4->write(s);
arnaudsuire 35:3c410cdc4792 233 wait(0.02);
arnaudsuire 34:acc8ea8694b4 234 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 235 }
arnaudsuire 34:acc8ea8694b4 236 wait(1);
arnaudsuire 34:acc8ea8694b4 237 }