Emile Toupet / Mbed 2 deprecated VISITY

IUT DE CACHAN, test robot

Dependencies:   mbed QEI

main.cpp@0:b07e06edae6f, 2011-03-23 (annotated)

Committer:
emiletoupet
Date:
Wed Mar 23 18:07:18 2011 +0000
Revision:
0:b07e06edae6f
V0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emiletoupet 0:b07e06edae6f 1 #include "mbed.h"
emiletoupet 0:b07e06edae6f 2 #include "QEI.h"
emiletoupet 0:b07e06edae6f 3
emiletoupet 0:b07e06edae6f 4
emiletoupet 0:b07e06edae6f 5 //***********************************************************//
emiletoupet 0:b07e06edae6f 6 // #define _GAUCHE_ Control le moteur Gauche
emiletoupet 0:b07e06edae6f 7 // #define _DROIT_ Control le moteur Gauche
emiletoupet 0:b07e06edae6f 8
emiletoupet 0:b07e06edae6f 9 #define _DROIT_
emiletoupet 0:b07e06edae6f 10
emiletoupet 0:b07e06edae6f 11 #ifdef _GAUCHE_
emiletoupet 0:b07e06edae6f 12 //*******************//
emiletoupet 0:b07e06edae6f 13 // MOTEUR GAUCHE //
emiletoupet 0:b07e06edae6f 14 //*******************//
emiletoupet 0:b07e06edae6f 15
emiletoupet 0:b07e06edae6f 16 #define CAN_ID_ECRITURE_ASSERVISSEMENT 0x050
emiletoupet 0:b07e06edae6f 17 #define CAN_ID_INFO_ASSERVISSEMENT 0x110
emiletoupet 0:b07e06edae6f 18 #define CAN_ID_INFO_HACHEUR 0x101
emiletoupet 0:b07e06edae6f 19
emiletoupet 0:b07e06edae6f 20 #define RECULE 0x01
emiletoupet 0:b07e06edae6f 21 #define AVANCE 0x02
emiletoupet 0:b07e06edae6f 22 #define PARAMETRE_MOTEUR 1
emiletoupet 0:b07e06edae6f 23
emiletoupet 0:b07e06edae6f 24 #endif
emiletoupet 0:b07e06edae6f 25
emiletoupet 0:b07e06edae6f 26 #ifdef _DROIT_
emiletoupet 0:b07e06edae6f 27 //******************//
emiletoupet 0:b07e06edae6f 28 // MOTEUR DROIT //
emiletoupet 0:b07e06edae6f 29 //******************//
emiletoupet 0:b07e06edae6f 30
emiletoupet 0:b07e06edae6f 31 #define CAN_ID_ECRITURE_ASSERVISSEMENT 0x060
emiletoupet 0:b07e06edae6f 32 #define CAN_ID_INFO_ASSERVISSEMENT 0x210
emiletoupet 0:b07e06edae6f 33 #define CAN_ID_INFO_HACHEUR 0x201
emiletoupet 0:b07e06edae6f 34
emiletoupet 0:b07e06edae6f 35 #define RECULE 0x01
emiletoupet 0:b07e06edae6f 36 #define AVANCE 0x02
emiletoupet 0:b07e06edae6f 37 #define PARAMETRE_MOTEUR -1
emiletoupet 0:b07e06edae6f 38
emiletoupet 0:b07e06edae6f 39 #endif
emiletoupet 0:b07e06edae6f 40 //***********************************************************//
emiletoupet 0:b07e06edae6f 41
emiletoupet 0:b07e06edae6f 42
emiletoupet 0:b07e06edae6f 43 #define T_Time 0.5
emiletoupet 0:b07e06edae6f 44 #define IMP_PAR_TOUR 36864 //impulsion par tour, codeur QEI
emiletoupet 0:b07e06edae6f 45 #define DIAMETRE_ROUE 15 //diametre des roues en mm
emiletoupet 0:b07e06edae6f 46 #define PI 3.14159
emiletoupet 0:b07e06edae6f 47
emiletoupet 0:b07e06edae6f 48 #define SEUIL_POSITION 0.3
emiletoupet 0:b07e06edae6f 49 #define Kp_position 1
emiletoupet 0:b07e06edae6f 50 #define Kd_position 0.5
emiletoupet 0:b07e06edae6f 51 #define Ki_position 1
emiletoupet 0:b07e06edae6f 52 #define MAX_INTEG_POSITION 4 //8
emiletoupet 0:b07e06edae6f 53
emiletoupet 0:b07e06edae6f 54 #define VITESSE_MAX_ASSERVISSEMENT 120
emiletoupet 0:b07e06edae6f 55 #define Kp_vitesse 2
emiletoupet 0:b07e06edae6f 56
emiletoupet 0:b07e06edae6f 57
emiletoupet 0:b07e06edae6f 58 DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
emiletoupet 0:b07e06edae6f 59
emiletoupet 0:b07e06edae6f 60 //defintion du port I2C
emiletoupet 0:b07e06edae6f 61 //I2C i2c_D(p28, p27);
emiletoupet 0:b07e06edae6f 62 //I2C i2c_G(p9, p10);
emiletoupet 0:b07e06edae6f 63
emiletoupet 0:b07e06edae6f 64 //QEI qei(pinA,pinB,index,puls/tour)
emiletoupet 0:b07e06edae6f 65 //QEI qei_D(p26,p25,p24,1);
emiletoupet 0:b07e06edae6f 66 //QEI qei_G(p23,p22,p21,1);
emiletoupet 0:b07e06edae6f 67
emiletoupet 0:b07e06edae6f 68 //liason serie avec le PC
emiletoupet 0:b07e06edae6f 69 Serial pc(USBTX, USBRX);
emiletoupet 0:b07e06edae6f 70
emiletoupet 0:b07e06edae6f 71
emiletoupet 0:b07e06edae6f 72 //definition de la class Moteur
emiletoupet 0:b07e06edae6f 73 class Moteur {
emiletoupet 0:b07e06edae6f 74 public :
emiletoupet 0:b07e06edae6f 75
emiletoupet 0:b07e06edae6f 76 Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td);
emiletoupet 0:b07e06edae6f 77
emiletoupet 0:b07e06edae6f 78
emiletoupet 0:b07e06edae6f 79 //*********************//
emiletoupet 0:b07e06edae6f 80 // CALCUL DISTANCE //
emiletoupet 0:b07e06edae6f 81 //*********************//
emiletoupet 0:b07e06edae6f 82 float Calcul_Distance(void) {
emiletoupet 0:b07e06edae6f 83 if (pulse != qei.getPulses())
emiletoupet 0:b07e06edae6f 84 {
emiletoupet 0:b07e06edae6f 85 pulse = PARAMETRE_MOTEUR * qei.getPulses();
emiletoupet 0:b07e06edae6f 86 distance = (pulse * PI * DIAMETRE_ROUE) / IMP_PAR_TOUR;
emiletoupet 0:b07e06edae6f 87 //pc.printf("\r\n Distance = %f.2 ", distance);
emiletoupet 0:b07e06edae6f 88 }
emiletoupet 0:b07e06edae6f 89 return distance;
emiletoupet 0:b07e06edae6f 90 }
emiletoupet 0:b07e06edae6f 91
emiletoupet 0:b07e06edae6f 92
emiletoupet 0:b07e06edae6f 93 //********************//
emiletoupet 0:b07e06edae6f 94 // CALCUL VITESSE //
emiletoupet 0:b07e06edae6f 95 //********************//
emiletoupet 0:b07e06edae6f 96 void Calcul_Vitesse(void){
emiletoupet 0:b07e06edae6f 97
emiletoupet 0:b07e06edae6f 98 dist_1 = Calcul_Distance();
emiletoupet 0:b07e06edae6f 99 wait(0.01);
emiletoupet 0:b07e06edae6f 100 dist_2 = Calcul_Distance();
emiletoupet 0:b07e06edae6f 101
emiletoupet 0:b07e06edae6f 102 Vitesse_moteur = (dist_2 - dist_1)*100;
emiletoupet 0:b07e06edae6f 103 //pc.printf("\r\n Vitesse = %f.1 ", Vitesse_mesure);
emiletoupet 0:b07e06edae6f 104 }
emiletoupet 0:b07e06edae6f 105
emiletoupet 0:b07e06edae6f 106
emiletoupet 0:b07e06edae6f 107 //***************************//
emiletoupet 0:b07e06edae6f 108 // CALCUL ERREUR VITESSE //
emiletoupet 0:b07e06edae6f 109 //***************************//
emiletoupet 0:b07e06edae6f 110
emiletoupet 0:b07e06edae6f 111 void Calcul_Erreur_Vitesse(int Vitesse_voulu)
emiletoupet 0:b07e06edae6f 112 {
emiletoupet 0:b07e06edae6f 113
emiletoupet 0:b07e06edae6f 114 Last_Erreur_vitesse = Delta_Erreur_vitesse ;
emiletoupet 0:b07e06edae6f 115 Calcul_Vitesse();
emiletoupet 0:b07e06edae6f 116
emiletoupet 0:b07e06edae6f 117 if (Vitesse_moteur < 0 )
emiletoupet 0:b07e06edae6f 118 Vitesse_moteur = Vitesse_moteur * (-1);
emiletoupet 0:b07e06edae6f 119
emiletoupet 0:b07e06edae6f 120 Erreur_vitesse = Vitesse_voulu - Vitesse_moteur;
emiletoupet 0:b07e06edae6f 121
emiletoupet 0:b07e06edae6f 122 }
emiletoupet 0:b07e06edae6f 123
emiletoupet 0:b07e06edae6f 124 //****************************//
emiletoupet 0:b07e06edae6f 125 // ASSERVISSEMENT VITESSE //
emiletoupet 0:b07e06edae6f 126 //****************************//
emiletoupet 0:b07e06edae6f 127 void Asservissement_vitesse ( char DIRECTION ,char VITESSE , char VITESSE_MAX ,char ACCELERATION)
emiletoupet 0:b07e06edae6f 128 {
emiletoupet 0:b07e06edae6f 129
emiletoupet 0:b07e06edae6f 130 char result;
emiletoupet 0:b07e06edae6f 131 Calcul_Vitesse();
emiletoupet 0:b07e06edae6f 132 Vitesse_Sortie = Kp_vitesse * VITESSE ;
emiletoupet 0:b07e06edae6f 133
emiletoupet 0:b07e06edae6f 134 //Bridage du moteur
emiletoupet 0:b07e06edae6f 135 if (Vitesse_Sortie > VITESSE_MAX_ASSERVISSEMENT)
emiletoupet 0:b07e06edae6f 136 Vitesse_Sortie = VITESSE_MAX_ASSERVISSEMENT;
emiletoupet 0:b07e06edae6f 137
emiletoupet 0:b07e06edae6f 138 if (Vitesse_Sortie < 0)
emiletoupet 0:b07e06edae6f 139 Vitesse_Sortie = 0;
emiletoupet 0:b07e06edae6f 140
emiletoupet 0:b07e06edae6f 141 data_speed[1] = Vitesse_Sortie;
emiletoupet 0:b07e06edae6f 142 data_comma[1] = DIRECTION;
emiletoupet 0:b07e06edae6f 143 data_accel[1] = ACCELERATION;
emiletoupet 0:b07e06edae6f 144
emiletoupet 0:b07e06edae6f 145 /*
emiletoupet 0:b07e06edae6f 146 if (DIRECTION==0x00)
emiletoupet 0:b07e06edae6f 147 {
emiletoupet 0:b07e06edae6f 148 data_speed[1] = 0x44;
emiletoupet 0:b07e06edae6f 149 data_comma[1] = 0x00;
emiletoupet 0:b07e06edae6f 150 data_accel[1] = 0x00;
emiletoupet 0:b07e06edae6f 151 }
emiletoupet 0:b07e06edae6f 152 else
emiletoupet 0:b07e06edae6f 153 {
emiletoupet 0:b07e06edae6f 154 //Copie des valeur dans les tableaux de registre
emiletoupet 0:b07e06edae6f 155 data_speed[1] = Vitesse_Sortie;
emiletoupet 0:b07e06edae6f 156 data_comma[1] = DIRECTION;
emiletoupet 0:b07e06edae6f 157 data_accel[1] = ACCELERATION;
emiletoupet 0:b07e06edae6f 158 }
emiletoupet 0:b07e06edae6f 159 */
emiletoupet 0:b07e06edae6f 160 /*
emiletoupet 0:b07e06edae6f 161 if (Last_data_comma != data_comma[1])
emiletoupet 0:b07e06edae6f 162 {
emiletoupet 0:b07e06edae6f 163 //pc.printf("\r\n ASSERVISSEMENT DIRECTION");
emiletoupet 0:b07e06edae6f 164 i2c.write(0xB0, data_comma,2,1);
emiletoupet 0:b07e06edae6f 165 Last_data_comma = data_comma[1];
emiletoupet 0:b07e06edae6f 166 }
emiletoupet 0:b07e06edae6f 167
emiletoupet 0:b07e06edae6f 168 if (Last_data_accel != data_accel[1])
emiletoupet 0:b07e06edae6f 169 {
emiletoupet 0:b07e06edae6f 170 //pc.printf("\r\n ASSERVISSEMENT ACCELERATION");
emiletoupet 0:b07e06edae6f 171 i2c.write(0xB0, data_accel,2,1);
emiletoupet 0:b07e06edae6f 172 Last_data_accel = data_accel[1];
emiletoupet 0:b07e06edae6f 173 }
emiletoupet 0:b07e06edae6f 174
emiletoupet 0:b07e06edae6f 175 if (Last_data_speed != data_speed[1])
emiletoupet 0:b07e06edae6f 176 {
emiletoupet 0:b07e06edae6f 177 //pc.printf("\r\n ASSERVISSEMENT VITESSE");
emiletoupet 0:b07e06edae6f 178 i2c.write(0xB0, data_speed,2,1);
emiletoupet 0:b07e06edae6f 179 Last_data_speed = data_speed[1];
emiletoupet 0:b07e06edae6f 180 }
emiletoupet 0:b07e06edae6f 181 */
emiletoupet 0:b07e06edae6f 182
emiletoupet 0:b07e06edae6f 183 pc.printf("\r\n ASSERVISSEMENT DIRECTION");
emiletoupet 0:b07e06edae6f 184 //while(i2c.write(0xB0, data_comma,2,1));
emiletoupet 0:b07e06edae6f 185 result=i2c.write(0xB0, data_comma,2,1);
emiletoupet 0:b07e06edae6f 186 //while(i2c.write(0xB0, data_accel,2,1));
emiletoupet 0:b07e06edae6f 187 pc.printf("\r\n ASSERVISSEMENT VITESSE %d",result);
emiletoupet 0:b07e06edae6f 188 result=i2c.write(0xB0, data_speed,2,1);
emiletoupet 0:b07e06edae6f 189 //while(i2c.write(0xB0, data_speed,2,1));
emiletoupet 0:b07e06edae6f 190 pc.printf("\r\n ASSERVISSEMENT VITESSE fin %d",result);
emiletoupet 0:b07e06edae6f 191
emiletoupet 0:b07e06edae6f 192 }
emiletoupet 0:b07e06edae6f 193
emiletoupet 0:b07e06edae6f 194 //****************************//
emiletoupet 0:b07e06edae6f 195 // CALCUL ERREUR POSITION //
emiletoupet 0:b07e06edae6f 196 //****************************//
emiletoupet 0:b07e06edae6f 197 void Calcul_Erreur_Position(int Position_voulu)
emiletoupet 0:b07e06edae6f 198 {
emiletoupet 0:b07e06edae6f 199 Last_Erreur_position = Erreur_position;
emiletoupet 0:b07e06edae6f 200 Position_moteur = Calcul_Distance();
emiletoupet 0:b07e06edae6f 201 Erreur_position = Position_voulu - Position_moteur;
emiletoupet 0:b07e06edae6f 202
emiletoupet 0:b07e06edae6f 203 Delta_Erreur_position += 5 * Erreur_position ;
emiletoupet 0:b07e06edae6f 204
emiletoupet 0:b07e06edae6f 205 if (Delta_Erreur_position > MAX_INTEG_POSITION)
emiletoupet 0:b07e06edae6f 206 Delta_Erreur_position = MAX_INTEG_POSITION;
emiletoupet 0:b07e06edae6f 207
emiletoupet 0:b07e06edae6f 208 if (Delta_Erreur_position < - MAX_INTEG_POSITION)
emiletoupet 0:b07e06edae6f 209 Delta_Erreur_position = - MAX_INTEG_POSITION;
emiletoupet 0:b07e06edae6f 210 }
emiletoupet 0:b07e06edae6f 211
emiletoupet 0:b07e06edae6f 212 //********************************//
emiletoupet 0:b07e06edae6f 213 // ASSERVISSEMENT POSITION BIS //
emiletoupet 0:b07e06edae6f 214 //********************************//
emiletoupet 0:b07e06edae6f 215 void Asservissement_position()
emiletoupet 0:b07e06edae6f 216 {
emiletoupet 0:b07e06edae6f 217 Calcul_Erreur_Position(POSITION);
emiletoupet 0:b07e06edae6f 218 Speed = Kp_position * Erreur_position - Kd_position * Last_Erreur_position + Ki_position * Delta_Erreur_position;
emiletoupet 0:b07e06edae6f 219
emiletoupet 0:b07e06edae6f 220 if (Speed < 0 )
emiletoupet 0:b07e06edae6f 221 Speed = Speed * (-1);
emiletoupet 0:b07e06edae6f 222
emiletoupet 0:b07e06edae6f 223 if (Erreur_position < - SEUIL_POSITION )
emiletoupet 0:b07e06edae6f 224 {
emiletoupet 0:b07e06edae6f 225
emiletoupet 0:b07e06edae6f 226 led1=0;
emiletoupet 0:b07e06edae6f 227 led2=1;
emiletoupet 0:b07e06edae6f 228 led3=0;
emiletoupet 0:b07e06edae6f 229 led4=0;
emiletoupet 0:b07e06edae6f 230
emiletoupet 0:b07e06edae6f 231 Sens = RECULE;
emiletoupet 0:b07e06edae6f 232 }
emiletoupet 0:b07e06edae6f 233 else if (Erreur_position > SEUIL_POSITION )
emiletoupet 0:b07e06edae6f 234 {
emiletoupet 0:b07e06edae6f 235
emiletoupet 0:b07e06edae6f 236 led1=0;
emiletoupet 0:b07e06edae6f 237 led2=0;
emiletoupet 0:b07e06edae6f 238 led3=1;
emiletoupet 0:b07e06edae6f 239 led4=0;
emiletoupet 0:b07e06edae6f 240
emiletoupet 0:b07e06edae6f 241 Sens = AVANCE;
emiletoupet 0:b07e06edae6f 242 }
emiletoupet 0:b07e06edae6f 243 else
emiletoupet 0:b07e06edae6f 244 {
emiletoupet 0:b07e06edae6f 245
emiletoupet 0:b07e06edae6f 246 led1=1;
emiletoupet 0:b07e06edae6f 247 led2=0;
emiletoupet 0:b07e06edae6f 248 led3=0;
emiletoupet 0:b07e06edae6f 249 led4=1;
emiletoupet 0:b07e06edae6f 250
emiletoupet 0:b07e06edae6f 251 Sens = 0x00;
emiletoupet 0:b07e06edae6f 252 }
emiletoupet 0:b07e06edae6f 253
emiletoupet 0:b07e06edae6f 254 if (Speed > VITESSE*2 )
emiletoupet 0:b07e06edae6f 255 Speed = VITESSE*2;
emiletoupet 0:b07e06edae6f 256
emiletoupet 0:b07e06edae6f 257 if (Speed > 254 )
emiletoupet 0:b07e06edae6f 258 Speed = 254;
emiletoupet 0:b07e06edae6f 259
emiletoupet 0:b07e06edae6f 260 Asservissement_vitesse(Sens,Speed,VITESSE,ACCELERATION);
emiletoupet 0:b07e06edae6f 261 }
emiletoupet 0:b07e06edae6f 262
emiletoupet 0:b07e06edae6f 263 //********************//
emiletoupet 0:b07e06edae6f 264 // INITIALISATION //
emiletoupet 0:b07e06edae6f 265 //********************//
emiletoupet 0:b07e06edae6f 266 void Initialisation (void)
emiletoupet 0:b07e06edae6f 267 {
emiletoupet 0:b07e06edae6f 268 //initialisation du CAN
emiletoupet 0:b07e06edae6f 269 can.frequency(1000000);
emiletoupet 0:b07e06edae6f 270
emiletoupet 0:b07e06edae6f 271 i2c.frequency(100000);
emiletoupet 0:b07e06edae6f 272
emiletoupet 0:b07e06edae6f 273 //Initialisation des param&#65533;tre d'asservissement.
emiletoupet 0:b07e06edae6f 274 ACCELERATION = 0x00;
emiletoupet 0:b07e06edae6f 275 VITESSE = 0x0;
emiletoupet 0:b07e06edae6f 276 POSITION = 0x0000;
emiletoupet 0:b07e06edae6f 277
emiletoupet 0:b07e06edae6f 278 //initialisation des registres d'adresse
emiletoupet 0:b07e06edae6f 279 data_comma[0] = 0x00;
emiletoupet 0:b07e06edae6f 280 data_speed[0] = 0x02;
emiletoupet 0:b07e06edae6f 281 data_accel[0] = 0x03;
emiletoupet 0:b07e06edae6f 282 }
emiletoupet 0:b07e06edae6f 283
emiletoupet 0:b07e06edae6f 284
emiletoupet 0:b07e06edae6f 285 //******************************//
emiletoupet 0:b07e06edae6f 286 // DEBUG SERIAL I2C HACHEUR //
emiletoupet 0:b07e06edae6f 287 //******************************//
emiletoupet 0:b07e06edae6f 288 void debug_serial_i2c_hacheur (void)
emiletoupet 0:b07e06edae6f 289 {
emiletoupet 0:b07e06edae6f 290 char data_recu[8];
emiletoupet 0:b07e06edae6f 291 unsigned char i;
emiletoupet 0:b07e06edae6f 292
emiletoupet 0:b07e06edae6f 293 i2c.read(0xB0, data_recu ,8);
emiletoupet 0:b07e06edae6f 294
emiletoupet 0:b07e06edae6f 295 pc.printf("\r\n \r\n");
emiletoupet 0:b07e06edae6f 296 pc.printf("temp moto unus revi | comm stat spee acce\r\n");
emiletoupet 0:b07e06edae6f 297
emiletoupet 0:b07e06edae6f 298 for(i=0;i<8;i++)
emiletoupet 0:b07e06edae6f 299 {
emiletoupet 0:b07e06edae6f 300 if (i==4) pc.printf("| ");
emiletoupet 0:b07e06edae6f 301 pc.printf(" %02x ", data_recu[i] );
emiletoupet 0:b07e06edae6f 302 }
emiletoupet 0:b07e06edae6f 303 }
emiletoupet 0:b07e06edae6f 304
emiletoupet 0:b07e06edae6f 305
emiletoupet 0:b07e06edae6f 306 //***********************//
emiletoupet 0:b07e06edae6f 307 // DEBUG SERIAL mBed //
emiletoupet 0:b07e06edae6f 308 //***********************//
emiletoupet 0:b07e06edae6f 309 void debug_serial_mbed (void)
emiletoupet 0:b07e06edae6f 310 {
emiletoupet 0:b07e06edae6f 311 pc.printf(" %d | %.1f | %02x | %.lf | %d | %.1f \r\n ",
emiletoupet 0:b07e06edae6f 312 data_speed[1], Vitesse_moteur, Sens,Position_moteur,Speed,Erreur_position);
emiletoupet 0:b07e06edae6f 313 }
emiletoupet 0:b07e06edae6f 314
emiletoupet 0:b07e06edae6f 315
emiletoupet 0:b07e06edae6f 316 //*****************************************//
emiletoupet 0:b07e06edae6f 317 // DEBUG SERIAL I2C mBed ASSERVISSEMENT //
emiletoupet 0:b07e06edae6f 318 //*****************************************//
emiletoupet 0:b07e06edae6f 319 void debug_serial_i2c_mbed_asservissement(void)
emiletoupet 0:b07e06edae6f 320 {
emiletoupet 0:b07e06edae6f 321 pc.printf(" %d = %.lf - %.1f + %.lf \r\n ",
emiletoupet 0:b07e06edae6f 322 Speed, Kp_position * Erreur_position , Kd_position * Last_Erreur_position , Ki_position * Delta_Erreur_position);
emiletoupet 0:b07e06edae6f 323 }
emiletoupet 0:b07e06edae6f 324
emiletoupet 0:b07e06edae6f 325 //**********************//
emiletoupet 0:b07e06edae6f 326 // CAN MESSAGE SEND //
emiletoupet 0:b07e06edae6f 327 //**********************//
emiletoupet 0:b07e06edae6f 328 void CANMSG_Send(int CANID,char CAN_data[])
emiletoupet 0:b07e06edae6f 329 {
emiletoupet 0:b07e06edae6f 330 char i;
emiletoupet 0:b07e06edae6f 331 CANMessage canmsg = CANMessage();
emiletoupet 0:b07e06edae6f 332
emiletoupet 0:b07e06edae6f 333 canmsg.id = CANID; /*L'identification du message*/
emiletoupet 0:b07e06edae6f 334 canmsg.len = 8; /*Longeur de la trame en octet*/
emiletoupet 0:b07e06edae6f 335 canmsg.format = CANStandard; /*Format de la trame CANStandard 29 bits*/
emiletoupet 0:b07e06edae6f 336 canmsg.type = CANData; /*Type de la trame CANData*/
emiletoupet 0:b07e06edae6f 337
emiletoupet 0:b07e06edae6f 338 for(i=0;i<8;i++)
emiletoupet 0:b07e06edae6f 339 canmsg.data[i] = CAN_data[i];
emiletoupet 0:b07e06edae6f 340
emiletoupet 0:b07e06edae6f 341 can.write(canmsg); /*Envoi du message*/
emiletoupet 0:b07e06edae6f 342 }
emiletoupet 0:b07e06edae6f 343
emiletoupet 0:b07e06edae6f 344 //*****************************//
emiletoupet 0:b07e06edae6f 345 // CAN INFO ASSERVISSEMENT //
emiletoupet 0:b07e06edae6f 346 //*****************************//
emiletoupet 0:b07e06edae6f 347 void CAN_INFO_Asservissement (void)
emiletoupet 0:b07e06edae6f 348 {
emiletoupet 0:b07e06edae6f 349 char data_asservicement[8];
emiletoupet 0:b07e06edae6f 350 short Position_moteur_Can, Vitesse_moteur_Can;
emiletoupet 0:b07e06edae6f 351
emiletoupet 0:b07e06edae6f 352 Position_moteur_Can = Position_moteur;
emiletoupet 0:b07e06edae6f 353 Vitesse_moteur_Can = Vitesse_moteur;
emiletoupet 0:b07e06edae6f 354
emiletoupet 0:b07e06edae6f 355 if (Vitesse_moteur_Can <0)
emiletoupet 0:b07e06edae6f 356 Vitesse_moteur_Can = Vitesse_moteur_Can *(-1);
emiletoupet 0:b07e06edae6f 357
emiletoupet 0:b07e06edae6f 358 data_asservicement[0] = Sens;
emiletoupet 0:b07e06edae6f 359 data_asservicement[1] = Vitesse_moteur_Can & 0x00FF;
emiletoupet 0:b07e06edae6f 360 data_asservicement[2] = 0;
emiletoupet 0:b07e06edae6f 361 data_asservicement[3] = 0;
emiletoupet 0:b07e06edae6f 362
emiletoupet 0:b07e06edae6f 363 data_asservicement[4] = POSITION >>8;
emiletoupet 0:b07e06edae6f 364 data_asservicement[5] = POSITION & 0x00FF;
emiletoupet 0:b07e06edae6f 365 data_asservicement[6] = Position_moteur_Can >>8;
emiletoupet 0:b07e06edae6f 366 data_asservicement[7] = Position_moteur_Can & 0x00FF;
emiletoupet 0:b07e06edae6f 367
emiletoupet 0:b07e06edae6f 368 CANMSG_Send(CAN_ID_INFO_ASSERVISSEMENT,data_asservicement);
emiletoupet 0:b07e06edae6f 369 }
emiletoupet 0:b07e06edae6f 370
emiletoupet 0:b07e06edae6f 371 //**********************//
emiletoupet 0:b07e06edae6f 372 // CAN INFO Hacheur //
emiletoupet 0:b07e06edae6f 373 //**********************//
emiletoupet 0:b07e06edae6f 374 void CAN_INFO_Hacheur (void)
emiletoupet 0:b07e06edae6f 375 {
emiletoupet 0:b07e06edae6f 376 char data_Hacheur[8];
emiletoupet 0:b07e06edae6f 377 pc.printf("\r\n ENTRER INFO ");
emiletoupet 0:b07e06edae6f 378 i2c.read(0xB0, data_Hacheur ,8);
emiletoupet 0:b07e06edae6f 379 pc.printf("\r\n SORTIE INFO ");
emiletoupet 0:b07e06edae6f 380 //temp moto unus revi | comm stat spee acce
emiletoupet 0:b07e06edae6f 381 CANMSG_Send(CAN_ID_INFO_HACHEUR,data_Hacheur);
emiletoupet 0:b07e06edae6f 382 }
emiletoupet 0:b07e06edae6f 383
emiletoupet 0:b07e06edae6f 384 //*************************//
emiletoupet 0:b07e06edae6f 385 // CAN MESSAGE RECIEVE //
emiletoupet 0:b07e06edae6f 386 //*************************//
emiletoupet 0:b07e06edae6f 387 void CAN_RecieveInterrupt_Routine(void)
emiletoupet 0:b07e06edae6f 388 {
emiletoupet 0:b07e06edae6f 389
emiletoupet 0:b07e06edae6f 390 static CANMessage can_recieve_msg;
emiletoupet 0:b07e06edae6f 391
emiletoupet 0:b07e06edae6f 392 if(can.read(can_recieve_msg))
emiletoupet 0:b07e06edae6f 393 {
emiletoupet 0:b07e06edae6f 394 if(can_recieve_msg.id==CAN_ID_ECRITURE_ASSERVISSEMENT)
emiletoupet 0:b07e06edae6f 395 {
emiletoupet 0:b07e06edae6f 396
emiletoupet 0:b07e06edae6f 397 Sens = can_recieve_msg.data[0] ;
emiletoupet 0:b07e06edae6f 398 VITESSE = can_recieve_msg.data[1] ;
emiletoupet 0:b07e06edae6f 399 ACCELERATION = can_recieve_msg.data[2] ;
emiletoupet 0:b07e06edae6f 400 POSITION = (short) can_recieve_msg.data[6]<<8 | can_recieve_msg.data[7];
emiletoupet 0:b07e06edae6f 401
emiletoupet 0:b07e06edae6f 402 }
emiletoupet 0:b07e06edae6f 403
emiletoupet 0:b07e06edae6f 404 }
emiletoupet 0:b07e06edae6f 405 }
emiletoupet 0:b07e06edae6f 406
emiletoupet 0:b07e06edae6f 407 // protected bite:
emiletoupet 0:b07e06edae6f 408 QEI qei;
emiletoupet 0:b07e06edae6f 409 I2C i2c;
emiletoupet 0:b07e06edae6f 410 CAN can;
emiletoupet 0:b07e06edae6f 411
emiletoupet 0:b07e06edae6f 412 short POSITION;
emiletoupet 0:b07e06edae6f 413 char VITESSE,ACCELERATION, Sens;
emiletoupet 0:b07e06edae6f 414
emiletoupet 0:b07e06edae6f 415 float Position_moteur, Erreur_position, Delta_Erreur_position, Last_Erreur_position ;
emiletoupet 0:b07e06edae6f 416 float distance , dist_1, dist_2;
emiletoupet 0:b07e06edae6f 417 float Vitesse_Sortie, Vitesse_moteur, Erreur_vitesse,Last_Erreur_vitesse,Delta_Erreur_vitesse;
emiletoupet 0:b07e06edae6f 418
emiletoupet 0:b07e06edae6f 419 int pulse,i;
emiletoupet 0:b07e06edae6f 420 int Speed;
emiletoupet 0:b07e06edae6f 421
emiletoupet 0:b07e06edae6f 422 char Last_data_speed, Last_data_comma, Last_data_accel;
emiletoupet 0:b07e06edae6f 423
emiletoupet 0:b07e06edae6f 424 // data_hacheur[2]={ registre, valeur}
emiletoupet 0:b07e06edae6f 425 char data_comma[2];
emiletoupet 0:b07e06edae6f 426 char data_speed[2];
emiletoupet 0:b07e06edae6f 427 char data_accel[2];
emiletoupet 0:b07e06edae6f 428
emiletoupet 0:b07e06edae6f 429 };
emiletoupet 0:b07e06edae6f 430
emiletoupet 0:b07e06edae6f 431 //constructeur de la class "Moteur"
emiletoupet 0:b07e06edae6f 432 Moteur::Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td) :
emiletoupet 0:b07e06edae6f 433 qei(qeiA, qeiB, qei_index, pulsesPerRev),
emiletoupet 0:b07e06edae6f 434 i2c(sda,scl),
emiletoupet 0:b07e06edae6f 435 can(rd,td){
emiletoupet 0:b07e06edae6f 436 distance = 0.0;
emiletoupet 0:b07e06edae6f 437 pulse = 0;
emiletoupet 0:b07e06edae6f 438 }
emiletoupet 0:b07e06edae6f 439
emiletoupet 0:b07e06edae6f 440 //definiton des Classes
emiletoupet 0:b07e06edae6f 441 Moteur moteur(p21,p22,p23,1,p9,p10,p30,p29);//, moteur_G(p23,p22,p21,1,p9,p10);
emiletoupet 0:b07e06edae6f 442
emiletoupet 0:b07e06edae6f 443
emiletoupet 0:b07e06edae6f 444 //fonction principal
emiletoupet 0:b07e06edae6f 445 int main()
emiletoupet 0:b07e06edae6f 446 {
emiletoupet 0:b07e06edae6f 447 moteur.Initialisation();
emiletoupet 0:b07e06edae6f 448 pc.printf("\r\n DEBUT");
emiletoupet 0:b07e06edae6f 449
emiletoupet 0:b07e06edae6f 450 while (1)
emiletoupet 0:b07e06edae6f 451 {
emiletoupet 0:b07e06edae6f 452
emiletoupet 0:b07e06edae6f 453
emiletoupet 0:b07e06edae6f 454 pc.printf("\r\n ASSERVISSEMENT");
emiletoupet 0:b07e06edae6f 455 moteur.CAN_INFO_Asservissement();
emiletoupet 0:b07e06edae6f 456 //moteur.CAN_INFO_Hacheur();
emiletoupet 0:b07e06edae6f 457 pc.printf("\r\n RECEIVE");
emiletoupet 0:b07e06edae6f 458 moteur.CAN_RecieveInterrupt_Routine();
emiletoupet 0:b07e06edae6f 459 pc.printf("\r\n POSITION");
emiletoupet 0:b07e06edae6f 460 moteur.Asservissement_position();
emiletoupet 0:b07e06edae6f 461 //moteur.debug_serial_mbed();
emiletoupet 0:b07e06edae6f 462 //moteur.debug_serial_i2c_mbed_asservissement();
emiletoupet 0:b07e06edae6f 463
emiletoupet 0:b07e06edae6f 464
emiletoupet 0:b07e06edae6f 465 pc.printf("\r\n TEMPO");
emiletoupet 0:b07e06edae6f 466 wait(0.01);
emiletoupet 0:b07e06edae6f 467 }
emiletoupet 0:b07e06edae6f 468 }