Emile Toupet
/
VISITY
IUT DE CACHAN, test robot
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?
User | Revision | Line number | New 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�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 | } |