Arnaud Suire / Labo_TRSE_Drone

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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@34:acc8ea8694b4, 2013-04-22 (annotated)

Committer:
arnaudsuire
Date:
Mon Apr 22 19:13:35 2013 +0000
Revision:
34:acc8ea8694b4
Parent:
15:793cf784dc7a
Child:
35:3c410cdc4792
ajout module_Mouvement (init, deconnexion)

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
arnaudsuire 34:acc8ea8694b4 28
arnaudsuire 34:acc8ea8694b4 29
NicolasH 14:ca1bcc05e423 30
NicolasH 14:ca1bcc05e423 31 /* CONSRTRUCTEUR(S) */
NicolasH 14:ca1bcc05e423 32 ModuleMouvement::ModuleMouvement()
NicolasH 14:ca1bcc05e423 33 {
arnaudsuire 34:acc8ea8694b4 34 m_motor1 = new PwmOut (p21);
arnaudsuire 34:acc8ea8694b4 35 m_motor2 = new PwmOut (p22);
arnaudsuire 34:acc8ea8694b4 36 m_motor3 = new PwmOut (p23);
arnaudsuire 34:acc8ea8694b4 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 34:acc8ea8694b4 43 delete m_motor1;
arnaudsuire 34:acc8ea8694b4 44 delete m_motor2;
arnaudsuire 34:acc8ea8694b4 45 delete m_motor3;
arnaudsuire 34:acc8ea8694b4 46 delete m_motor4;
arnaudsuire 34:acc8ea8694b4 47 }
NicolasH 14:ca1bcc05e423 48
arnaudsuire 34:acc8ea8694b4 49
arnaudsuire 34:acc8ea8694b4 50 /*Get*/
arnaudsuire 34:acc8ea8694b4 51 int ModuleMouvement::GetpVitesseFonctionnement()
arnaudsuire 34:acc8ea8694b4 52 { return pVitesseFonctionnement; }
arnaudsuire 34:acc8ea8694b4 53
arnaudsuire 34:acc8ea8694b4 54 int ModuleMouvement::GetuiCommand()
arnaudsuire 34:acc8ea8694b4 55 { return m_uiCommand; }
arnaudsuire 34:acc8ea8694b4 56
arnaudsuire 34:acc8ea8694b4 57 float ModuleMouvement::GetuiDistance()
arnaudsuire 34:acc8ea8694b4 58 { return m_uiDistance; }
arnaudsuire 34:acc8ea8694b4 59
arnaudsuire 34:acc8ea8694b4 60
arnaudsuire 34:acc8ea8694b4 61 /*Set*/
arnaudsuire 34:acc8ea8694b4 62 void ModuleMouvement::SetpVitesseFonctionnement(int m)
arnaudsuire 34:acc8ea8694b4 63 { pVitesseFonctionnement = m ; }
arnaudsuire 34:acc8ea8694b4 64
arnaudsuire 34:acc8ea8694b4 65 void ModuleMouvement::SetuiCommand(int n)
arnaudsuire 34:acc8ea8694b4 66 { m_uiCommand = n ; }
arnaudsuire 34:acc8ea8694b4 67
arnaudsuire 34:acc8ea8694b4 68 void ModuleMouvement::SetuiDistance(float o)
arnaudsuire 34:acc8ea8694b4 69 { m_uiDistance = o ; }
arnaudsuire 34:acc8ea8694b4 70
arnaudsuire 34:acc8ea8694b4 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 34:acc8ea8694b4 76 //init moteurs
arnaudsuire 34:acc8ea8694b4 77 InitMotor();
arnaudsuire 34:acc8ea8694b4 78
arnaudsuire 34:acc8ea8694b4 79 //décollage
arnaudsuire 34:acc8ea8694b4 80 m_uiCommand = 0;
arnaudsuire 34:acc8ea8694b4 81 m_uiDistance = 0.6; //60cm
arnaudsuire 34:acc8ea8694b4 82 GestionVitesseMotors(m_uiCommand, m_uiDistance);
arnaudsuire 34:acc8ea8694b4 83
arnaudsuire 34:acc8ea8694b4 84 //vol stationnaire
arnaudsuire 34:acc8ea8694b4 85 VolStationnaire();
arnaudsuire 34:acc8ea8694b4 86
arnaudsuire 34:acc8ea8694b4 87
NicolasH 14:ca1bcc05e423 88 while(1){
arnaudsuire 34:acc8ea8694b4 89
arnaudsuire 34:acc8ea8694b4 90 // recuperation commande provenant de trajectoire, comment recup, quel format, quel taille?
arnaudsuire 34:acc8ea8694b4 91 // gestion motors
arnaudsuire 34:acc8ea8694b4 92
arnaudsuire 34:acc8ea8694b4 93 // gestion stabilisation, get services?
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 34:acc8ea8694b4 158 /*erreur pas de commande*/
arnaudsuire 34:acc8ea8694b4 159
arnaudsuire 34:acc8ea8694b4 160
arnaudsuire 34:acc8ea8694b4 161 }
arnaudsuire 34:acc8ea8694b4 162 CommandFin = true;
arnaudsuire 34:acc8ea8694b4 163 }
arnaudsuire 34:acc8ea8694b4 164
arnaudsuire 34:acc8ea8694b4 165 void ModuleMouvement::GestionStabilisation(void)
arnaudsuire 34:acc8ea8694b4 166 {
arnaudsuire 34:acc8ea8694b4 167
arnaudsuire 34:acc8ea8694b4 168 }
arnaudsuire 34:acc8ea8694b4 169
arnaudsuire 34:acc8ea8694b4 170 void ModuleMouvement::TestMotor(void)
arnaudsuire 34:acc8ea8694b4 171 {
arnaudsuire 34:acc8ea8694b4 172
arnaudsuire 34:acc8ea8694b4 173 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 34:acc8ea8694b4 174 m_motor1->period(0.002);
arnaudsuire 34:acc8ea8694b4 175
arnaudsuire 34:acc8ea8694b4 176 //initialisation, attends du premier bip
arnaudsuire 34:acc8ea8694b4 177 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 34:acc8ea8694b4 178 {
arnaudsuire 34:acc8ea8694b4 179 m_motor1->write(s);
arnaudsuire 34:acc8ea8694b4 180 wait(0.2);
arnaudsuire 34:acc8ea8694b4 181 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 182 }
arnaudsuire 34:acc8ea8694b4 183 wait(2);
arnaudsuire 34:acc8ea8694b4 184
arnaudsuire 34:acc8ea8694b4 185 m_motor1->write(0.96);
arnaudsuire 34:acc8ea8694b4 186 pc1.printf("test motor");
arnaudsuire 34:acc8ea8694b4 187
arnaudsuire 34:acc8ea8694b4 188
arnaudsuire 34:acc8ea8694b4 189 }
arnaudsuire 34:acc8ea8694b4 190
arnaudsuire 34:acc8ea8694b4 191
arnaudsuire 34:acc8ea8694b4 192 void ModuleMouvement::InitMotor(void)
arnaudsuire 34:acc8ea8694b4 193 {
arnaudsuire 34:acc8ea8694b4 194 //frequence à 500Hz (init à 0.40 PWMmax = 0.99)
arnaudsuire 34:acc8ea8694b4 195 m_motor1->period(0.002);
arnaudsuire 34:acc8ea8694b4 196 m_motor2->period(0.002);
arnaudsuire 34:acc8ea8694b4 197 m_motor3->period(0.002);
arnaudsuire 34:acc8ea8694b4 198 m_motor4->period(0.002);
arnaudsuire 34:acc8ea8694b4 199
arnaudsuire 34:acc8ea8694b4 200 //frequence à 400HZ (attention : init à 0.30 et PWMmax=0.85, trés rapide à l'acceleration )
arnaudsuire 34:acc8ea8694b4 201 //m_motor1->period(0.0025);
arnaudsuire 34:acc8ea8694b4 202
arnaudsuire 34:acc8ea8694b4 203 //initialisation, attends du premier bip
arnaudsuire 34:acc8ea8694b4 204 for (float s= 0; s < 0.40; s += 0.01)
arnaudsuire 34:acc8ea8694b4 205 {
arnaudsuire 34:acc8ea8694b4 206 m_motor1->write(s);
arnaudsuire 34:acc8ea8694b4 207 wait(0.2);
arnaudsuire 34:acc8ea8694b4 208 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 209 }
arnaudsuire 34:acc8ea8694b4 210 wait(1);
arnaudsuire 34:acc8ea8694b4 211 }
arnaudsuire 34:acc8ea8694b4 212
arnaudsuire 34:acc8ea8694b4 213
arnaudsuire 34:acc8ea8694b4 214 void ModuleMouvement::DeconnexionMotor(void)
arnaudsuire 34:acc8ea8694b4 215 {
arnaudsuire 34:acc8ea8694b4 216 //deconnexion
arnaudsuire 34:acc8ea8694b4 217 for(float s= 0.40; s > 0; s-= 0.01)
arnaudsuire 34:acc8ea8694b4 218 {
arnaudsuire 34:acc8ea8694b4 219 m_motor1->write(s);
arnaudsuire 34:acc8ea8694b4 220 wait(0.01);
arnaudsuire 34:acc8ea8694b4 221 pc1.printf("%f", s);
arnaudsuire 34:acc8ea8694b4 222 }
arnaudsuire 34:acc8ea8694b4 223 wait(1);
arnaudsuire 34:acc8ea8694b4 224 }