TER Atienza Pongnot 2019 / Mbed 2 deprecated Carte_Moteur_test_asservissement_1M

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 7366_lib TLE5206_lib

main.cpp@9:eb3f9744ae5d, 2019-03-16 (annotated)

Committer:
natienza
Date:
Sat Mar 16 08:45:31 2019 +0000
Revision:
9:eb3f9744ae5d
Parent:
8:91eb7435c3e0 
asserv test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gpongnot 0:509b29d50fcb 1 //Includes
gpongnot 0:509b29d50fcb 2 #include "mbed.h"
gpongnot 0:509b29d50fcb 3 #include "7366_lib.h"
gpongnot 0:509b29d50fcb 4 #include "TLE5206_lib.h"
gpongnot 0:509b29d50fcb 5
gpongnot 3:737ac9c24ca5 6 // Caractéristiques de structure
gpongnot 4:80f612396136 7 #define RAYON_ROUE 0.025 // [m]
gpongnot 4:80f612396136 8 #define DIST1 0.15 // [m]
gpongnot 4:80f612396136 9 #define DIST2 0.15 // [m]
gpongnot 4:80f612396136 10 #define DIST3 0.0075 // [m]
gpongnot 3:737ac9c24ca5 11 #define PI 3.14159265359
gpongnot 4:80f612396136 12 #define RESOLUTION_ENCO 14336 // = 14*1024 = rapport_reduction * nbr de top par tour
gpongnot 4:80f612396136 13 #define ETALONNAGE_LACET 0.88
gpongnot 4:80f612396136 14 #define ETALONNAGE_XY 1.065
gpongnot 0:509b29d50fcb 15 // Liaison SPI avec les compteurs
gpongnot 0:509b29d50fcb 16 #define SPI_SCLK PA_5 //A4
gpongnot 0:509b29d50fcb 17 #define SPI_MISO PA_6 //A5
gpongnot 0:509b29d50fcb 18 #define SPI_MOSI PA_7 //A6
gpongnot 0:509b29d50fcb 19 #define SPI_CS3 PA_0//A0
gpongnot 0:509b29d50fcb 20 #define SPI_CS2 PA_1
gpongnot 0:509b29d50fcb 21 #define SPI_CS1 PA_3
gpongnot 0:509b29d50fcb 22
gpongnot 0:509b29d50fcb 23 // Vers le pont en H
gpongnot 0:509b29d50fcb 24 #define H1_IN1 PA_9 //D1 - PWM1/2
gpongnot 0:509b29d50fcb 25 #define H1_IN2 PA_10 //D0 - PWM1/3
gpongnot 0:509b29d50fcb 26 #define H2_IN1 PA_8 //D9 - PWM1/1
gpongnot 0:509b29d50fcb 27 #define H2_IN2 PB_7 //D4 - PWM17/1
gpongnot 0:509b29d50fcb 28 #define H3_IN1 PB_6 //D5 - PWM16/1
gpongnot 0:509b29d50fcb 29 #define H3_IN2 PA_4 //A3 - PWM3/2
gpongnot 0:509b29d50fcb 30
gpongnot 0:509b29d50fcb 31 #define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
natienza 2:486bb9b6bd78 32 #define PERIODE_AFF 500 //ms
natienza 9:eb3f9744ae5d 33 #define PERIODE_ASSERV 25 //ms
natienza 9:eb3f9744ae5d 34 #define Te 25 //ms /!\ Te doit être égale à la période d'asservissement
gpongnot 0:509b29d50fcb 35
gpongnot 3:737ac9c24ca5 36 // Constantes Asservissement
natienza 9:eb3f9744ae5d 37 #define GAIN_POS 1.4
natienza 9:eb3f9744ae5d 38 #define GAIN_ANG 5
natienza 9:eb3f9744ae5d 39 #define Ti 5000000 //ms
gpongnot 6:85ade96c99b1 40 #define GAIN_POS_INT 0.1
gpongnot 6:85ade96c99b1 41 #define GAIN_ANG_INT 0.1
natienza 9:eb3f9744ae5d 42 #define ERREUR_POS 0
natienza 9:eb3f9744ae5d 43 #define ERREUR_ANG 0
gpongnot 3:737ac9c24ca5 44
natienza 2:486bb9b6bd78 45 Serial pc(USBTX,USBRX);
natienza 2:486bb9b6bd78 46 Timer timer;
gpongnot 0:509b29d50fcb 47 DigitalOut myled(LED3);
gpongnot 0:509b29d50fcb 48
gpongnot 0:509b29d50fcb 49 SPI_7366 compt1(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS1);
gpongnot 0:509b29d50fcb 50 SPI_7366 compt2(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS2);
gpongnot 0:509b29d50fcb 51 SPI_7366 compt3(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS3);
gpongnot 0:509b29d50fcb 52
gpongnot 0:509b29d50fcb 53 TLE5206 moteur1(H1_IN1,H1_IN2);
gpongnot 0:509b29d50fcb 54 TLE5206 moteur2(H2_IN1,H2_IN2);
gpongnot 0:509b29d50fcb 55 TLE5206 moteur3(H3_IN1,H3_IN2);
gpongnot 0:509b29d50fcb 56
gpongnot 3:737ac9c24ca5 57 struct Vect3{
gpongnot 3:737ac9c24ca5 58 double x;
gpongnot 3:737ac9c24ca5 59 double y;
gpongnot 3:737ac9c24ca5 60 double z;
gpongnot 3:737ac9c24ca5 61 };
gpongnot 3:737ac9c24ca5 62
gpongnot 3:737ac9c24ca5 63
gpongnot 3:737ac9c24ca5 64 struct Vect2{
gpongnot 3:737ac9c24ca5 65 float x;
gpongnot 3:737ac9c24ca5 66 float y;
gpongnot 3:737ac9c24ca5 67 };
gpongnot 3:737ac9c24ca5 68
gpongnot 3:737ac9c24ca5 69 Vect3 initVect3(){
gpongnot 3:737ac9c24ca5 70 Vect3 result;
gpongnot 3:737ac9c24ca5 71 result.x = 0;
gpongnot 3:737ac9c24ca5 72 result.y = 0;
gpongnot 3:737ac9c24ca5 73 result.z = 0;
gpongnot 3:737ac9c24ca5 74 return result;
gpongnot 3:737ac9c24ca5 75 }
gpongnot 3:737ac9c24ca5 76
gpongnot 3:737ac9c24ca5 77 Vect3 calculatePosition(){
gpongnot 3:737ac9c24ca5 78 static Vect3 theta = initVect3();
gpongnot 3:737ac9c24ca5 79 static Vect3 position = initVect3();
gpongnot 3:737ac9c24ca5 80 Vect3 dTheta;
gpongnot 3:737ac9c24ca5 81 double dPsi = 0;
gpongnot 3:737ac9c24ca5 82
gpongnot 3:737ac9c24ca5 83 dTheta.x = 2*PI/RESOLUTION_ENCO*compt2.read_value() - theta.x;
gpongnot 3:737ac9c24ca5 84 dTheta.y = 2*PI/RESOLUTION_ENCO*compt1.read_value() - theta.y;
gpongnot 3:737ac9c24ca5 85 dTheta.z = 2*PI/RESOLUTION_ENCO*compt3.read_value() - theta.z;
gpongnot 3:737ac9c24ca5 86 theta.x += dTheta.x;
gpongnot 3:737ac9c24ca5 87 theta.y += dTheta.y;
gpongnot 3:737ac9c24ca5 88 theta.z += dTheta.z;
gpongnot 3:737ac9c24ca5 89
gpongnot 4:80f612396136 90 dPsi = ETALONNAGE_LACET*RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
natienza 2:486bb9b6bd78 91
gpongnot 3:737ac9c24ca5 92 position.z += dPsi;// Psi actuel
gpongnot 3:737ac9c24ca5 93
gpongnot 4:80f612396136 94 position.x += ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) * cos(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) * cos(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*cos(position.z + 4*PI/3))*2/3;
gpongnot 4:80f612396136 95 position.y +=-ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) * sin(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) * sin(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*sin(position.z + 4*PI/3))*2/3;
gpongnot 3:737ac9c24ca5 96
gpongnot 3:737ac9c24ca5 97 return position;
gpongnot 3:737ac9c24ca5 98 }
gpongnot 3:737ac9c24ca5 99
gpongnot 3:737ac9c24ca5 100 Vect3 calcErreur(Vect3 consigne, Vect3 position){
gpongnot 3:737ac9c24ca5 101 Vect3 erreur;
gpongnot 3:737ac9c24ca5 102 erreur.x = position.x - consigne.x;
natienza 9:eb3f9744ae5d 103 erreur.y = position.y - consigne.y;
natienza 9:eb3f9744ae5d 104 erreur.z = position.z - consigne.z;
gpongnot 3:737ac9c24ca5 105 return erreur;
gpongnot 3:737ac9c24ca5 106 }
gpongnot 3:737ac9c24ca5 107
gpongnot 3:737ac9c24ca5 108 Vect3 calcCommandeXYZ(Vect3 erreur){
natienza 9:eb3f9744ae5d 109 /*
gpongnot 6:85ade96c99b1 110 static Vect3 commande;
natienza 8:91eb7435c3e0 111 static Vect3 oldErreur;
gpongnot 6:85ade96c99b1 112 if (erreur.x > ERREUR_POS){
gpongnot 6:85ade96c99b1 113 commande.x = GAIN_POS*erreur.x;
natienza 9:eb3f9744ae5d 114 oldErreur.x = 0;
gpongnot 6:85ade96c99b1 115 } else {
natienza 8:91eb7435c3e0 116 commande.x = commande.x + GAIN_POS*(Ti+Te)/Te*erreur.x - GAIN_POS*oldErreur.x;
natienza 8:91eb7435c3e0 117 //commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*commande.x);
gpongnot 6:85ade96c99b1 118 }
gpongnot 6:85ade96c99b1 119 if (erreur.y > ERREUR_POS){
gpongnot 6:85ade96c99b1 120 commande.y = GAIN_POS*erreur.y;
gpongnot 6:85ade96c99b1 121 } else {
natienza 8:91eb7435c3e0 122 commande.y = commande.y + GAIN_POS*(Ti+Te)/Te*erreur.y - GAIN_POS*oldErreur.y;
natienza 8:91eb7435c3e0 123 //commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*commande.y);
gpongnot 6:85ade96c99b1 124 }
gpongnot 6:85ade96c99b1 125 if (erreur.z > ERREUR_ANG){
gpongnot 6:85ade96c99b1 126 commande.z = GAIN_ANG*erreur.z;
gpongnot 6:85ade96c99b1 127 } else {
natienza 8:91eb7435c3e0 128 commande.z = commande.z + GAIN_ANG*(Ti+Te)/Te*erreur.z - GAIN_ANG*oldErreur.z;
natienza 8:91eb7435c3e0 129 //commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
gpongnot 6:85ade96c99b1 130 }
natienza 8:91eb7435c3e0 131 oldErreur = erreur;
gpongnot 3:737ac9c24ca5 132 return commande;
natienza 9:eb3f9744ae5d 133 */
natienza 9:eb3f9744ae5d 134 static Vect3 integrateur = initVect3();
natienza 9:eb3f9744ae5d 135 Vect3 commande;
natienza 9:eb3f9744ae5d 136 if (abs(erreur.x) > ERREUR_POS){
natienza 9:eb3f9744ae5d 137 integrateur.x = 0;
natienza 9:eb3f9744ae5d 138 } else {
natienza 9:eb3f9744ae5d 139 integrateur.x = integrateur.x + Te*erreur.x;
natienza 9:eb3f9744ae5d 140 }
natienza 9:eb3f9744ae5d 141 commande.x = GAIN_POS*(erreur.x + integrateur.x/Ti);
natienza 9:eb3f9744ae5d 142
natienza 9:eb3f9744ae5d 143 if (abs(erreur.y) > ERREUR_POS){
natienza 9:eb3f9744ae5d 144 integrateur.y = 0;
natienza 9:eb3f9744ae5d 145 } else {
natienza 9:eb3f9744ae5d 146 integrateur.y = integrateur.y + Te*erreur.y;
natienza 9:eb3f9744ae5d 147 }
natienza 9:eb3f9744ae5d 148 commande.y = GAIN_POS*(erreur.y + integrateur.y/Ti);
natienza 9:eb3f9744ae5d 149
natienza 9:eb3f9744ae5d 150 if (abs(erreur.z) > ERREUR_ANG){
natienza 9:eb3f9744ae5d 151 integrateur.z = 0;
natienza 9:eb3f9744ae5d 152 } else {
natienza 9:eb3f9744ae5d 153 integrateur.z = integrateur.z + Te*erreur.z;
natienza 9:eb3f9744ae5d 154 }
natienza 9:eb3f9744ae5d 155 commande.z = GAIN_ANG*(erreur.z + integrateur.z/Ti);
natienza 9:eb3f9744ae5d 156
natienza 9:eb3f9744ae5d 157 return commande;
natienza 9:eb3f9744ae5d 158
gpongnot 3:737ac9c24ca5 159 }
gpongnot 3:737ac9c24ca5 160
gpongnot 3:737ac9c24ca5 161 Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
gpongnot 3:737ac9c24ca5 162 Vect3 commande123;
gpongnot 3:737ac9c24ca5 163 commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 164 commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 165 commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 166 return commande123;
gpongnot 3:737ac9c24ca5 167 }
gpongnot 3:737ac9c24ca5 168
gpongnot 3:737ac9c24ca5 169 void moveBot(Vect3 commande123){
gpongnot 5:efd14a490f49 170 moteur1.write(commande123.y);
gpongnot 5:efd14a490f49 171 moteur2.write(commande123.x);
gpongnot 3:737ac9c24ca5 172 moteur3.write(commande123.z);
gpongnot 3:737ac9c24ca5 173 }
gpongnot 3:737ac9c24ca5 174
gpongnot 3:737ac9c24ca5 175 int main(){
gpongnot 3:737ac9c24ca5 176
natienza 2:486bb9b6bd78 177 //setup
gpongnot 0:509b29d50fcb 178 compt1.setup();
gpongnot 0:509b29d50fcb 179 compt2.setup();
gpongnot 0:509b29d50fcb 180 compt3.setup();
gpongnot 0:509b29d50fcb 181
gpongnot 0:509b29d50fcb 182 moteur1.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 183 moteur2.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 184 moteur3.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 185
natienza 2:486bb9b6bd78 186 timer.start();
gpongnot 5:efd14a490f49 187 pc.printf("SETUP effectue\n\r");
natienza 2:486bb9b6bd78 188
natienza 2:486bb9b6bd78 189 //variables
gpongnot 3:737ac9c24ca5 190 Vect3 position = initVect3();
gpongnot 3:737ac9c24ca5 191 Vect3 erreur = initVect3();
gpongnot 3:737ac9c24ca5 192 Vect3 commandeXYZ = initVect3();
gpongnot 3:737ac9c24ca5 193 Vect3 commande123 = initVect3();
gpongnot 3:737ac9c24ca5 194 Vect3 consigne = initVect3();
natienza 9:eb3f9744ae5d 195 consigne.x = 0.50;
natienza 9:eb3f9744ae5d 196 consigne.y = 0;
natienza 9:eb3f9744ae5d 197 consigne.z = 0;
gpongnot 7:09004b460bd1 198
natienza 2:486bb9b6bd78 199 uint32_t seuilAffichage = PERIODE_AFF;
natienza 2:486bb9b6bd78 200 uint32_t seuilAsserv = PERIODE_ASSERV;
natienza 2:486bb9b6bd78 201
gpongnot 0:509b29d50fcb 202 // Loop
gpongnot 0:509b29d50fcb 203 while(1) {
natienza 2:486bb9b6bd78 204
natienza 2:486bb9b6bd78 205 if (timer.read_ms() > seuilAffichage){
natienza 2:486bb9b6bd78 206 seuilAffichage += PERIODE_AFF;
natienza 9:eb3f9744ae5d 207 pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",360/(2*PI)*position.z, position.x, position.y);
gpongnot 5:efd14a490f49 208 pc.printf("erreur lacet : %f\n\rerreurX : %f\n\rereeurY: %f\n\n\r",erreur.z, erreur.x, erreur.y);
gpongnot 5:efd14a490f49 209 pc.printf("commande lacet : %f\n\rcommandeX : %f\n\rcommandeY: %f\n\n\r",commande123.z, commande123.x, commande123.y);
gpongnot 3:737ac9c24ca5 210 //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
gpongnot 3:737ac9c24ca5 211 //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
gpongnot 5:efd14a490f49 212
natienza 2:486bb9b6bd78 213 myled = !myled;
natienza 2:486bb9b6bd78 214 }
natienza 2:486bb9b6bd78 215 if (timer.read_ms() > seuilAsserv){
natienza 2:486bb9b6bd78 216 seuilAsserv += PERIODE_ASSERV;
gpongnot 3:737ac9c24ca5 217 position = calculatePosition();
gpongnot 3:737ac9c24ca5 218 erreur = calcErreur(consigne, position);
gpongnot 3:737ac9c24ca5 219 commandeXYZ = calcCommandeXYZ(erreur);
gpongnot 3:737ac9c24ca5 220 commande123 = calcCommande123(commandeXYZ, position);
gpongnot 3:737ac9c24ca5 221 moveBot(commande123);
gpongnot 7:09004b460bd1 222 }
gpongnot 0:509b29d50fcb 223 }
gpongnot 0:509b29d50fcb 224 }