Projet / Mbed 2 deprecated progprincipale

prog principale

Dependencies:   mbed AX12

main.cpp@2:62ccfb1a8e17, 2017-06-07 (annotated)

Committer:
Cointepas
Date:
Wed Jun 07 18:26:34 2017 +0000
Revision:
2:62ccfb1a8e17
Parent:
1:87582f792133 
final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Cointepas 1:87582f792133 1 #include <mbed.h>
Cointepas 1:87582f792133 2 #include <AX12.h>
Cointepas 1:87582f792133 3 #include <iostream>
Cointepas 1:87582f792133 4 #include <iomanip>
Cointepas 1:87582f792133 5 #include <sstream>
victorien 0:17d8186b53b2 6
victorien 0:17d8186b53b2 7
Cointepas 2:62ccfb1a8e17 8 using namespace std;
Cointepas 2:62ccfb1a8e17 9
Cointepas 2:62ccfb1a8e17 10 DigitalOut led1(LED1);
Cointepas 2:62ccfb1a8e17 11 DigitalOut led2(LED2);
Cointepas 2:62ccfb1a8e17 12 DigitalOut led3(LED3);
Cointepas 2:62ccfb1a8e17 13 DigitalOut led4(LED4);
Cointepas 2:62ccfb1a8e17 14
victorien 0:17d8186b53b2 15 /* ************************************************************ */
victorien 0:17d8186b53b2 16 /* Déclarations variables globales et fonctions pour HMC5883L */
victorien 0:17d8186b53b2 17 /* ************************************************************ */
Cointepas 1:87582f792133 18 short Bx, By, Bz;
victorien 0:17d8186b53b2 19 I2C hmc(p9, p10);
Cointepas 1:87582f792133 20 const int addr = 0x1E; // adresse d'écriture de du capteur
Cointepas 1:87582f792133 21 //int frequency(100000);
victorien 0:17d8186b53b2 22 void hmc_config(void);
victorien 0:17d8186b53b2 23 void hmc_lecture(void);
victorien 0:17d8186b53b2 24
Cointepas 2:62ccfb1a8e17 25 /* ************************************************** */
Cointepas 2:62ccfb1a8e17 26 /* Déclarations des var globales de communication */
Cointepas 2:62ccfb1a8e17 27 /* ************************************************** */
Cointepas 2:62ccfb1a8e17 28 Serial conf(p9, p10);
Cointepas 2:62ccfb1a8e17 29 Serial pc(USBTX, USBRX);
Cointepas 2:62ccfb1a8e17 30
victorien 0:17d8186b53b2 31 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 32 /* Déclarations des var globales de position */
Cointepas 2:62ccfb1a8e17 33 /* ********************************************* */
Cointepas 1:87582f792133 34 int moteur1i;
Cointepas 1:87582f792133 35 int moteur2i;
Cointepas 1:87582f792133 36 int moteur3i;
Cointepas 1:87582f792133 37
Cointepas 2:62ccfb1a8e17 38 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 39 /* Déclarations des var globales de position */
Cointepas 2:62ccfb1a8e17 40 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 41 bool flag;
Cointepas 1:87582f792133 42
Cointepas 2:62ccfb1a8e17 43 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 44 /* Déclarations des var globales moteur */
Cointepas 2:62ccfb1a8e17 45 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 46 AX12 myax01 (p28, p27, 1);
Cointepas 2:62ccfb1a8e17 47 AX12 myax02 (p28, p27, 2);
Cointepas 2:62ccfb1a8e17 48 AX12 myax03 (p28, p27, 3);
Cointepas 2:62ccfb1a8e17 49 AX12 myax04 (p28, p27, 6);
Cointepas 2:62ccfb1a8e17 50
Cointepas 2:62ccfb1a8e17 51 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 52 /* Déclarations des var globales des relever de position */
Cointepas 2:62ccfb1a8e17 53 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 54 float checkmot1;
Cointepas 2:62ccfb1a8e17 55 float checkmot2;
Cointepas 2:62ccfb1a8e17 56 float checkmot3;
Cointepas 2:62ccfb1a8e17 57 float checkmot4;
Cointepas 2:62ccfb1a8e17 58
Cointepas 2:62ccfb1a8e17 59 void Lecture(void);
Cointepas 1:87582f792133 60 void envoi_position(void);
Cointepas 1:87582f792133 61 void lecture_position(void);
Cointepas 1:87582f792133 62 void envoi_12octets(void);
victorien 0:17d8186b53b2 63
Cointepas 2:62ccfb1a8e17 64
victorien 0:17d8186b53b2 65 int main()
victorien 0:17d8186b53b2 66 {
Cointepas 2:62ccfb1a8e17 67 led1 = 1;
Cointepas 2:62ccfb1a8e17 68 Lecture();
Cointepas 2:62ccfb1a8e17 69 led1 = 0;
Cointepas 2:62ccfb1a8e17 70 led2 = 1;
Cointepas 1:87582f792133 71 envoi_position();
Cointepas 2:62ccfb1a8e17 72 led2 = 0;
Cointepas 2:62ccfb1a8e17 73 led3 = 1;
victorien 0:17d8186b53b2 74 hmc_config();
Cointepas 2:62ccfb1a8e17 75 led3 = 0;
Cointepas 2:62ccfb1a8e17 76 led4 = 1;
victorien 0:17d8186b53b2 77 hmc_lecture();
Cointepas 2:62ccfb1a8e17 78 led4 = 1;
Cointepas 2:62ccfb1a8e17 79 led1 = 1;
Cointepas 1:87582f792133 80 lecture_position();
Cointepas 2:62ccfb1a8e17 81 led1 = 0;
Cointepas 2:62ccfb1a8e17 82 led2 = 1;
Cointepas 1:87582f792133 83 envoi_12octets();
Cointepas 2:62ccfb1a8e17 84 led1 = 1;
Cointepas 2:62ccfb1a8e17 85 led2 = 1;
Cointepas 2:62ccfb1a8e17 86 led3 = 1;
Cointepas 2:62ccfb1a8e17 87 led4 = 1;
victorien 0:17d8186b53b2 88
victorien 0:17d8186b53b2 89
Cointepas 2:62ccfb1a8e17 90 // pc.printf("\nBx=%d, By=%d, Bz=%d\n\r",Bx*4.35,By*4.35,Bz*4.35);
victorien 0:17d8186b53b2 91
victorien 0:17d8186b53b2 92
victorien 0:17d8186b53b2 93
victorien 0:17d8186b53b2 94
victorien 0:17d8186b53b2 95 }
victorien 0:17d8186b53b2 96
victorien 0:17d8186b53b2 97 /* ************************************** */
victorien 0:17d8186b53b2 98 /* Fonction de confihuration de HMC5883l */
victorien 0:17d8186b53b2 99 /* Mode:single, sensibilité +/- 8Guauss */
victorien 0:17d8186b53b2 100 /* */
victorien 0:17d8186b53b2 101 /* ************************************** */
victorien 0:17d8186b53b2 102 void hmc_config(void)
victorien 0:17d8186b53b2 103 {
Cointepas 2:62ccfb1a8e17 104 conf.baud(9600);
Cointepas 1:87582f792133 105 //Variable interne à la fonction.
Cointepas 1:87582f792133 106 char config_hmc[2];
Cointepas 1:87582f792133 107 config_hmc[0]=0x00; // adresse registre A
Cointepas 1:87582f792133 108 config_hmc[1]=0x70;// registre A configuré en mesure normal sur 16 bits
Cointepas 1:87582f792133 109 hmc.write(addr, config_hmc,2);
victorien 0:17d8186b53b2 110
Cointepas 1:87582f792133 111 config_hmc[0]=0x01; // à commenter
Cointepas 1:87582f792133 112 config_hmc[1]=0xE0;
Cointepas 1:87582f792133 113 hmc.write(addr, config_hmc,2);
victorien 0:17d8186b53b2 114
Cointepas 1:87582f792133 115 config_hmc[0]=0x02; // à commenter
Cointepas 1:87582f792133 116 config_hmc[1]=0x01;
Cointepas 1:87582f792133 117 hmc.write(addr, config_hmc,2);
Cointepas 1:87582f792133 118
Cointepas 1:87582f792133 119 wait(0.006); // attente imposée constructeur
victorien 0:17d8186b53b2 120
victorien 0:17d8186b53b2 121 }
victorien 0:17d8186b53b2 122
victorien 0:17d8186b53b2 123 /* ************************************** */
victorien 0:17d8186b53b2 124 /* Fonction de lecture champ magnétique */
victorien 0:17d8186b53b2 125 /* sur 3 axes, chaque lecture sur 16 bits */
victorien 0:17d8186b53b2 126 /* ************************************** */
victorien 0:17d8186b53b2 127
victorien 0:17d8186b53b2 128 void hmc_lecture()
victorien 0:17d8186b53b2 129 {
Cointepas 1:87582f792133 130 //Variable interne a la fonction.
Cointepas 1:87582f792133 131 char readhmc[6];
victorien 0:17d8186b53b2 132
victorien 0:17d8186b53b2 133 hmc.read(addr, readhmc, 6); // commenter
victorien 0:17d8186b53b2 134
victorien 0:17d8186b53b2 135 Bx=((readhmc[0] << 8)|readhmc[1]); // commenter le décalage
victorien 0:17d8186b53b2 136 By=((readhmc[2] << 8)|readhmc[3]);
victorien 0:17d8186b53b2 137 Bz=((readhmc[4] << 8)|readhmc[5]);
victorien 0:17d8186b53b2 138
victorien 0:17d8186b53b2 139 wait(0.07);
victorien 0:17d8186b53b2 140
victorien 0:17d8186b53b2 141 }
Cointepas 2:62ccfb1a8e17 142 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 143 /* Fonction de lecture de la trame */
Cointepas 1:87582f792133 144 /* ********************************************* */
Cointepas 2:62ccfb1a8e17 145
Cointepas 2:62ccfb1a8e17 146 void Lecture(void) {
Cointepas 2:62ccfb1a8e17 147
Cointepas 1:87582f792133 148 string trame; //Trame complète.
Cointepas 1:87582f792133 149 stringstream sstr; //stringstream permet de faire la liaison entre le string de départ et le int sortant.
Cointepas 2:62ccfb1a8e17 150 int cchecksum = 0; //Octet permetant le controle de 'checksumi'.
Cointepas 1:87582f792133 151 string moteur1; //Octet de position moteur1.
Cointepas 1:87582f792133 152 string moteur2; //Octet de position moteur2.
Cointepas 1:87582f792133 153 string moteur3; //Octet de position moteur3.
Cointepas 1:87582f792133 154 string checksum; //Octet du checksum.
Cointepas 1:87582f792133 155 int checksumi; //'checksum' converti en int.
Cointepas 2:62ccfb1a8e17 156 char tablecture[15]; //Tableau de reception
Cointepas 2:62ccfb1a8e17 157 char reception[1];
Cointepas 2:62ccfb1a8e17 158 int tablecturei[15]; //Tableau de reception converti en int
Cointepas 2:62ccfb1a8e17 159 string tab; //Octet de start.
Cointepas 2:62ccfb1a8e17 160 int tabi[14]; //'start1' converti en int.
Cointepas 2:62ccfb1a8e17 161 string start1; //Octet de start.
Cointepas 2:62ccfb1a8e17 162 int start1i; //'start1' converti en int.
Cointepas 2:62ccfb1a8e17 163 string start2; //Octet de start.
Cointepas 2:62ccfb1a8e17 164 int start2i; //'start2' converti en int.
Cointepas 2:62ccfb1a8e17 165 string start3; //Octet de start.
Cointepas 2:62ccfb1a8e17 166 int start3i; //'start1' converti en int.
Cointepas 2:62ccfb1a8e17 167 string start4; //Octet de start.
Cointepas 2:62ccfb1a8e17 168 int start4i; //'start2' converti en int.
Cointepas 2:62ccfb1a8e17 169 conf.baud(9600);
Cointepas 2:62ccfb1a8e17 170
Cointepas 2:62ccfb1a8e17 171
Cointepas 2:62ccfb1a8e17 172 //tablecture = conf.getc();
Cointepas 1:87582f792133 173
Cointepas 2:62ccfb1a8e17 174 for(int i = 0; i<15; i++){
Cointepas 2:62ccfb1a8e17 175 conf.gets(reception, 2);
Cointepas 2:62ccfb1a8e17 176 pc.printf("\nreception = %s \n",reception);
Cointepas 2:62ccfb1a8e17 177 trame += reception;
Cointepas 2:62ccfb1a8e17 178 //tablecture[i] = conf.getc(); //Récupère la trame complète sur 15 Octets dans un trableau de char.
Cointepas 2:62ccfb1a8e17 179 //conf.scanf("%d", &tablecture[i]);
Cointepas 2:62ccfb1a8e17 180 //pc.printf("\ntablecture[%d] = %s \n",i,tablecture[i]);
Cointepas 2:62ccfb1a8e17 181 //pc.printf("\ntablecturei[%d] = %d \n",i,tablecturei[i]);
Cointepas 2:62ccfb1a8e17 182 //trame = tablecture[i];
Cointepas 2:62ccfb1a8e17 183 }
Cointepas 1:87582f792133 184
Cointepas 2:62ccfb1a8e17 185 //trame = tablecture; //Copie le contenue de 'tablecture' dans 'trame'.
Cointepas 2:62ccfb1a8e17 186 pc.printf("\ntrame = %s \n",trame);
Cointepas 2:62ccfb1a8e17 187 led1 = 1;
Cointepas 1:87582f792133 188
Cointepas 2:62ccfb1a8e17 189 for(int i=0; i<13; i++){
Cointepas 2:62ccfb1a8e17 190 tab = trame.substr(i,1);
Cointepas 2:62ccfb1a8e17 191 tabi[i] = tab[0];
Cointepas 2:62ccfb1a8e17 192 cchecksum += tabi[i];
Cointepas 2:62ccfb1a8e17 193 pc.printf("\ntab[%d] = %s \n", i, tab);
Cointepas 2:62ccfb1a8e17 194 pc.printf("\ntabi[%d] = %d \n", i, tabi[i]);
Cointepas 2:62ccfb1a8e17 195 pc.printf("\ncchecksum = %d \n",cchecksum);
Cointepas 2:62ccfb1a8e17 196 }
Cointepas 2:62ccfb1a8e17 197
Cointepas 2:62ccfb1a8e17 198 pc.printf("\ncchecksum = %d \n",cchecksum);
Cointepas 2:62ccfb1a8e17 199 moteur1 = trame.substr(4,3); //Enregistrement de deux Nombres à partir de 4.
Cointepas 1:87582f792133 200 sstr << moteur1; //Stock le contenue du string 'start2' dans le buffer 'sstr'.
Cointepas 1:87582f792133 201 sstr >> dec >> moteur1i; //Enregistre le contenue du buffer 'sstr' en hexa dans l'int 'start2i'.
Cointepas 1:87582f792133 202 sstr.clear(); //Vide le buffer de 'sstr'.
Cointepas 2:62ccfb1a8e17 203 pc.printf("\nmoteur1 = %s \n",moteur1);
Cointepas 2:62ccfb1a8e17 204 pc.printf("\nmoteur1i = %d \n",moteur1i);
Cointepas 1:87582f792133 205
Cointepas 2:62ccfb1a8e17 206 moteur2 = trame.substr(7,3); //Enregistrement de deux Nombres à partir de 6.
Cointepas 1:87582f792133 207 sstr << moteur2; //Stock le contenue du string 'start2' dans le buffer 'sstr'.
Cointepas 1:87582f792133 208 sstr >> dec >> moteur2i; //Enregistre le contenue du buffer 'sstr' en hexa dans l'int 'start2i'.
Cointepas 1:87582f792133 209 sstr.clear(); //Vide le buffer de 'sstr'.
Cointepas 2:62ccfb1a8e17 210 pc.printf("\nmoteur2 = %s \n",moteur2);
Cointepas 2:62ccfb1a8e17 211 pc.printf("\nmoteur2i = %d \n",moteur2i);
Cointepas 1:87582f792133 212
Cointepas 2:62ccfb1a8e17 213 moteur3 = trame.substr(10,3); //Enregistrement de deux Nombres à partir de 8.
Cointepas 1:87582f792133 214 sstr << moteur3; //Stock le contenue du string 'start2' dans le buffer 'sstr'.
Cointepas 1:87582f792133 215 sstr >> dec >> moteur3i; //Enregistre le contenue du buffer 'sstr' en hexa dans l'int 'start2i'.
Cointepas 1:87582f792133 216 sstr.clear(); //Vide le buffer de 'sstr'.
Cointepas 1:87582f792133 217 led2 = 0;
Cointepas 2:62ccfb1a8e17 218 pc.printf("\nmoteur3 = %s \n",moteur3);
Cointepas 2:62ccfb1a8e17 219 pc.printf("\nmoteur3i = %d \n",moteur3i);
Cointepas 1:87582f792133 220
Cointepas 2:62ccfb1a8e17 221 checksum = trame.substr(13,2); //Enregistrement de deux Nombres à partir de 10.
Cointepas 1:87582f792133 222 sstr << checksum; //Stock le contenue du string 'checksum' dans le buffer 'sstr'.
Cointepas 1:87582f792133 223 sstr >> dec >> checksumi; //Enregistre le contenue du buffer 'sstr' en hexa dans l'int 'checksumi'.
Cointepas 1:87582f792133 224 sstr.clear(); //Vide le buffer de 'sstr'.
Cointepas 2:62ccfb1a8e17 225 pc.printf("\nchecksum = %s \n",checksum);
Cointepas 2:62ccfb1a8e17 226 pc.printf("\nchecksumi = %d \n",checksumi);
Cointepas 1:87582f792133 227
Cointepas 2:62ccfb1a8e17 228 cchecksum = cchecksum % 256; //'cchecksum' % 256.
Cointepas 2:62ccfb1a8e17 229 pc.printf("\ncchecksum = %d modulo 256\n",cchecksum);
Cointepas 2:62ccfb1a8e17 230 cchecksum = cchecksum % 100; //'cchecksum' % 100.
Cointepas 2:62ccfb1a8e17 231 pc.printf("\ncchecksum = %d modulo 100\n",cchecksum);
Cointepas 1:87582f792133 232
Cointepas 1:87582f792133 233
Cointepas 2:62ccfb1a8e17 234 if((cchecksum) == checksumi){
Cointepas 2:62ccfb1a8e17 235 led2 = 1;
Cointepas 2:62ccfb1a8e17 236 flag = 1;
Cointepas 1:87582f792133 237 }
Cointepas 1:87582f792133 238 else{
Cointepas 2:62ccfb1a8e17 239 led3 = 1;
Cointepas 2:62ccfb1a8e17 240 flag = 0;
Cointepas 1:87582f792133 241 }
Cointepas 1:87582f792133 242 }
Cointepas 1:87582f792133 243
Cointepas 1:87582f792133 244 /* ********************************************* */
Cointepas 1:87582f792133 245 /* Fonction d'envoi de position moteur */
Cointepas 1:87582f792133 246 /* ********************************************* */
Cointepas 1:87582f792133 247
Cointepas 2:62ccfb1a8e17 248 void envoi_position(void){
Cointepas 2:62ccfb1a8e17 249 int moteur4i;
Cointepas 2:62ccfb1a8e17 250 myax01.SetGoal(moteur1i); //Va a la position 'moteur1i'.
Cointepas 2:62ccfb1a8e17 251 wait(0.25); //Attente de 0.25sec avant de commencer a placer le moteur suivant.
Cointepas 2:62ccfb1a8e17 252 myax02.SetGoal(moteur2i); //Va a la position 'moteur2i'.
Cointepas 2:62ccfb1a8e17 253 wait(0.25); //Attente de 0.25sec avant de commencer a placer le moteur suivant.
Cointepas 2:62ccfb1a8e17 254 myax03.SetGoal(moteur3i); //Va a la position 'moteur3i'.
Cointepas 2:62ccfb1a8e17 255 moteur4i = 300 - moteur3i; //Calcul pour la position du moteur myax03.SetGoal.
Cointepas 2:62ccfb1a8e17 256 myax04.SetGoal(moteur4i); //Va a la position 'moteur4i'.
Cointepas 2:62ccfb1a8e17 257 wait(3);
Cointepas 2:62ccfb1a8e17 258 }
Cointepas 1:87582f792133 259
Cointepas 1:87582f792133 260 /* ********************************************* */
Cointepas 1:87582f792133 261 /* Fonction de lecture de position moteur */
Cointepas 1:87582f792133 262 /* ********************************************* */
Cointepas 1:87582f792133 263
Cointepas 2:62ccfb1a8e17 264 void lecture_position(void){
Cointepas 2:62ccfb1a8e17 265 checkmot1 = myax01.GetPosition(); //Récupération de la position du moteur 'myax01'.
Cointepas 2:62ccfb1a8e17 266 checkmot2 = myax02.GetPosition(); //Récupération de la position du moteur 'myax02'.
Cointepas 2:62ccfb1a8e17 267 checkmot3 = myax03.GetPosition(); //Récupération de la position du moteur 'myax03'.
Cointepas 2:62ccfb1a8e17 268 checkmot4 = myax04.GetPosition(); //Récupération de la position du moteur 'myax04'.
Cointepas 2:62ccfb1a8e17 269 }
Cointepas 1:87582f792133 270 /* ********************************************* */
Cointepas 1:87582f792133 271 /* Fonction d'envoi de la trame sur 12 Octets */
Cointepas 1:87582f792133 272 /* ********************************************* */
Cointepas 1:87582f792133 273 void envoi_12octets(void){
Cointepas 2:62ccfb1a8e17 274 Serial conf(p13, p14); //(tx, rx, baud)
Cointepas 1:87582f792133 275 //Varible interne a la fonction
Cointepas 1:87582f792133 276 int start3 = 0xFF; //Octet de start
Cointepas 1:87582f792133 277 int start4 = 0xFF; //Octet de start
Cointepas 1:87582f792133 278 int echecksum; //Octet de Checksum
Cointepas 2:62ccfb1a8e17 279 conf.baud(9600);
Cointepas 1:87582f792133 280
Cointepas 1:87582f792133 281
Cointepas 1:87582f792133 282 led1 = 0;
Cointepas 1:87582f792133 283
Cointepas 1:87582f792133 284 conf.putc(start3);
Cointepas 1:87582f792133 285 echecksum += start3;
Cointepas 1:87582f792133 286 conf.putc(start4);
Cointepas 1:87582f792133 287 echecksum += start4;
Cointepas 1:87582f792133 288 conf.putc(checkmot1);
Cointepas 1:87582f792133 289 echecksum += checkmot1;
Cointepas 1:87582f792133 290 conf.putc(checkmot2);
Cointepas 1:87582f792133 291 echecksum += checkmot2;
Cointepas 1:87582f792133 292 conf.putc(checkmot3);
Cointepas 1:87582f792133 293 echecksum += checkmot3;
Cointepas 1:87582f792133 294 conf.putc(Bx);
Cointepas 1:87582f792133 295 echecksum += Bx;
Cointepas 1:87582f792133 296 conf.putc(By);
Cointepas 1:87582f792133 297 echecksum += By;
Cointepas 1:87582f792133 298 conf.putc(Bz);
Cointepas 1:87582f792133 299 echecksum += Bz;
Cointepas 1:87582f792133 300 conf.putc(echecksum);
Cointepas 1:87582f792133 301
Cointepas 1:87582f792133 302 led3 = 1;
Cointepas 1:87582f792133 303 }