hardware.cpp

00001 #define _POSIX_C_SOURCE 199309L
00002 #include "mbed.h"
00003 #include "reglages.h"
00004 #include "hardware.h"
00005 #include "DevSPI.h"
00006 #include "XNucleoIHM02A1.h"
00007 #include "commande_moteurs.h"
00008 
00009 // PWM_MAX est définit dans réglage;
00010 bool moteurs_arret = false;
00011 
00012 
00013 XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
00014 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00015     /* First Motor.G */
00016     {
00017         12,                           /* Motor supply voltage in V. */
00018         200,                           /* Min number of steps per revolution for the motor. */
00019         4,                           /* Max motor phase voltage in A. */
00020         7,                          /* Max motor phase voltage in V. */
00021         300,                         /* Motor initial speed [step/s]. */
00022         246.5,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00023         1500.0,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00024         992.0,                         /* Motor maximum speed [step/s]. */
00025         0.0,                           /* Motor minimum speed [step/s]. */
00026         602.7,                         /* Motor full-step speed threshold [step/s]. */
00027         5,                          /* Holding kval [V]. */
00028         5,                          /* Constant speed kval [V]. */
00029         5,                          /* Acceleration starting kval [V]. */
00030         5,                          /* Deceleration starting kval [V]. */
00031         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00032         392.1569e-6,                   /* Start slope [s/step]. */
00033         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00034         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00035         0,                             /* Thermal compensation factor (range [0, 15]). */
00036         5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00037         5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00038         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00039         0xFF,                          /* Alarm conditions enable. */
00040         0x2E88                         /* Ic configuration. */
00041     },
00042 
00043     /* Second Motor. */
00044     {
00045         12,                           /* Motor supply voltage in V. */
00046         200,                           /* Min number of steps per revolution for the motor. */
00047         4,                           /* Max motor phase voltage in A. */
00048         7,                          /* Max motor phase voltage in V. */
00049         300,                         /* Motor initial speed [step/s]. */
00050         251.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00051         1500.0,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00052         992.0,                         /* Motor maximum speed [step/s]. */
00053         0.0,                           /* Motor minimum speed [step/s]. */
00054         602.7,                         /* Motor full-step speed threshold [step/s]. */
00055         5,                          /* Holding kval [V]. */
00056         5,                          /* Constant speed kval [V]. */
00057         5,                          /* Acceleration starting kval [V]. */
00058         5,                          /* Deceleration starting kval [V]. */
00059         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00060         392.1569e-6,                   /* Start slope [s/step]. */
00061         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00062         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00063         0,                             /* Thermal compensation factor (range [0, 15]). */
00064         5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00065         5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00066         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00067         0xFF,                          /* Alarm conditions enable. */
00068         0x2E88                         /* Ic configuration. */
00069     }
00070 };
00071 
00072 L6470 **motors; //Instance des moteurs
00073 
00074 DigitalOut led(LED2);
00075 Serial pc(USBTX, USBRX); // tx, rx
00076 DevSPI dev_spi(D11, D12, D3);
00077 
00078 
00079 //Connections codeuses
00080 //Nucleo 401re
00081 InterruptIn ENCAL(D9);
00082 InterruptIn ENCAJ(D8);
00083 InterruptIn ENCBL(D6);
00084 InterruptIn ENCBJ(D5);
00085 /*//Nucelo 746zg
00086 InterruptIn ENCAL(D8);
00087 InterruptIn ENCAJ(D7);
00088 InterruptIn ENCBL(D4);
00089 InterruptIn ENCBJ(D0);*/
00090 
00091 volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
00092 volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
00093 
00094 void init_hardware()
00095 {
00096     pc.baud(2000000); //Initialisation de l'USART pc 
00097 
00098     /* Initializing Motor Control Expansion Board. */
00099     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00100     motors = x_nucleo_ihm02a1->get_components();
00101     
00102     ENCAL.mode(PullUp); //Initialisation des codeuses
00103     ENCAJ.mode(PullUp);
00104     ENCBL.mode(PullUp);
00105     ENCBJ.mode(PullUp);
00106 
00107     ENCAL.rise(&updateEncoderA);
00108     ENCAL.fall(&updateEncoderA);
00109     ENCAJ.rise(&updateEncoderA);
00110     ENCAJ.fall(&updateEncoderA);
00111 
00112     ENCBL.rise(&updateEncoderB);
00113     ENCBL.fall(&updateEncoderB);
00114     ENCBJ.rise(&updateEncoderB);
00115     ENCBJ.fall(&updateEncoderB);
00116 
00117 }
00118 
00119 void set_PWM_moteur_D(int PWM)
00120 {
00121     if (!moteurs_arret) {
00122         if (PWM > PWM_MAX) {
00123             motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
00124         } else if (PWM <-PWM_MAX) {
00125             motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
00126         } else if (PWM > 0) {
00127             motors[0]->prepare_run(StepperMotor::BWD, PWM);
00128         } else if (PWM < 0) {
00129             motors[0]->prepare_run(StepperMotor::FWD, -PWM);
00130         } else if (PWM == 0) {
00131             motors[0]->prepare_run(StepperMotor::BWD, 0);
00132         }
00133     } else {
00134         motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00135     }
00136     x_nucleo_ihm02a1->perform_prepared_actions();
00137 }
00138 
00139 void set_PWM_moteur_G(int PWM)
00140 {
00141 
00142     if (!moteurs_arret) {
00143         if (PWM > PWM_MAX) {
00144             motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
00145         } else if (PWM <-PWM_MAX) {
00146             motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
00147         } else if (PWM > 0) {
00148             motors[1]->prepare_run(StepperMotor::FWD, PWM);
00149         } else if (PWM < 0) {
00150             motors[1]->prepare_run(StepperMotor::BWD, -PWM);
00151         } else if (PWM == 0) {
00152             motors[1]->prepare_run(StepperMotor::BWD, 0);
00153         }
00154     } else {
00155         motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00156     }
00157     x_nucleo_ihm02a1->perform_prepared_actions();
00158 }
00159 
00160 void set_step_moteur_D(int steps)
00161 {
00162     if (!moteurs_arret) {
00163         if (1) {
00164             motors[0]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive
00165         /*} else if (PWM <-PWM_MAX) {
00166             motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
00167         } else if (PWM > 0) {
00168             motors[0]->prepare_run(StepperMotor::BWD, PWM);
00169         } else if (PWM < 0) {
00170             motors[0]->prepare_run(StepperMotor::FWD, -PWM);
00171         } else if (PWM == 0) {
00172             motors[0]->prepare_run(StepperMotor::BWD, 0);
00173         */}
00174     /*} else {
00175         motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/
00176     }
00177     x_nucleo_ihm02a1->perform_prepared_actions();
00178 }
00179 /*
00180 void set_step_moteur_G(int PWM)
00181 {
00182 
00183     if (!moteurs_arret) {
00184         if (PWM > PWM_MAX) {
00185             motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
00186         } else if (PWM <-PWM_MAX) {
00187             motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
00188         } else if (PWM > 0) {
00189             motors[1]->prepare_run(StepperMotor::FWD, PWM);
00190         } else if (PWM < 0) {
00191             motors[1]->prepare_run(StepperMotor::BWD, -PWM);
00192         } else if (PWM == 0) {
00193             motors[1]->prepare_run(StepperMotor::BWD, 0);
00194         }
00195     } else {
00196         motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00197     }
00198     x_nucleo_ihm02a1->perform_prepared_actions();
00199 }*/
00200 
00201 long int get_nbr_tick_D()
00202 {
00203     return encoderValueA;
00204 }
00205 
00206 long int get_nbr_tick_G()
00207 {
00208     return encoderValueB;
00209 }
00210 
00211 void attente_synchro()
00212 {
00213     //structute du temps d'attente de l'asservissement 10ms
00214     wait(0.010);
00215 }
00216 
00217 void motors_stop()
00218 {
00219     moteurs_arret=1;
00220     motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00221     motors[1]->prepare_hard_hiz(); 
00222     x_nucleo_ihm02a1->perform_prepared_actions();
00223 }
00224 
00225 void motors_on()
00226 {
00227     moteurs_arret=0;
00228 }
00229 
00230 
00231 void allumer_del()
00232 {
00233     led = 1;
00234 }
00235 
00236 void eteindre_del()
00237 {
00238     led = 0;
00239 }
00240 
00241 void delay_ms()
00242 {
00243 }
00244 
00245 void allumer_autres_del()
00246 {
00247 }
00248 
00249 void eteindre_autres_del()
00250 {
00251 }
00252 void toggle_autres_del() {}
00253 
00254 void set_all_led()
00255 {
00256 
00257 }
00258 
00259 
00260 volatile int lastEncodedA = 0;
00261 long lastencoderValueA = 0;
00262 int lastMSBA = 0;
00263 int lastLSBA = 0;
00264 
00265 void updateEncoderA()
00266 {
00267     int MSBA = ENCAL.read(); //MSB = most significant bit
00268     int LSBA = ENCAJ.read(); //LSB = least significant bit
00269 
00270     int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
00271     int sumA  = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
00272 
00273     if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
00274     if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
00275 
00276     lastEncodedA = encodedA; //store this value for next time
00277 }
00278 
00279 
00280 volatile int lastEncodedB = 0;
00281 long lastencoderValueB = 0;
00282 int lastMSBB = 0;
00283 int lastLSBB = 0;
00284 
00285 void updateEncoderB()
00286 {
00287     int MSBB = ENCBL.read(); //MSB = most significant bit
00288     int LSBB = ENCBJ.read(); //LSB = least significant bit
00289 
00290     int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
00291     int sumB  = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
00292 
00293     if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
00294     if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
00295 
00296     lastEncodedB = encodedB; //store this value for next time
00297 }
00298 
00299 
00300 
00301 
00302 /*void asservissement(){
00303     long int tick_D = get_nbr_tick_D();
00304     long int tick_G = get_nbr_tick_G();
00305     
00306     long int tick_D_passe = tick_D-tick_prec_D;
00307     long int tick_G_passe = tick_G-tick_prec_G;
00308     
00309     tick_prec_D=tick_D;
00310     tick_prec_G=tick_G;
00311     
00312     float vitesse_codeuse_D = tick_D_passe;
00313     float vitesse_codeuse_G = tick_G_passe;
00314     
00315     float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
00316     float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
00317     
00318     somme_erreur_D += erreur_D;
00319     somme_erreur_G += erreur_G;
00320     
00321     float delta_erreur_D = erreur_D-erreur_precedente_D;
00322     float delta_erreur_G = erreur_G-erreur_precedente_G;
00323     
00324     erreur_precedente_G = erreur_G;
00325     erreur_precedente_D = erreur_D;
00326     
00327     float cmd_D = Kp*erreur_D+Ki*somme_erreur_D + Kd*delta_erreur_D;
00328     float cmd_G = Kp*erreur_G+Ki*somme_erreur_G + Kd*delta_erreur_G;
00329     
00330     if (cmd_G <0){
00331         cmd_G = 0;
00332     }
00333     if (cmd_G > 500){
00334         cmd_G = 500;
00335     }
00336     if (cmd_D <0){
00337         cmd_D = 0;
00338     }
00339     if (cmd_D > 500){
00340         cmd_D = 500;
00341     }
00342     commande_vitesse(cmd_G,cmd_D);
00343     //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
00344     //printf("%f\n",vitesse_codeuse_G);
00345     //printf("oui");
00346 }*/
00347 
00348 void debugEncoder()
00349 {
00350     printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G());
00351 }
00352 long int get_position_G(){
00353     /* Getting the current position. */
00354     long int position = motors[1]->get_position();
00355     return position;
00356     /* Printing to the console. */
00357     //printf("--> Getting the current position: %d\r\n", position);
00358     
00359 }
00360 long int get_position_D(){
00361     /* Getting the current position. */
00362     long int position = motors[0]->get_position();
00363     
00364     /* Printing to the console. */
00365     //printf("--> Getting the current position: %d\r\n", position);
00366     return position;
00367     
00368 }