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@3:737ac9c24ca5, 2019-02-13 (annotated)

Committer:
gpongnot
Date:
Wed Feb 13 18:12:28 2019 +0000
Revision:
3:737ac9c24ca5
Parent:
2:486bb9b6bd78
Child:
4:80f612396136
Child:
6:85ade96c99b1
test endometrie ok etalonnage a faire en situ relle et tester les fonctions d assersv

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 3:737ac9c24ca5 7 #define RAYON_ROUE 0.005 // [m]
gpongnot 3:737ac9c24ca5 8 #define DIST1 0.015 // [m]
gpongnot 3:737ac9c24ca5 9 #define DIST2 0.015 // [m]
gpongnot 3:737ac9c24ca5 10 #define DIST3 0.015 // [m]
gpongnot 3:737ac9c24ca5 11 #define PI 3.14159265359
gpongnot 3:737ac9c24ca5 12 #define RESOLUTION_ENCO 14336*4 // = 14*1024*4 = rapport_reduction * nbr de top par tour
gpongnot 3:737ac9c24ca5 13
gpongnot 0:509b29d50fcb 14 // Liaison SPI avec les compteurs
gpongnot 0:509b29d50fcb 15 #define SPI_SCLK PA_5 //A4
gpongnot 0:509b29d50fcb 16 #define SPI_MISO PA_6 //A5
gpongnot 0:509b29d50fcb 17 #define SPI_MOSI PA_7 //A6
gpongnot 0:509b29d50fcb 18 #define SPI_CS3 PA_0//A0
gpongnot 0:509b29d50fcb 19 #define SPI_CS2 PA_1
gpongnot 0:509b29d50fcb 20 #define SPI_CS1 PA_3
gpongnot 0:509b29d50fcb 21
gpongnot 0:509b29d50fcb 22 // Vers le pont en H
gpongnot 0:509b29d50fcb 23 #define H1_IN1 PA_9 //D1 - PWM1/2
gpongnot 0:509b29d50fcb 24 #define H1_IN2 PA_10 //D0 - PWM1/3
gpongnot 0:509b29d50fcb 25 #define H2_IN1 PA_8 //D9 - PWM1/1
gpongnot 0:509b29d50fcb 26 #define H2_IN2 PB_7 //D4 - PWM17/1
gpongnot 0:509b29d50fcb 27 #define H3_IN1 PB_6 //D5 - PWM16/1
gpongnot 0:509b29d50fcb 28 #define H3_IN2 PA_4 //A3 - PWM3/2
gpongnot 0:509b29d50fcb 29
gpongnot 0:509b29d50fcb 30 #define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
natienza 2:486bb9b6bd78 31 #define PERIODE_AFF 500 //ms
natienza 2:486bb9b6bd78 32 #define PERIODE_ASSERV 50 //ms
gpongnot 0:509b29d50fcb 33
gpongnot 3:737ac9c24ca5 34 // Constantes Asservissement
gpongnot 3:737ac9c24ca5 35 #define GAIN_POS 0.0001
gpongnot 3:737ac9c24ca5 36 #define GAIN_ANG 0.0001
gpongnot 3:737ac9c24ca5 37
natienza 2:486bb9b6bd78 38 Serial pc(USBTX,USBRX);
natienza 2:486bb9b6bd78 39 Timer timer;
gpongnot 0:509b29d50fcb 40 DigitalOut myled(LED3);
gpongnot 0:509b29d50fcb 41
gpongnot 0:509b29d50fcb 42 SPI_7366 compt1(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS1);
gpongnot 0:509b29d50fcb 43 SPI_7366 compt2(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS2);
gpongnot 0:509b29d50fcb 44 SPI_7366 compt3(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS3);
gpongnot 0:509b29d50fcb 45
gpongnot 0:509b29d50fcb 46 TLE5206 moteur1(H1_IN1,H1_IN2);
gpongnot 0:509b29d50fcb 47 TLE5206 moteur2(H2_IN1,H2_IN2);
gpongnot 0:509b29d50fcb 48 TLE5206 moteur3(H3_IN1,H3_IN2);
gpongnot 0:509b29d50fcb 49
gpongnot 3:737ac9c24ca5 50 struct Vect3{
gpongnot 3:737ac9c24ca5 51 double x;
gpongnot 3:737ac9c24ca5 52 double y;
gpongnot 3:737ac9c24ca5 53 double z;
gpongnot 3:737ac9c24ca5 54 };
gpongnot 3:737ac9c24ca5 55
gpongnot 3:737ac9c24ca5 56
gpongnot 3:737ac9c24ca5 57 struct Vect2{
gpongnot 3:737ac9c24ca5 58 float x;
gpongnot 3:737ac9c24ca5 59 float y;
gpongnot 3:737ac9c24ca5 60 };
gpongnot 3:737ac9c24ca5 61
gpongnot 3:737ac9c24ca5 62 Vect3 initVect3(){
gpongnot 3:737ac9c24ca5 63 Vect3 result;
gpongnot 3:737ac9c24ca5 64 result.x = 0;
gpongnot 3:737ac9c24ca5 65 result.y = 0;
gpongnot 3:737ac9c24ca5 66 result.z = 0;
gpongnot 3:737ac9c24ca5 67 return result;
gpongnot 3:737ac9c24ca5 68 }
gpongnot 3:737ac9c24ca5 69
gpongnot 3:737ac9c24ca5 70 Vect3 calculatePosition(){
gpongnot 3:737ac9c24ca5 71
gpongnot 3:737ac9c24ca5 72 static Vect3 theta = initVect3();
gpongnot 3:737ac9c24ca5 73 static Vect3 position = initVect3();
gpongnot 3:737ac9c24ca5 74 Vect3 dTheta;
gpongnot 3:737ac9c24ca5 75 double dPsi = 0;
gpongnot 3:737ac9c24ca5 76
gpongnot 3:737ac9c24ca5 77 dTheta.x = 2*PI/RESOLUTION_ENCO*compt2.read_value() - theta.x;
gpongnot 3:737ac9c24ca5 78 dTheta.y = 2*PI/RESOLUTION_ENCO*compt1.read_value() - theta.y;
gpongnot 3:737ac9c24ca5 79 dTheta.z = 2*PI/RESOLUTION_ENCO*compt3.read_value() - theta.z;
gpongnot 3:737ac9c24ca5 80 theta.x += dTheta.x;
gpongnot 3:737ac9c24ca5 81 theta.y += dTheta.y;
gpongnot 3:737ac9c24ca5 82 theta.z += dTheta.z;
gpongnot 3:737ac9c24ca5 83
gpongnot 3:737ac9c24ca5 84 dPsi = RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
natienza 2:486bb9b6bd78 85
gpongnot 3:737ac9c24ca5 86 position.z += dPsi;// Psi actuel
gpongnot 3:737ac9c24ca5 87
gpongnot 3:737ac9c24ca5 88 position.x += ((-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 3:737ac9c24ca5 89 position.y +=-((-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 90
gpongnot 3:737ac9c24ca5 91 return position;
gpongnot 3:737ac9c24ca5 92 }
gpongnot 3:737ac9c24ca5 93
gpongnot 3:737ac9c24ca5 94 Vect3 calcErreur(Vect3 consigne, Vect3 position){
gpongnot 3:737ac9c24ca5 95 Vect3 erreur;
gpongnot 3:737ac9c24ca5 96 erreur.x = position.x - consigne.x;
gpongnot 3:737ac9c24ca5 97 erreur.x = position.y - consigne.y;
gpongnot 3:737ac9c24ca5 98 erreur.x = position.z - consigne.z;
gpongnot 3:737ac9c24ca5 99 return erreur;
gpongnot 3:737ac9c24ca5 100 }
gpongnot 3:737ac9c24ca5 101
gpongnot 3:737ac9c24ca5 102 Vect3 calcCommandeXYZ(Vect3 erreur){
gpongnot 3:737ac9c24ca5 103 Vect3 commande;
gpongnot 3:737ac9c24ca5 104 commande.x = GAIN_POS*erreur.x;
gpongnot 3:737ac9c24ca5 105 commande.y = GAIN_POS*erreur.y;
gpongnot 3:737ac9c24ca5 106 commande.z = GAIN_ANG*erreur.z;
gpongnot 3:737ac9c24ca5 107 return commande;
gpongnot 3:737ac9c24ca5 108 }
gpongnot 3:737ac9c24ca5 109
gpongnot 3:737ac9c24ca5 110 Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
gpongnot 3:737ac9c24ca5 111 Vect3 commande123;
gpongnot 3:737ac9c24ca5 112 commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 113 commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 114 commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
gpongnot 3:737ac9c24ca5 115 return commande123;
gpongnot 3:737ac9c24ca5 116 }
gpongnot 3:737ac9c24ca5 117
gpongnot 3:737ac9c24ca5 118 void moveBot(Vect3 commande123){
gpongnot 3:737ac9c24ca5 119 moteur1.write(commande123.x);
gpongnot 3:737ac9c24ca5 120 moteur2.write(commande123.y);
gpongnot 3:737ac9c24ca5 121 moteur3.write(commande123.z);
gpongnot 3:737ac9c24ca5 122 }
gpongnot 3:737ac9c24ca5 123
gpongnot 3:737ac9c24ca5 124 int main(){
gpongnot 3:737ac9c24ca5 125
natienza 2:486bb9b6bd78 126 //setup
gpongnot 0:509b29d50fcb 127 compt1.setup();
gpongnot 0:509b29d50fcb 128 compt2.setup();
gpongnot 0:509b29d50fcb 129 compt3.setup();
gpongnot 0:509b29d50fcb 130
gpongnot 0:509b29d50fcb 131 moteur1.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 132 moteur2.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 133 moteur3.setup(DECOUP_HACH);
gpongnot 0:509b29d50fcb 134
gpongnot 0:509b29d50fcb 135 moteur1.write(-0.4);
gpongnot 0:509b29d50fcb 136 moteur2.write(-0.7);
gpongnot 0:509b29d50fcb 137 moteur3.write(0.7);
gpongnot 0:509b29d50fcb 138
natienza 2:486bb9b6bd78 139 timer.start();
gpongnot 3:737ac9c24ca5 140 pc.printf("SETUP effectue");
natienza 2:486bb9b6bd78 141
natienza 2:486bb9b6bd78 142 //variables
gpongnot 3:737ac9c24ca5 143 Vect3 position = initVect3();
gpongnot 3:737ac9c24ca5 144 Vect3 erreur = initVect3();
gpongnot 3:737ac9c24ca5 145 Vect3 commandeXYZ = initVect3();
gpongnot 3:737ac9c24ca5 146 Vect3 commande123 = initVect3();
gpongnot 3:737ac9c24ca5 147 Vect3 consigne = initVect3();
gpongnot 3:737ac9c24ca5 148 consigne.x = 0.10;
gpongnot 3:737ac9c24ca5 149 consigne.y = 0.05;
gpongnot 3:737ac9c24ca5 150 consigne.z = PI/2;
natienza 2:486bb9b6bd78 151 uint32_t seuilAffichage = PERIODE_AFF;
natienza 2:486bb9b6bd78 152 uint32_t seuilAsserv = PERIODE_ASSERV;
natienza 2:486bb9b6bd78 153
gpongnot 0:509b29d50fcb 154 // Loop
gpongnot 0:509b29d50fcb 155 while(1) {
natienza 2:486bb9b6bd78 156
natienza 2:486bb9b6bd78 157 if (timer.read_ms() > seuilAffichage){
natienza 2:486bb9b6bd78 158 seuilAffichage += PERIODE_AFF;
gpongnot 3:737ac9c24ca5 159 pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",position.z, position.x, position.y);
gpongnot 3:737ac9c24ca5 160 //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 161 //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());
natienza 2:486bb9b6bd78 162 myled = !myled;
natienza 2:486bb9b6bd78 163 }
natienza 2:486bb9b6bd78 164 if (timer.read_ms() > seuilAsserv){
natienza 2:486bb9b6bd78 165 seuilAsserv += PERIODE_ASSERV;
gpongnot 3:737ac9c24ca5 166 position = calculatePosition();
gpongnot 3:737ac9c24ca5 167 erreur = calcErreur(consigne, position);
gpongnot 3:737ac9c24ca5 168 commandeXYZ = calcCommandeXYZ(erreur);
gpongnot 3:737ac9c24ca5 169 commande123 = calcCommande123(commandeXYZ, position);
gpongnot 3:737ac9c24ca5 170 moveBot(commande123);
gpongnot 3:737ac9c24ca5 171 }
gpongnot 3:737ac9c24ca5 172
gpongnot 0:509b29d50fcb 173 }
gpongnot 0:509b29d50fcb 174 }