TER Atienza Pongnot 2019 / Mbed 2 deprecated identification

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@0:e91488ba4701, 2019-03-25 (annotated)

Committer:
natienza
Date:
Mon Mar 25 12:41:24 2019 +0000
Revision:
0:e91488ba4701
A TESTER -- routine permettant une identification de la reponse indicielle selon tous les axes

Who changed what in which revision?

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