Denis Brousse
/
AX12-VoiceTronics_DB03-IRQ
Avec mesure de pression
Fork of Serial_HelloWorld_Mbed by
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?
User | Revision | Line number | New 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 | } |