Denis Brousse / Mbed 2 deprecated AX12-VoiceTronics_DB03-IRQ

Avec mesure de pression

Dependencies:   mbed

Fork of Serial_HelloWorld_Mbed by mbed official

main.cpp@4:0cab1f60b700, 2015-05-27 (annotated)

Committer:
dbrousse
Date:
Wed May 27 14:46:59 2015 +0000
Revision:
4:0cab1f60b700
Parent:
1:b9cade20e07e 
mesure pression

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dbrousse 1:b9cade20e07e 1 /************************************
dbrousse 4:0cab1f60b700 2 * DB le 25/03/2015 modif le 27/05/2015
dbrousse 1:b9cade20e07e 3 * Pilotage AX-12A avec liaison série : Trame VoiceTronics2015
dbrousse 1:b9cade20e07e 4 * Piloter les positions
dbrousse 1:b9cade20e07e 5 * Débit = 1Mbit/s
dbrousse 4:0cab1f60b700 6 * commande 0/10volt Ok
dbrousse 4:0cab1f60b700 7 * mesure pression avec filtrage par moyenne
dbrousse 1:b9cade20e07e 8 *************************************/
dbrousse 1:b9cade20e07e 9
mbed_official 0:879aa9d0247b 10 #include "mbed.h"
dbrousse 1:b9cade20e07e 11 #include "MX-28_DefConstantes.h"
dbrousse 1:b9cade20e07e 12
dbrousse 4:0cab1f60b700 13 #define NBRE_ACK 256 // nombre d'acquisition pour la moyenne sur mesure pression
mbed_official 0:879aa9d0247b 14
dbrousse 1:b9cade20e07e 15 DigitalOut led01(LED1);
dbrousse 1:b9cade20e07e 16 DigitalOut led02(LED2);
dbrousse 1:b9cade20e07e 17 DigitalOut led03(LED3);
dbrousse 1:b9cade20e07e 18 DigitalOut led04(LED4);
dbrousse 1:b9cade20e07e 19 DigitalOut LedRouge(p23);
dbrousse 1:b9cade20e07e 20
dbrousse 1:b9cade20e07e 21 DigitalOut dir(p8);
dbrousse 1:b9cade20e07e 22 AnalogOut servoVanne(p18);
dbrousse 1:b9cade20e07e 23 AnalogIn mesPression(p20);
dbrousse 1:b9cade20e07e 24
dbrousse 1:b9cade20e07e 25 unsigned char numeroOctetRecu=0;
dbrousse 1:b9cade20e07e 26 char octetRecu; //premier octet reçu
dbrousse 1:b9cade20e07e 27 char bufferRec[5]; // buffer de réception serialPc
dbrousse 1:b9cade20e07e 28 int valeurPressionPascal;
dbrousse 1:b9cade20e07e 29
dbrousse 1:b9cade20e07e 30 //Serial serialPc(p28, p27); // tx, rx (c'est l'uart2 du LPC1768)compatible LabVIEW
dbrousse 1:b9cade20e07e 31 Serial serialPc(USBTX, USBRX); // tx, rx, 9600baud par défaut, pas compatible LabVIEW
dbrousse 1:b9cade20e07e 32 Serial uart3(p9, p10); // tx, rx pour servomoteurs numériques
dbrousse 1:b9cade20e07e 33
dbrousse 1:b9cade20e07e 34
dbrousse 1:b9cade20e07e 35 // Envoi de la trame de pilotage a un servomoteur AX-12A
dbrousse 1:b9cade20e07e 36 void write (char id, char longueurTrame, char instruction, char param1 = NULL, char param2 = NULL, char param3 = NULL, char param4 = NULL)
dbrousse 1:b9cade20e07e 37 {
dbrousse 1:b9cade20e07e 38 char Cks;
dbrousse 1:b9cade20e07e 39
dbrousse 1:b9cade20e07e 40 Cks = ~( id + longueurTrame + instruction + param1 + param2 + param3 + param4); //calcul du checkSum
dbrousse 1:b9cade20e07e 41 //serialPc.printf("Cks : %d\n", Cks);
dbrousse 1:b9cade20e07e 42 dir = 1;
dbrousse 1:b9cade20e07e 43 uart3.putc(0xFF);
dbrousse 1:b9cade20e07e 44 uart3.putc(0xFF);
dbrousse 1:b9cade20e07e 45 uart3.putc(id);
dbrousse 1:b9cade20e07e 46 uart3.putc(longueurTrame);
dbrousse 1:b9cade20e07e 47 uart3.putc(instruction);
dbrousse 1:b9cade20e07e 48 if (longueurTrame >= 3) { uart3.putc(param1); }
dbrousse 1:b9cade20e07e 49 if (longueurTrame >= 4) { uart3.putc(param2); }
dbrousse 1:b9cade20e07e 50 if (longueurTrame >= 5) { uart3.putc(param3); }
dbrousse 1:b9cade20e07e 51 if (longueurTrame >= 6) { uart3.putc(param4); }
dbrousse 1:b9cade20e07e 52 uart3.putc(Cks);
dbrousse 1:b9cade20e07e 53
dbrousse 1:b9cade20e07e 54 wait_us(MX28_WAIT_AFTER_WRITE);
dbrousse 1:b9cade20e07e 55 dir = 0;
dbrousse 1:b9cade20e07e 56 }
dbrousse 1:b9cade20e07e 57
dbrousse 1:b9cade20e07e 58 // Set goal position of engine
dbrousse 1:b9cade20e07e 59 void setPosition(char id, int goal)
dbrousse 1:b9cade20e07e 60 {
dbrousse 1:b9cade20e07e 61 char goal_h = goal >> 8;
dbrousse 1:b9cade20e07e 62 char goal_l = goal;
dbrousse 1:b9cade20e07e 63 //serialPc.printf("Goal set : %d %d %d\n", goal, goal_h, goal_l);
dbrousse 1:b9cade20e07e 64 write(id, 5, MX28_WRITE_DATA, MX28_GOAL_POSITION_L, goal_l, goal_h);
dbrousse 1:b9cade20e07e 65 }
dbrousse 1:b9cade20e07e 66
dbrousse 1:b9cade20e07e 67 // Mesure de la pression avec moyenne sur NBRE_ACK
dbrousse 1:b9cade20e07e 68 int mesureDePression() {
dbrousse 1:b9cade20e07e 69 float sommePression=0;
dbrousse 1:b9cade20e07e 70 int valP;
dbrousse 1:b9cade20e07e 71 for(int i=0; i<NBRE_ACK;i++) {
dbrousse 1:b9cade20e07e 72 sommePression += mesPression;
dbrousse 1:b9cade20e07e 73 }
dbrousse 1:b9cade20e07e 74 valP = ((sommePression*3.3*1.7979)/0.000805/NBRE_ACK);
dbrousse 1:b9cade20e07e 75 return valP;
dbrousse 1:b9cade20e07e 76 }
dbrousse 1:b9cade20e07e 77 // Reception d'un octet
dbrousse 1:b9cade20e07e 78 void receptionPc() {
dbrousse 1:b9cade20e07e 79 // fonction appelée par interruption si réception sur serialPc
dbrousse 1:b9cade20e07e 80 led04 =1;
dbrousse 1:b9cade20e07e 81 octetRecu = serialPc.getc();
dbrousse 1:b9cade20e07e 82 if(octetRecu == '$') {
dbrousse 1:b9cade20e07e 83 numeroOctetRecu = 0;
dbrousse 1:b9cade20e07e 84 memset(&bufferRec[0], 0, sizeof(bufferRec));
dbrousse 1:b9cade20e07e 85 }
dbrousse 1:b9cade20e07e 86 else {
dbrousse 1:b9cade20e07e 87 bufferRec[numeroOctetRecu-1] = octetRecu;
dbrousse 1:b9cade20e07e 88 }
dbrousse 1:b9cade20e07e 89 if(numeroOctetRecu == 5) {
dbrousse 1:b9cade20e07e 90 if (bufferRec[0] == '0') // si c'est une commande de position AX12
dbrousse 1:b9cade20e07e 91 {
dbrousse 1:b9cade20e07e 92 int idServo = bufferRec[1] - 0x30;
dbrousse 1:b9cade20e07e 93 int a1 = bufferRec[2] - 0x30;
dbrousse 1:b9cade20e07e 94 int a2 = bufferRec[3] - 0x30;
dbrousse 1:b9cade20e07e 95 int a3 = bufferRec[4] - 0x30;
dbrousse 1:b9cade20e07e 96 int anglePosition = (a1 * 100) + (a2 * 10) + a3;
dbrousse 1:b9cade20e07e 97 int valeurPosition = anglePosition * 1023 / 300;
dbrousse 1:b9cade20e07e 98 setPosition(idServo, valeurPosition);
dbrousse 1:b9cade20e07e 99 //Pour Debug
dbrousse 1:b9cade20e07e 100 //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",idServo, valeurPosition);
dbrousse 1:b9cade20e07e 101 }
dbrousse 1:b9cade20e07e 102 if (bufferRec[0] == '1') // si c'est une commande pour servovanne
dbrousse 1:b9cade20e07e 103 {
dbrousse 1:b9cade20e07e 104 int a1 = bufferRec[2] - 0x30;
dbrousse 1:b9cade20e07e 105 int a2 = bufferRec[3] - 0x30;
dbrousse 1:b9cade20e07e 106 int a3 = bufferRec[4] - 0x30;
dbrousse 1:b9cade20e07e 107 float commandeVanne = ((a1 * 100) + (a2 * 10) + a3)*3/3.3/100; // à étalonner !!!
dbrousse 1:b9cade20e07e 108 servoVanne = commandeVanne;
dbrousse 1:b9cade20e07e 109 //Pour Debug
dbrousse 1:b9cade20e07e 110 //serialPc.printf("Commande servovanne %d %%\n",int (commandeVanne*100*3.3/3));
dbrousse 1:b9cade20e07e 111 }
dbrousse 1:b9cade20e07e 112 if (bufferRec[0] == '2') // si c'est une demande de pression
dbrousse 1:b9cade20e07e 113 {
dbrousse 1:b9cade20e07e 114 //Pour Debug, valeur fixe retournée
dbrousse 1:b9cade20e07e 115 //serialPc.printf("$203625"); // pour test envoie de la mesure=3625Pa
dbrousse 1:b9cade20e07e 116 serialPc.printf("$2%0#5d",valeurPressionPascal); //voir cours Garreta page 85
dbrousse 1:b9cade20e07e 117 }
dbrousse 1:b9cade20e07e 118
dbrousse 1:b9cade20e07e 119 numeroOctetRecu = 0;
dbrousse 1:b9cade20e07e 120 }
dbrousse 1:b9cade20e07e 121 else {
dbrousse 1:b9cade20e07e 122 numeroOctetRecu++;
dbrousse 1:b9cade20e07e 123 }
dbrousse 1:b9cade20e07e 124 led04 =0;
dbrousse 1:b9cade20e07e 125 }
dbrousse 1:b9cade20e07e 126
mbed_official 0:879aa9d0247b 127 int main() {
dbrousse 1:b9cade20e07e 128
dbrousse 1:b9cade20e07e 129 uart3.baud(1000000);
dbrousse 1:b9cade20e07e 130 serialPc.baud(115200);
dbrousse 1:b9cade20e07e 131 serialPc.attach(&receptionPc); // defini la fonction interruption
dbrousse 1:b9cade20e07e 132 wait(1);
dbrousse 1:b9cade20e07e 133 //serialPc.printf("Entrer une commande\n");
dbrousse 1:b9cade20e07e 134 led01 = 1;
dbrousse 1:b9cade20e07e 135 LedRouge=1;
dbrousse 1:b9cade20e07e 136 // Clear buffer
dbrousse 1:b9cade20e07e 137 memset(&bufferRec[0], 0, sizeof(bufferRec));
dbrousse 1:b9cade20e07e 138
mbed_official 0:879aa9d0247b 139 while(1) {
dbrousse 1:b9cade20e07e 140 led01 = 0;
dbrousse 1:b9cade20e07e 141 wait(0.1);
dbrousse 1:b9cade20e07e 142 led01 = 1;
dbrousse 1:b9cade20e07e 143 wait(0.1);
dbrousse 1:b9cade20e07e 144 valeurPressionPascal = mesureDePression(); //mesure cyclique de pression
mbed_official 0:879aa9d0247b 145 }
mbed_official 0:879aa9d0247b 146 }